Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*Arduino Self Balancing Robot
- * Code by: B.Aswinth Raj
- * Build on top of Lib: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
- * Website: circuitdigest.com
- Code source credits to this man above !!!
- Here is the exact page: https://circuitdigest.com/microcontroller-projects/arduino-based-self-balancing-robot
- Sincerely Peter Lunk
- Video of my version running this code: https://www.youtube.com/watch?v=4iKDf0UMjdc <-----------------------------
- */
- #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 = true; // 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= 179; //set the value when the bot is perpendicular to ground using serial monitor. was 176
- //Read the project documentation on circuitdigest.com to learn how to set these values
- double Kp = 21; //Set this first was 21
- double Kd = 0.8; //Set this secound was 0.8
- double Ki = 140; //Finally set this was 140
- /******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
- void dmpDataReady()
- {
- mpuInterrupt = true;
- }
- void setup() {
- Serial.begin(115200);
- // 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();
- // supply your own gyro offsets here, scaled for min sensitivity
- mpu.setXGyroOffset(130);
- mpu.setYGyroOffset(8);
- mpu.setZGyroOffset(22);
- mpu.setZAccelOffset(930);
- // 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); // was 10 LUNK adjust testing
- pid.SetOutputLimits(-210, 210); // was -255, 255 LUNK testing
- }
- 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 outpu pins
- pinMode (6, OUTPUT);
- pinMode (9, OUTPUT);
- pinMode (10, OUTPUT);
- pinMode (11, OUTPUT);
- //By default turn off both the motors
- analogWrite(6,LOW);
- analogWrite(9,LOW);
- analogWrite(10,LOW);
- analogWrite(11,LOW);
- }
- void loop() {
- // 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>150 && input<200){//If the Bot is falling
- if (output>0) //Falling towards front
- Forward(); //Rotate the wheels forward
- else if (output<0) //Falling towards back
- Reverse(); //Rotate the wheels backward
- }
- else //If Bot not falling
- Stop(); //Hold the wheels still
- }
- // 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(6,output);
- analogWrite(9,0);
- analogWrite(10,output);
- analogWrite(11,0);
- Serial.print("F"); //Debugging information
- }
- void Reverse() //Code to rotate the wheel Backward
- {
- analogWrite(6,0);
- analogWrite(9,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");
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement