Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter [email protected]. 1....

16
Arduino Self Balancing Roboter [email protected]

Transcript of Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter [email protected]. 1....

Page 2: Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter mail@AndreBetz.de. 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth

1. Bauteile

Arduino UNO R3

Prototyping Shield Mini Breadboard Für Arduino UNO R3

Bluetooth Modul HC-06 für Arduino

Page 3: Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter mail@AndreBetz.de. 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth

MPU-6050

Batteriehalter 4xAA Mignon

Batteriehalterung 9V

2x Servomotoren SM-S4303R 360 Degree Continuous Rotation

Page 4: Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter mail@AndreBetz.de. 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth

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

Page 5: Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter mail@AndreBetz.de. 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth

Doppelseitiges Klebeband

Gewindestange M6 DIN975 1m

8x V2A Unterlegscheiben DIN 9021 6,4 (6 mm Schrauben)

8x V2A Sechskantmuttern (selbstsichernd) DIN 985 M6

Sekundenkleber

Page 6: Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter mail@AndreBetz.de. 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth

Mini Breadbord

- Polycarbonat Platte

2. Werkzeug

- Lötkolben- Lötzinn- Schraubenzieher

Page 7: Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter mail@AndreBetz.de. 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth

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.

Page 8: Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter mail@AndreBetz.de. 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth

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

Page 9: Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter mail@AndreBetz.de. 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth

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.

Page 10: Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter mail@AndreBetz.de. 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth

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

Page 11: Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter mail@AndreBetz.de. 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth

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

Page 12: Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter mail@AndreBetz.de. 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth

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-06entfernt werden.

Bevor die Steuerung zum Laufen gebracht werden kann, müssen die Offset Werte derMPU 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.

Page 13: Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter mail@AndreBetz.de. 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth

4. Software

4.1 benötigte Arduino Bibliotheken:

https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/I2Cdevhttps://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

4.2 Arduino Code:

##include <SoftwareSerial.h>#include <Servo.h>#include <PID_v1.h>#include "I2Cdev.h"#include "MPU6050_6Axis_MotionApps20.h"#include "Wire.h"

#define LOG_INPUT#define PIN_SERVO_MOTOR_LINKS 6#define PIN_SERVO_MOTOR_RECHTS 5#define PIN_BLUETOOTH_TX 10#define PIN_BLUETOOTH_RX 11

//////////////////////////////////////////////////////////////// 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);

Page 14: Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter mail@AndreBetz.de. 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth

} 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);

Page 15: Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter mail@AndreBetz.de. 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth

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();

Page 16: Arduino Self Balancing Roboter · 2016. 1. 9. · Self Balancing Roboter mail@AndreBetz.de. 1. Bauteile Arduino UNO R3 Prototyping Shield Mini Breadboard Für Arduino UNO R3 Bluetooth

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();}//////////////////////////////////////////////////////////////