Components and supplies
L293d motor driver shield
Universal Wheel
MPU6050
Battery 7.4V lithium
Arduino Uno Rev3
Geared DC Motor
Tools and machines
Soldering kit
Apps and platforms
Arduino IDE
Project description
Code
Code
cpp
..
1#include "I2Cdev.h" 2#include <PID_v1.h> //From https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h 3#include "MPU6050_6Axis_MotionApps20.h" //https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050 4MPU6050 mpu; 5// MPU control/status vars 6bool dmpReady = false; // set true if DMP init was successful 7uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 8uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 9uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 10uint16_t fifoCount; // count of all bytes currently in FIFO 11uint8_t fifoBuffer[64]; // FIFO storage buffer 12// orientation/motion vars 13Quaternion q; // [w, x, y, z] quaternion container 14VectorFloat gravity; // [x, y, z] gravity vector 15float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 16/*********Tune these 4 values for your BOT*********/ 17double setpoint= 182; //set the value when the bot is perpendicular to ground using serial monitor. 18//Read the project documentation on circuitdigest.com to learn how to set these values 19double Kp = 15; //21 Set this first 20double Kd = 0.9; //0.8 Set this secound 21double Ki = 140; //140 Finally set this 22/******End of values setting*********/ 23double input, output; 24PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT); 25volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high 26void dmpDataReady() 27{ 28 mpuInterrupt = true; 29} 30void setup() { 31 Serial.begin(115200); 32 // initialize device 33 Serial.println(F("Initializing I2C devices...")); 34 mpu.initialize(); 35 // verify connection 36 Serial.println(F("Testing device connections...")); 37 Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); 38 // load and configure the DMP 39 devStatus = mpu.dmpInitialize(); 40 // supply your own gyro offsets here, scaled for min sensitivity 41 mpu.setXGyroOffset(-479); 42 mpu.setYGyroOffset(84); 43 mpu.setZGyroOffset(15); 44 mpu.setZAccelOffset(1638); 45 // make sure it worked (returns 0 if so) 46 if (devStatus == 0) 47 { 48 // turn on the DMP, now that it's ready 49 Serial.println(F("Enabling DMP...")); 50 mpu.setDMPEnabled(true); 51 // enable Arduino interrupt detection 52 Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); 53 attachInterrupt(0, dmpDataReady, RISING); 54 mpuIntStatus = mpu.getIntStatus(); 55 // set our DMP Ready flag so the main loop() function knows it's okay to use it 56 Serial.println(F("DMP ready! Waiting for first interrupt...")); 57 dmpReady = true; 58 // get expected DMP packet size for later comparison 59 packetSize = mpu.dmpGetFIFOPacketSize(); 60 //setup PID 61 pid.SetMode(AUTOMATIC); 62 pid.SetSampleTime(10); 63 pid.SetOutputLimits(-255, 255); 64 } 65 else 66 { 67 // ERROR! 68 // 1 = initial memory load failed 69 // 2 = DMP configuration updates failed 70 // (if it's going to break, usually the code will be 1) 71 Serial.print(F("DMP Initialization failed (code ")); 72 Serial.print(devStatus); 73 Serial.println(F(")")); 74 } 75//Initialise the Motor outpu pins 76 pinMode (6, OUTPUT); 77 pinMode (9, OUTPUT); 78 pinMode (10, OUTPUT); 79 pinMode (11, OUTPUT); 80//By default turn off both the motors 81 analogWrite(6,LOW); 82 analogWrite(9,LOW); 83 analogWrite(10,LOW); 84 analogWrite(11,LOW); 85} 86void loop() { 87 // if programming failed, don't try to do anything 88 if (!dmpReady) return; 89 // wait for MPU interrupt or extra packet(s) available 90 while (!mpuInterrupt && fifoCount < packetSize) 91 { 92 //no mpu data - performing PID calculations and output to motors 93 pid.Compute(); 94 //Print the value of Input and Output on serial monitor to check how it is working. 95 Serial.print(input); Serial.print(" =>"); Serial.println(output); 96 if (input>150 && input<200){//If the Bot is falling 97 if (output>0) //Falling towards front 98 Forward(); //Rotate the wheels forward 99 else if (output<0) //Falling towards back 100 Reverse(); //Rotate the wheels backward 101 } 102 else //If Bot not falling 103 Stop(); //Hold the wheels still 104 } 105 // reset interrupt flag and get INT_STATUS byte 106 mpuInterrupt = false; 107 mpuIntStatus = mpu.getIntStatus(); 108 // get current FIFO count 109 fifoCount = mpu.getFIFOCount(); 110 // check for overflow (this should never happen unless our code is too inefficient) 111 if ((mpuIntStatus & 0x10) || fifoCount == 1024) 112 { 113 // reset so we can continue cleanly 114 mpu.resetFIFO(); 115 Serial.println(F("FIFO overflow!")); 116 // otherwise, check for DMP data ready interrupt (this should happen frequently) 117 } 118 else if (mpuIntStatus & 0x02) 119 { 120 // wait for correct available data length, should be a VERY short wait 121 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 122 // read a packet from FIFO 123 mpu.getFIFOBytes(fifoBuffer, packetSize); 124 // track FIFO count here in case there is > 1 packet available 125 // (this lets us immediately read more without waiting for an interrupt) 126 fifoCount -= packetSize; 127 mpu.dmpGetQuaternion(&q, fifoBuffer); //get value for q 128 mpu.dmpGetGravity(&gravity, &q); //get value for gravity 129 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //get value for ypr 130 input = ypr[1] * 180/M_PI + 180; 131 } 132} 133void Forward() //Code to rotate the wheel forward 134{ 135 analogWrite(6,output); 136 analogWrite(9,0); 137 analogWrite(10,output); 138 analogWrite(11,0); 139 Serial.print("F"); //Debugging information 140} 141void Reverse() //Code to rotate the wheel Backward 142{ 143 analogWrite(6,0); 144 analogWrite(9,output*-1); 145 analogWrite(10,0); 146 analogWrite(11,output*-1); 147 Serial.print("R"); 148} 149void Stop() //Code to stop both the wheels 150{ 151 analogWrite(6,0); 152 analogWrite(9,0); 153 analogWrite(10,0); 154 analogWrite(11,0); 155 Serial.print("S"); 156}
Downloadable files
Libs
...
Libraries.zip
Documentation
Schematic
...
SchematicJPG.jpg
Comments
Only logged in users can leave comments