Balancerende Robots

Maart 2020:
Dit was een uitdagend project dat lang op zich heeft laten wachten. De eerste versie van deze robot maakte ik twee jaar geleden en nu pas maak ik het project eindelijk af.

Onderdelen

Er zijn niet veel onderdelen nodig.
1) De arduino Nano
Deze is bekend (toch?)
Klik op het plaatje voor een link naar een betrouwbare winkel

2) De mpu6050 (GY-521)
Dit is een versnellingsmeter (3 assen, x,y en z en gyroscoop)
Deze module kan ons vertellen of de robot in evenwicht is of dat hij aan het kantelen is


3) Motoren
Dit zijn de motors die ik gebruikte.. ik denk echter dat ze nog iets te langzaam draaien.. Ik zou dus aanraden om op zoek te gaan naar snellere motoren of grotere wielen
Deze zijn te vinden op Ebay, ze bestaan in verschillende snelheden. Deze werden verkocht als 200rpm

4) Motorcontroller
Dit is de bekende L298N motor driver


5) Batterijen en houder
Ik gebruik 18650 Cellen. Ik heb een houder die er twee in serie en dat geeft dus ongeveer 8 volt.
6) Wielen
Ik bestelde de wielen op ebay, hier zijn voorbeelden:
Alternatief voor de wielen:
Dit is misschien een betere optie voor de wielen, die had ik eerder moeten zien, deze zijn groter (dat is beter omdat mijn motortjes wat traag zijn) en de set is goedkoop!
Dit is een set van 2 wielen met adapters voor een as van 4mm, perfect!


7) Diversen
Verder heb je nog wat kleine onderdeeltjes nodig zoals:
- 2 weerstanden van 180 ohm (0,25 watt)
- minstens 2 (koop er 6) condensatoren van 100nF
- Een aan/uit schakelaar
- Optioneel een resetknop
- Dubbelzijdig plakband of hete lijm

Het Schema

Dit is het schema. In de video bespreek ik het.

Klik om te vergroten!

3d Prints

Hier is een links naar mijn tinkercad projecten (klik op de plaatjes).

Hier is een zip bestand met beide frames als stl bestanden.

Arduino Sketches

Calibratie

De eerste sketch is de calibratie sketch. Zie video!
Deze stap moet je niet overslaan!
// MPU-6050 Short Example Sketch // By Arduino User JohnChi // August 17, 2014 // Public Domain #include<Wire.h> const int MPU_addr=0x68; // I2C address of the MPU-6050 int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; int i =0; void setup(){ Wire.begin(); Wire.beginTransmission(MPU_addr); Wire.write(0x6B); // PWR_MGMT_1 register Wire.write(0); // set to zero (wakes up the MPU-6050) Wire.endTransmission(true); Serial.begin(115200); } void loop(){ Wire.beginTransmission(MPU_addr); Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) Wire.endTransmission(false); Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) // Serial.print("AcX = "); Serial.print(AcX); // Serial.print(" | AcY = "); Serial.print(AcY); // Serial.print(" | AcZ = "); Serial.print(AcZ); // Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); //equation for temperature in degrees C from datasheet // Serial.print(" | GyX = "); Serial.print(GyX); // Serial.print(" | GyY = "); Serial.print(GyY); // Serial.print(" | GyZ = "); Serial.println(GyZ); if (i++ > 10) { Serial.println("") ; Serial.print("mpu.setXGyroOffset(") ; Serial.print(GyX); Serial.println(");") ; Serial.print("mpu.setYGyroOffset("); Serial.print(GyY); Serial.println(");") ; Serial.print("mpu.setZGyroOffset(") ; Serial.print(GyZ); Serial.println(");") ; Serial.print("mpu.setZAccelOffset("); Serial.print(AcZ); Serial.println(");") ; Serial.println("") ; i=0; } else { Serial.print("."); } delay(333); }

Balance Sketch

