Follower

Die Funktion von Follower soll ganz einfach sein – Gegenständen folgen!

Nachdem ich mit einem Dagu Compund Eye etwas rumgespielt habe, musste ich damit natürlich auch einen Roboter ausstatten. Ich  konnte leider kein Pan-Tilt-Modul finden, das meinen Vorstellungen entspricht. Um meine Vorräte aus der Bastlerkiste etwas aufzubrauchen, habe ich mir ein Pololu Sumo Chassis geschnappt und ein Pan-Titl-Modul mit SketchUp entworfen. Die blauen Teile für das Pan-Tilt-Modul habe ich mit meinem 3D-Drucker gedruckt (PLA).

Die entsprechenden STL-Files sind auf www.youmagine.com zu finden

Die Teile sind für die HS-311 Servos von HiTec ausgelegt. Ich mag diese Servos, weil sie von ziemlich guter Qualität sind und nicht so viel Strom verbrauchen.

Da in den Pololu Chassis nur 4 Batterien reinpassen und ich meine Roboter nach Möglichkeit mit Akkus versorge, habe ich als Spannungsregler, einen Step-Up/Step-Down Schaltregler S7V7F5 von Pololu genommen.

Als Controller kommt ein Klon von Arduino Pro Mini (Version 16MHz / 5V)

Der Motortreiber ist ein DRV8835 von Pololu.

Die Elektronik habe ich auf einer Lochrasterplatine miteinander “verheiratet”.

Hier ist noch ein Blockdiagramm, wie ich das miteinander verbunden habe.

Um mir den Aufbau etwas zu erleichtern und anderen eine kleine Vorlage zum Nachbauen zur Verfügung zu stellen, habe ich mir mal auf die Schnelle ein Schaltplan gezeichnet.

Die Versorgungsspannung führt zuerst auf die Platine des Motortreibers. Warum? Auf dem Motortreiber ist eine Schaltung mit Verpolungsschutz, und man kann es für die gesamte Schaltung verwenden. Nach dem Motortreiber wird die Spannung auf 5V geregelt.

Die Vcc Pins der Servos habe ich auch auf Vin gelegt, da sie bis zu 6V aushalten. Pro Servo wird ein Digital-Pin verwendet. Für den Motortreiber werden 4 digitale Pins benötigt. Zwei Pins um die beiden Kanäle ein- und auszuschalten und zwei für die Drehrichtung der Motoren. Um diesen Modus zu aktivieren, wird der Pin “MODE” auf HIGH (5V) gezogen.

Für den Compund Eye werden 4 analoge und ein digitaler Pin benötigt. Die analogen (Eingänge) um die Signale einzulesen und digital (Ausgang) um den Sensor ein-/auszuschalten.

Um die ganzen kleinen Platinchen miteinander zu verbinden, habe ich eine kleine Lochrasterplatine und Silberdraht verwendet.

Programmierung.

Die Profi-Programmierer, die heute einen schlechten Tag hatten, sollten hier aufhören zu lesen  🙂

Wie ich den Sensor ansteuere und die Werte aufbereite, habe ich in dem oben erwähnten Artikel schon gezeigt.

Das Gesamte Programm ist eine “State Machine”, d.h. die Befehle werden der Reihe nach ausgeführt, ohne dass irgendwelche Interrupts dazwischen funken.

Es werden zuerst die Werte der 4 Fototransistoren eingelesen und je nachdem auf welcher Seite die maximale Einstrahlung gemessen wird, wird auch der Kopf mit Hilfe von zwei Servos ausgerichtet. Obwohl der  Pan-Servo seine Anschläge bei 0° und 180° hat, habe ich diese im Code etwas angepasst. Das Chassis beginnt bei 115° und 75° zu rotieren. Es sieht einfach besser aus, als wenn man den Servo bis 180° oder 0° drehen lassen würde.

Beim Hoch-Runter-Servo (Tilt) musste ich die Anschläge in der Software einschränken, da für die Bewegung nach unten, der Pan-Servo im Weg steht.

