Lichtverfolgung

Diese Seite zeigt ein Programm, damit ein Fahrzeug automatisch eine Lichtquelle ansteuert.

Verbesserungen sind willkommen

Andreas Hecker

 

/* Motosteuerung eines Roboters */

int geschwindigkeit = 150;        /* Standardgewindigkeit für die Motoren  */
float faktor = 0.95;             /* Ausgleichsfaktor für die Motoren.  Muss experimentell ermittelt werden.*/
/* !!!!!!!!  Achtung   Achtung  Bei Linkskurven wird der Kurvenfaktor 2x eingesetzt
Wahrscheinlich reagieren die Motoren bzw. das Fahrgestell beim Vorwärtsgang anders als bei Rückwärtsgang :-((*/
float kurvenfaktor = 1.2;      /* Faktor für den Kurvenradius  */
const int El = 6;          /*   PWM - Ausgang zur Steuerung des Motors 1 = links  */
const int Ml = 7;          /*  Laufrichtung von Motor 1 = links  einstellen HIGH = vorwärts, LOW = rückwärts  */
const int Er = 5;          /*  Analog für Motor 2  = rechts  */
const int Mr= 4;          /*  Analog für Motor 2  = rechts  */

const int drehzeit = 5000;
      long zeit = 0;
const int lichtsensorL = A0;
const int lichtsensorR = A1;
      float lichtwertFaktor = 0.92;
      float lichtwertL;
      float lichtwertLSumme;
      float lichtwertR;
      float lichtwertRSumme;
      float lichtwert;
      float lichtwertMAX;
const int lichtwertWENIGER = 5;
 

void halt()
{
    int l = 0;
    int r = 0;
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  halt");
    digitalWrite(Ml,HIGH);
    digitalWrite(Mr, HIGH);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control  
}
 
void geradeausvorwaerts(int g)
{
    int l = floor(g*faktor);
    int r = g;
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  geradeausvorwaerts");
    digitalWrite(Ml,HIGH);
    digitalWrite(Mr, HIGH);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}
 
void geradeausrueckwaerts(int g)
{
    int l = floor(g*faktor);
    int r = g;
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  geradeausrueckwaerts");
    digitalWrite(Ml,LOW);
    digitalWrite(Mr, LOW);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}
 
void rechtsdrehen(int g)
{
    int l = floor(g*faktor);
    int r = g;
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  rechtsdrehen");
    digitalWrite(Ml,HIGH);
    digitalWrite(Mr, LOW);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}
 
void linksdrehen(int g)
{
    int l = floor(g*faktor);
    int r = g;
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  linksdrehen");
    digitalWrite(Ml, LOW);
    digitalWrite(Mr, HIGH);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}
 
void rechtskurvevorwaerts(int g)
{
    int l = floor(g*faktor*kurvenfaktor);
    int r = floor(g/kurvenfaktor);
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  rechtskurvevorwaerts");
    digitalWrite(Ml,HIGH);
    digitalWrite(Mr, HIGH);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}
 
void linkskurvevorwaerts(int g)
{
    int l = floor(g*faktor/kurvenfaktor/kurvenfaktor);
    int r = floor(g*kurvenfaktor*kurvenfaktor);
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  linkskurvevorwaerts");
    digitalWrite(Ml,HIGH);
    digitalWrite(Mr, HIGH);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}
 
void rechtskurverueckwaerts(int g)
{
    int l = floor(g*faktor*kurvenfaktor);
    int r = floor(g/kurvenfaktor);
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  rechtskurverueckwaerts");
    digitalWrite(Ml,LOW);
    digitalWrite(Mr, LOW);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}
 
void linkskurverueckwaerts(int g)
{
    int l = floor(g*faktor/kurvenfaktor/kurvenfaktor);
    int r = floor(g*kurvenfaktor*kurvenfaktor);
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  linkskurverueckwaerts");
    digitalWrite(Ml,LOW);
    digitalWrite(Mr, LOW);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}



void setup() {
  Serial.begin(9600);
  pinMode(El, OUTPUT);
  pinMode(Ml, OUTPUT);
  pinMode(Er, OUTPUT);
  pinMode(Mr, OUTPUT);
 
  rechtsdrehen(geschwindigkeit);
// Maximale Helligkeit bei 2 Drehungen ermitteln
  while (millis() < drehzeit*2) {
    lichtwert = (analogRead(lichtsensorL) + analogRead(lichtsensorR)) / 2;
    if (lichtwert > lichtwertMAX) {
      lichtwertMAX = lichtwert;
    }
  }
// Roboter auf maximale Helligkeit ausrichten
  zeit = millis();
  while ((analogRead(lichtsensorL) + analogRead(lichtsensorR)) / 2 < lichtwertMAX) {
    if (millis()-zeit > drehzeit) {
      lichtwertMAX = lichtwertMAX - lichtwertWENIGER;
      zeit = millis();
    }
  }
  halt();
  delay(1000);
/* Sensortest
   delay(5000);
   lichtwertLSumme = 0;
   lichtwertRSumme = 0;
   int i;
   for (i=1;i<20;i++) {
     delay(1000);
     lichtwertL = analogRead(lichtsensorL);
     lichtwertLSumme = lichtwertLSumme + lichtwertL;
     lichtwertR = analogRead(lichtsensorR);
     lichtwertRSumme = lichtwertRSumme + lichtwertR;
     Serial.print("Links: ");  Serial.print(lichtwertL);  Serial.print("   Rechts: ");  Serial.println(lichtwertR);
   }
   lichtwertFaktor = lichtwertLSumme / lichtwertRSumme;
   Serial.print("lichtwertFaktor: ");  Serial.println(lichtwertFaktor);
*/
}


void loop()
{
 
   if (analogRead(lichtsensorL) > analogRead(lichtsensorR)*lichtwertFaktor) {
     linkskurvevorwaerts(geschwindigkeit);
   }
   else {
     rechtskurvevorwaerts(geschwindigkeit);
   }  

/* Fahrtest
   geradeausvorwaerts(geschwindigkeit);
   delay(5000);
   halt();
   delay(1000);
   geradeausrueckwaerts(geschwindigkeit);
   delay(3000);
   halt();
   delay(1000);
   rechtsdrehen(geschwindigkeit);
   delay(3000);
   halt();
   delay(1000);
   linksdrehen(geschwindigkeit);
   delay(3000);
   halt();
   delay(1000);
   rechtskurvevorwaerts(geschwindigkeit);
   delay(3000);
   halt();
   delay(1000);
   linkskurvevorwaerts(geschwindigkeit);
   delay(3000);
   halt();
   delay(1000);
   rechtskurverueckwaerts(geschwindigkeit);
   delay(3000);
   halt();
   delay(1000);
   linkskurverueckwaerts(geschwindigkeit);
   delay(3000);
   halt();
   delay(1000);
*/
}