Dit is de balance sketch
Je zult nog het een en ander moeten afstellen.. zie video
Er worden externe libraries gebruikt voor de PID controller en voor de MPU6050
#include "I2Cdev.h" #include <PID_v1.h> //From https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h #include "MPU6050_6Axis_MotionApps20.h" //https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050 MPU6050 mpu; // MPU control/status vars bool dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // FIFO storage buffer // orientation/motion vars Quaternion q; // [w, x, y, z] quaternion container VectorFloat gravity; // [x, y, z] gravity vector float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector /*********Tune these 4 values for your BOT*********/ double setpoint= 196.55; //set the value when the bot is perpendicular to ground using serial monitor. //Read the project documentation on circuitdigest.com to learn how to set these values double Kp = 90;//70; //Set this first double Kd = 1.2 ;//1.3 ; //Set this secound double Ki = 200;// 100; //Finally set this /******End of values setting*********/ double input, output; PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT); volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high int irsignal=0; bool noIR=true; void setup() { Serial.begin(115200); pinMode(4,INPUT); pinMode(A0,INPUT); pinMode(A1,INPUT); pinMode(A2,INPUT); pinMode(A3,INPUT); // initialize device Serial.println(F("Initializing I2C devices...")); mpu.initialize(); // verify connection Serial.println(F("Testing device connections...")); Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); // load and configure the DMP devStatus = mpu.dmpInitialize(); // Voeg hieronder de resultaten in van de Calibration sketch mpu.setXGyroOffset(19); mpu.setYGyroOffset(60); mpu.setZGyroOffset(-13); mpu.setZAccelOffset(32767); // make sure it worked (returns 0 if so) if (devStatus == 0) { // turn on the DMP, now that it's ready Serial.println(F("Enabling DMP...")); mpu.setDMPEnabled(true); // enable Arduino interrupt detection Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); attachInterrupt(0, dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus(); // set our DMP Ready flag so the main loop() function knows it's okay to use it Serial.println(F("DMP ready! Waiting for first interrupt...")); dmpReady = true; // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); //setup PID pid.SetMode(AUTOMATIC); pid.SetSampleTime(10); pid.SetOutputLimits(-255, 255); } else { // ERROR! // 1 = initial memory load failed // 2 = DMP configuration updates failed // (if it's going to break, usually the code will be 1) Serial.print(F("DMP Initialization failed (code ")); Serial.print(devStatus); Serial.println(F(")")); } //Initialise the Motor output pins pinMode (9, OUTPUT); pinMode (6, OUTPUT); pinMode (10, OUTPUT); pinMode (11, OUTPUT); //By default turn off both the motors analogWrite(9,LOW); analogWrite(6,LOW); analogWrite(10,LOW); analogWrite(11,LOW); } void loop() { if ( digitalRead(4) and noIR ) { getsignal(); noIR=false; Serial.println(irsignal); } if (!digitalRead(4) and !noIR) noIR=true; // if programming failed, don't try to do anything if (!dmpReady) return; // wait for MPU interrupt or extra packet(s) available while (!mpuInterrupt && fifoCount < packetSize) { //no mpu data - performing PID calculations and output to motors pid.Compute(); //Print the value of Input and Output on serial monitor to check how it is working. // Serial.print(input); Serial.print(" =>"); Serial.println(output); if (input>(setpoint - 20) && input< (setpoint + 20)){ if (output>0) Forward(); else if (output<0) Reverse(); } else Stop(); // stop the motors if the robot falls over too far } // reset interrupt flag and get INT_STATUS byte mpuInterrupt = false; mpuIntStatus = mpu.getIntStatus(); // get current FIFO count fifoCount = mpu.getFIFOCount(); // check for overflow (this should never happen unless our code is too inefficient) if ((mpuIntStatus & 0x10) || fifoCount == 1024) { // reset so we can continue cleanly mpu.resetFIFO(); Serial.println(F("FIFO overflow!")); // otherwise, check for DMP data ready interrupt (this should happen frequently) } else if (mpuIntStatus & 0x02) { // wait for correct available data length, should be a VERY short wait while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // read a packet from FIFO mpu.getFIFOBytes(fifoBuffer, packetSize); // track FIFO count here in case there is > 1 packet available // (this lets us immediately read more without waiting for an interrupt) fifoCount -= packetSize; mpu.dmpGetQuaternion(&q, fifoBuffer); //get value for q mpu.dmpGetGravity(&gravity, &q); //get value for gravity mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //get value for ypr input = ypr[1] * 180/M_PI + 180; } } void Forward() //Code to rotate the wheel forward { analogWrite(9,output); analogWrite(6,0); analogWrite(10,output); analogWrite(11,0); // Serial.print("F"); //Debugging information } void Reverse() //Code to rotate the wheel Backward { analogWrite(9,0); analogWrite(6,output*-1); analogWrite(10,0); analogWrite(11,output*-1); // Serial.print("R"); } void Stop() //Code to stop both the wheels { analogWrite(6,0); analogWrite(9,0); analogWrite(10,0); analogWrite(11,0); // Serial.print("S"); } void dmpDataReady() { mpuInterrupt = true; } void getsignal(){ irsignal=0; bitWrite(irsignal,3,digitalRead(A0)); bitWrite(irsignal,2,digitalRead(A1)); bitWrite(irsignal,1,digitalRead(A2)); bitWrite(irsignal,0,digitalRead(A3)); }

De Video

Kijk, like en subscribe!

---