Wenn die Hand bzw. ein Objekt zu nah an dem Sensor ist, fährt der Roboter zurück. Wenn das zu weit entfernt ist, fährt er vorwärts. Wenn das Objekt komplett fehlt, bleibt der Roboter stehen und “zickt etwas rum” – der Kopf wird ganz nach Links-Oben gedreht  😉 Dieses zickige Verhalten habe ich ihm nicht beigebracht. Es liegt einfach an Signal-Toleranzen. Man könnnte das zwar im Code mit ein paar Tricks beseitigen, ich lasse ihn aber seinen Charakter haben  😀

/*
- Identifiziere dich, Programm!
- Programm fuer Roboter "Follower" mit Dagu Compound Eye!
;)


*/

#include <Servo.h>

Servo pan_servo;    //Servo Kopfbewegung links/rechts
Servo tilt_servo;   //Servo für Kopfbewegung hoch/runter



//Pins fuer Motortreiber
int benable = 5;    //Kanal b ein-/ausschalten
int bphase = 6;     //Kanal b Drehrichtung
int aenable = 7;    //Kanal a ein-/ausschalten
int aphase = 8;     //Kanal a Drehrichtung

//Pins fuer Compund Eye
int enable_eye = 13;    //Pin mit Ein-/Ausschalter fuer IR-LEDs
int left = A0;          //IR-Sensor links
int top = A1;           //IR-Sensor oben
int right = A2;         //IR-Sensor rechts
int bottom = A3;        //IR-Sensor unten


//Variablen Umgebungslicht + Infrarot
int absolute_top = 0;
int absolute_left = 0;
int absolute_bottom = 0;
int absolute_right = 0;
 
//Variablen nur Umgebungslicht
int offset_top = 0;
int offset_left = 0;
int offset_bottom = 0;
int offset_right = 0;
 
//Variablen Differenz
int relative_top = 0;
int relative_left = 0;
int relative_bottom = 0;
int relative_right = 0;



//Variablen fuer Posistionen der Servos
int pos_pan = 0;
int pos_tilt = 0;
int servo_step = 1;  //Wie schnell der Servo bewegt werden soll (Grad/Schritt)



void setup()
{
  //Pins fuer Motortreiber als Output
  pinMode(benable , OUTPUT);
  pinMode(bphase , OUTPUT);
  pinMode(aenable , OUTPUT);
  pinMode(aphase , OUTPUT); 
 
  //Die beiden Kanaele des Motortreibers erstmal deaktivieren 
  digitalWrite(benable , LOW);
  digitalWrite(aenable , LOW);
  
  //Enable Pin des Compound Eye's als Ausgang
  pinMode(enable_eye , OUTPUT);
  
  pan_servo.attach(11);  //Pan-Servo an Digital-Pin 11
  tilt_servo.attach(10); //Tilt-Servo an Digital-Pin 10
  
  pan_servo.write(90);    //weniger nach unten, mehr nach oben
  tilt_servo.write(90);   //weniger nach links, mehr nach rechts
  
  
  
  Serial.begin(9600);
  
  
}

//Sub-Routine fuer Vorwaerts
void fwd()
{
  digitalWrite(aenable , HIGH);
  digitalWrite(benable , HIGH);
  digitalWrite(aphase , HIGH);
  digitalWrite(bphase , HIGH);
  
}

//Sub-Routine fuer Bremse
void brake ()
{
  digitalWrite(aenable , LOW);
  digitalWrite(benable , LOW);
}


//Sub-Routine fuer Rueckwaerts
void bwd()
{
  digitalWrite(aenable , HIGH);
  digitalWrite(benable , HIGH);
  digitalWrite(aphase , LOW);
  digitalWrite(bphase , LOW); 
  
}

//Sub-Routine Drehen nach links
void rotate_left()
{
  digitalWrite(aenable , HIGH);
  digitalWrite(benable , HIGH);
  digitalWrite(aphase , LOW);
  digitalWrite(bphase , HIGH); 
}

//Sub-Routine Drehen nach Rechts
void rotate_right()
{
  digitalWrite(aenable , HIGH);
  digitalWrite(benable , HIGH);
  digitalWrite(aphase , HIGH);
  digitalWrite(bphase , LOW);  
}


