Arduino Self Balancing Roboter.

Transkript

1 Arduino Self Balancing Roboter

2 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth Modul HC-06 für Arduino

3 MPU-6050 Batteriehalter 4xAA Mignon Batteriehalterung 9V 2x Servomotoren SM-S4303R 360 Degree Continuous Rotation

4 2x 67mm Rad für GM9 9V Batterieclip auf 2,1mm x 5,5mm Stecker 4x Jumper Wire male-male USB Kabel für Arduino UNO R3 Board

5 Doppelseitiges Klebeband Gewindestange M6 DIN975 1m 8x V2A Unterlegscheiben DIN ,4 (6 mm Schrauben) 8x V2A Sechskantmuttern (selbstsichernd) DIN 985 M6 Sekundenkleber

6 Mini Breadbord - Polycarbonat Platte 2. Werkzeug - Lötkolben - Lötzinn - Schraubenzieher

7 3. Zusammenbau 3.1 Gestell zwei gleich grosse rechteckige Platten (20cm x 5cm) aus den Polycarbonatplatten ausschneiden. Aus der Gewindestange zwei 25cm lange Stücke mit der Eisensäge absägen. In jede Polycarbonatplatte zwei Löcher mittig mit 6mm bohren mit einem Abtsand von den Seiten von 3,5cm.

8 Beide Polycarbonatplatten mit den Gweindenstangen in einem Abstand von 10cm verbinden. Unten bleibt ein Rest von 2,5cm der Gewindestange übrig. Dabei soll jede Platte an der Gewindestange mit Beilgescheiben oben und unten mit den Muttern befestigt werden und richtig festgezogen werden, so dass ein verdrehen oder Wackeln der platten vermieden wird. Der Abstand der platte sollte auch gleich sein und besonders die oberste Platte Horizontal. Mit einer Wasserwaage könnte man dies ermitteln. Die Motoren sind parallel mit Sekundenkleber befestigt, wobei die Achse der Motoren auch die Mitte der Platte bilden

9 Unten ist die Batteriehalterung mit Sekundenkleber befestigt. Die Lage des Batteriefaches sind vollbestückt und so ausgerichtet, dass der Schwerpunkt bei aufrechten Stand des Roboters mittig ist und keine Schräglage verursacht. Oben ist die Elektronik mit doppelseitigem Klebeband befestigt. Auch diese ist auf Beibehaltung des Schwerpunktes bei aufrechtem Stand zu achten. Die Verteilung der 6V Stromzufuhr für die Servomotoren ist über ein Minibreadbord gesteckt.

10 Der Sensor befindest sich in der Mittellinie zwischen den Stangen. Die Stromversorgung des Arduinos erfolgt über die 9V Batterie

11 Die 9V Batterie ist unter dem 6V Batteriefach mit doppelseitigem Klebeband befestigt.

12 3.2 ELektronik Wichtig ist bei der Schaltung, dass die Stromversorgung von Arduino und Motoren getrennt ist aber trotzdem mit GND verbunden sind.sollte man keine weitere Debugausgaben während des Laufens benötigen, kann auch das Bluetooth Modul HC-06 entfernt werden. Bevor die Steuerung zum Laufen gebracht werden kann, müssen die Offset Werte der MPU angepasst werden, die für jedes Modul unterschiedlich sind. Am einfachsten ist es, wenn man das MPU Modul auf einer geraden Ebene hinlegt und sich die Werte ausgeben lässt von myawpitchroll[0], myawpitchroll[1] und myawpitchroll[2] und auch die Gravity Werte. Dann an den aktuell eingestellten Offset Werten so lange herumdrehen, bis myawpitchroll auf 0 sind.

13 4. Software 4.1 benötigte Arduino Bibliotheken: Arduino Code: ##include <SoftwareSerial.h> #include <Servo.h> #include <PID_v1.h> #include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" #include "Wire.h" #define #define #define #define #define LOG_INPUT PIN_SERVO_MOTOR_LINKS PIN_SERVO_MOTOR_RECHTS PIN_BLUETOOTH_TX PIN_BLUETOOTH_RX // Bluetooth // HC-06 Arduino // VCC +5V/+3,3V // GND GND // TXD pinrx (10) // RXD pintx (11) // Servo // Servo Arduino // schwarz GND // rot 6V Batterie // weiss D5/D6 // MPU-6050 // MPU Arduino // VCC +5V // GND GND // SCL A5 // SDA A4 // INT D2 class MyServo public: MyServo(int pin) : mpin(pin), mmaxspeed(90) void init() servomotor.attach(mpin); void stop() servomotor.write(90);

