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 😀
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 |
/* - 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 🙂
Hallo, ich finde deine Seite sehr gut struckturiert und die Projekte hervorragend erklärt. 🙂 und werde es versuchen , diesen Roboter nach zu bauen.
Danke.
Danke 🙂
Viel Spaß beim Nachbauen! Hoffe dass die Informationen für ein Nachbau ausreichend sind 🙂