//=====================================================================
//Endlose Hauptschleife
void loop()
{
  
 //Werte bei IR=AN einlesen
  digitalWrite(enable_eye , HIGH);    //IR-LEDs einschalten
  delay(1);                           //ganz kurze Pause
  
  //Lichteinstrahlung einlesen (Umgebungslicht und IR-Licht von LEDs
  absolute_top = analogRead(top);    
  absolute_left = analogRead(left);
  absolute_bottom = analogRead(bottom);
  absolute_right = analogRead(right);
 
  //Werte bei IR=AUS einlesen
  digitalWrite(enable_eye , LOW);
  delay(1);
 
 //Lichteinstrahlung einlesen (nur Umgebungslicht)
  offset_top = analogRead(top);
  offset_left = analogRead(left);
  offset_bottom = analogRead(bottom);
  offset_right = analogRead(right);
 
 
  //Differenz berechnen (Gesamteinstrahlung - Umgebungslicht)
  relative_top = absolute_top-offset_top;
  relative_left = absolute_left-offset_left;
  relative_bottom = absolute_bottom-offset_bottom;
  relative_right = absolute_right-offset_right;
 
//============================================================


//Ist die Einstrahlung links staerker als rechts?
if(relative_left > relative_right)
{
 pos_pan = pos_pan-servo_step;    //1° von Servoposition abziehen
}
//Ist die Einstrahlung rechts staerker als links?
 else if(relative_left < relative_right )
 {
   pos_pan = pos_pan+servo_step;  //Servoposition um 1° erhoehen
 }
 
 //Sicherstellen, dass der Winkel nur max. 180° betragen kann
 if(pos_pan > 180)
 {
 pos_pan = 180;
 }
 
 //Sicherstellen, dass die Winkel nicht ins negative Bereich rutscht
 if(pos_pan < 0)
 {
  pos_pan = 0; 
 }

//Servo in die entsprechende Richtung drehen
 pan_servo.write(pos_pan);
 
 
 
 //Ist die Einstrahlung unten staerker als oben?
 if(relative_bottom > relative_top)
{
  pos_tilt = pos_tilt-servo_step;    //1° von Servoposition abziehen
}
//Ist die Einstrahlung oben staerker als unten?
 else if(relative_bottom < relative_top)
 {
   pos_tilt = pos_tilt+servo_step;    //Servoposition um 1° erhoehen
 }
  //Sicherstellen, dass der Winkel nur max. 180° betragen kann
 if(pos_tilt > 180)
 {
 pos_tilt = 180;
 }
  //Sicherstellen, dass die Winkel nicht ins negative Bereich rutscht
 if(pos_tilt < 80)
 {
  pos_tilt = 80; 
 }
//Servo in die entsprechende Richtung heben/absenken
 tilt_servo.write(pos_tilt);
 


//=======================================================
//Hier kommt der Kettenantrieb ins Spiel
    
//Wenn der Servo den Winkel 115° erreicht hat - recht drehen    
 if(pos_pan > 115)
 {
  rotate_right(); 
 }
//Wenn der Servo den Winkel 115° erreicht hat - links drehen    
 else if(pos_pan < 75)
 {
 rotate_left(); 
 }
 
 //Jetzt noch etwas mit Logik spielen :)
 //Ist der Winkel kleiner als 115° UND die Einstrahlung (oben UND unten UND rechts UND links) kleiner als 300 ist
 else if(pos_tilt < 115 && relative_top < 300 && relative_bottom < 300 && relative_left < 300 && relative_right < 300)
 {
   //fahre Vorwaerts
   fwd();
 }
 
 //Ist der Winkel kleiner als 115° UND die Einstrahlung (oben UND unten UND rechts UND links) groesser als 600 ist
 else if(pos_tilt < 115 && relative_top > 600 && relative_bottom > 600 && relative_left > 600 && relative_right > 600) 
 {
   //fahre Rueckwaerts
  bwd(); 
 }
 //in allen anderen Faellen, stehen bleiben
 else
 {
   brake();
 }
  
  //kurze Pause, damit der Bot sich insgesamt etwas sanfter bewegt. Ja, ja - delay ist boese ]=)
  delay(10);
  
}

//Ende des Programms

Hier noch ein kleines Video, vom Follower im Einsatz 🙂