14 void move(int speed,int minabsspeed) if ( speed < 0 ) speed = min(speed, -1*minAbsSpeed); speed = max(speed, -mmaxspeed); speed = 90 + speed; else speed = max(speed, minabsspeed); speed = min(speed, mmaxspeed); speed += 90; servomotor.write(speed); private: Servo servomotor; int mmaxspeed; int mpin; ; boolean mpuinterrupt = false; void dmpdataready() mpuinterrupt = true; class SelfBalancingRobot public: SelfBalancingRobot( int pin_motor_links, int pin_motor_rechts, int pin_bt_tx, int pin_bt_rx) : myserial(pin_bt_tx,pin_bt_rx), mservolinks(pin_motor_links), mservorechts(pin_motor_rechts), mpid(&minput, &moutput, &msetpoint, 70, 240, 1.9, DIRECT), moriginalsetpoint(1), mmovingangleoffset(0.3), maccoffx(-5180), maccoffy(-1336), maccoffz(1310), mgyroffx(55), mgyroffy(-24), mgyroffz(27) void init() myserial.begin(57600); delay(500); mmovestate = 0; msetpoint = moriginalsetpoint; mpid.setmode(automatic); mpid.setsampletime(10); mpid.setoutputlimits(-90, 90);

15 Wire.begin(); TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) mmpu.initialize(); Serial.println(mMpu.testConnection()? "MPU6050 connection successful" : "MPU6050 connection failed"); uint8_t devstatus = mmpu.dmpinitialize(); mmpu.setxgyrooffset(mgyroffx); mmpu.setygyrooffset(mgyroffy); mmpu.setzgyrooffset(mgyroffz); mmpu.setxacceloffset(maccoffx); mmpu.setyacceloffset(maccoffy); mmpu.setzacceloffset(maccoffz); if (devstatus == 0) mmpu.setdmpenabled(true); attachinterrupt(0, dmpdataready, RISING); mmpuintstatus = mmpu.getintstatus(); mdmpready = true; mpacketsize = mmpu.dmpgetfifopacketsize(); else Serial.print("mpu failed"); Serial.println(devStatus); mservolinks.init(); mservorechts.init(); void run() if (!mdmpready) return; while (!mpuinterrupt && mfifocount < mpacketsize) mpid.compute(); move(moutput); // myserial.print(minput); // myserial.print(" "); // myserial.print(msetpoint); // myserial.print(" "); // myserial.println(moutput); mpuinterrupt = false; uint8_t mpuintstatus = mmpu.getintstatus(); mfifocount = mmpu.getfifocount(); if ( (mpuintstatus & 0x10) mfifocount == 1024 ) mmpu.resetfifo(); myserial.println(f("fifo overflow!")); else if (mpuintstatus & 0x02) while (mfifocount < mpacketsize) mfifocount = mmpu.getfifocount();

16 mmpu.getfifobytes(mfifobuffer, mpacketsize); mfifocount -= mpacketsize; mmpu.dmpgetquaternion(&mquat, mfifobuffer); mmpu.dmpgetgravity(&mgravity, &mquat); mmpu.dmpgetyawpitchroll(myawpitchroll, &mquat, &mgravity); minput = myawpitchroll[1] * 180 / M_PI; private: void move(int speed) mservolinks.move(-speed,0); mservorechts.move(speed,0); SoftwareSerial myserial; MyServo mservolinks; MyServo mservorechts; PID mpid; MPU6050 mmpu; double minput; double moutput; double msetpoint; bool mdmpready; uint16_t mpacketsize; uint8_t mmpuintstatus; uint16_t mfifocount; Quaternion mquat; VectorFloat mgravity; float myawpitchroll[3]; uint8_t mfifobuffer[64]; int mmovestate; double mmovingangleoffset; double moriginalsetpoint; int maccoffx; int maccoffy; int maccoffz; int mgyroffx; int mgyroffy; int mgyroffz; ; SelfBalancingRobot robot( PIN_SERVO_MOTOR_LINKS, PIN_SERVO_MOTOR_RECHTS, PIN_BLUETOOTH_TX, PIN_BLUETOOTH_RX); void setup() Serial.begin(115200); robot.init(); void loop() robot.run();