Hallo, ich benutze den folgenden Code, der bei mir auch so gut funktioniert, er verfolgt mit einer Ir Positionierungskamera an einem Servoarm. Ir Lampen nur ist er jetzt wenn er kein Signal hat, also von 165 ° auf 35 ° gedreht und wieder zurück und wenn er die Lampe wieder finden kann, verfolgt sie und sucht noch mehr. Weiß jemand, wie das gehen könnte?
Vielen Dank im Voraus!
Ich freue mich über Antworten
Hier mein Code:
#include <Servo.h>
#include <Wire.h>
int servo2 = 0;
Servo panServo;
Servo tiltServo;
Servo lenkServo;
int servo = 0;
int pos = 0;
bool IRdetected = false;
byte result; // data from WiiCamera
#define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis() - _lasttime) > (t);_lasttime += (t))
const int EN1 = 9; //pwm, enable motor 1 = pin 1 of L293D
const int direction1 = 2; //direcion motor 1 = pin 2 of L293D
const int EN2 = 10; //pwm, enable motor 2 = pin 9 of L293D
const int direction2 = 3; //direction motor 2 = pin 15 of L293D
const int direction3 = 4; //direction motor 2 = pin 15 of L293D
const int direction4 = 5; //direction motor 2 = pin 15 of L293D
int speedLeft = 205;
int speedRight = 255;
int hi = 0;
int IRsensorAddress = 0xB0;
//int IRsensorAddress = 0x58;
int slaveAddress;
int ledPin = 13;
boolean ledState = false;
byte data_buf[16];
int i;
int Ix[4];
int Iy[4];
int s;
int size1 = 0;
void Write_2bytes(byte d1, byte d2)
{
Wire.beginTransmission(slaveAddress);
Wire.write(d1); Wire.write(d2);
Wire.endTransmission();
}
void setup() {
// put your setup code here, to run once:
slaveAddress = IRsensorAddress >> 1; // This results in 0x21 as the address to pass to TWI
Wire.begin();
// IR sensor initialize
Write_2bytes(0x30, 0x01); delay(10);
Write_2bytes(0x30, 0x08); delay(10);
Write_2bytes(0x06, 0x90); delay(10);
Write_2bytes(0x08, 0xC0); delay(10);
Write_2bytes(0x1A, 0x40); delay(10);
Write_2bytes(0x33, 0x33); delay(10);
delay(100);
Serial.begin(9600);
pinMode(EN1, OUTPUT);
pinMode(direction1, OUTPUT);
pinMode(EN2, OUTPUT);
pinMode(direction2, OUTPUT);
pinMode(direction3, OUTPUT);
pinMode(direction4, OUTPUT);
analogWrite(EN1, 0);
analogWrite(EN2, 0);
pinMode(13, OUTPUT);
delay(50);
panServo.attach(7);
tiltServo.attach(6);
lenkServo.attach(9);
delay(500);
}
void loop() {
// put your main code here, to run repeatedly:
//IR sensor read
Wire.beginTransmission(slaveAddress);
Wire.write(0x36);
Wire.endTransmission();
Wire.requestFrom(slaveAddress, 16); // Request the 2 byte heading (MSB comes first)
for (i = 0; i < 16; i++) {
data_buf[i] = 0;
}
i = 0;
while (Wire.available() && i < 16) {
data_buf[i] = Wire.read();
i++;
}
if( Serial.print("y-Achse:")){
}
}
Ix[0] = data_buf[1];
Iy[0] = data_buf[2];
s = data_buf[3];
Ix[0] += (s & 0x30) << 4;
Iy[0] += (s & 0xC0) << 2;
size1 = (s & 0x0F);
for (i = 0; i < 4; i++)
{
if (int(Iy[i]) > 0 && int(Iy[i]) != 1023) {
Serial.print("y-Achse:");
Serial.println( int(Iy[i]) );
}
if ( int(Ix[i]) !=0 && int(Ix[i]) != 1023) { // wenn signal...
Serial.print("x-Achse:");
Serial.println( int(Ix[i]) );
if ( int(Ix[i]) < 270 && int(Ix[i]) != 0) {
servo = (panServo.read() - 5);
panServo.write(servo);
Serial.print("rechts");
delay(50);
}
if ( int(Ix[i]) > 400 && int(Ix[i]) != 1023) {
servo = (panServo.read() + 5);
panServo.write(servo);
Serial.print("links");
delay(50);
}
IRdetected = true;
}
}
}
void forward()
{
digitalWrite(direction1, HIGH);
digitalWrite(direction2, LOW);
digitalWrite(direction3, HIGH);
digitalWrite(direction4, LOW);
analogWrite(EN1, speedLeft);
analogWrite(EN2, speedRight);
}
void stop()
{
digitalWrite(direction1, LOW);
digitalWrite(direction2, LOW);
analogWrite(EN1, 0);
analogWrite(EN2, 0);
}
void backward()
{
digitalWrite(direction2, HIGH);
digitalWrite(direction1, LOW);
digitalWrite(direction4, HIGH);
digitalWrite(direction3, LOW);
}