// ================================================================= // === Intel Galileo Gen 1/2 Board == Self-Balancing Robot === // ================================================================= // This project requires: 1x Arduino Motor shield, 1x MPU-6050 Accelerometer & // Gyroscope breakout, 2x 12v D.C. Gearbox Motors, 1x Custom BalanceBot Frame // Note: For best results, the motors should be powered by an external power //  supply, instead of through the Galileo Development Board // ============================================ // Written by Simon Bluett (Version 1.0, 16/7/14) // http://wired.chillibasket.com /* // This Demo makes use of the I2Cdev and MPU6050 libraries, and is based on the // demonstration sketch written by (Jeff Rowberg ), modified // to work with the Intel Galileo Development Board: // ============================================ // I2Cdev device library code is placed under the MIT license // Copyright (c) 2012 Jeff Rowberg // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal // in the Software without restriction, including without limitation the rights // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell // copies of the Software, and to permit persons to whom the Software is // furnished to do so, subject to the following conditions: // // The above copyright notice and this permission notice shall be included in // all copies or substantial portions of the Software. // ============================================ */ // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files // for both classes must be in the include path of your project #include #include #include // Specific I2C addresses may be passed as a parameter here MPU6050 mpu; // Default: AD0 low = 0x68 // Uncomment if you want to use the Debugging Interface // #define Debug_Interface #define LED_PIN 13 // (Galileo Gen1/2 = 13) #define DIR_A 7 // Direction Pin, Motor A (Needs to be jump-wired) #define DIR_B 6 // Direction Pin, Motor B (Needs to be jump-wired) #define PWM_A 3 // PWM, Motor A (Left Motor) #define PWM_B 11 // PWM, Motor B (Right Motor) #define BRK_A 9 // Brake, Motor A #define BRK_B 8 // Brake, Motor B bool blinkState = false; // MPU Control/Status bool dmpReady = false; // set true if DMP init was successful uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 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 Quaternion q; // [w, x, y, z] Quaternion Container VectorFloat gravity; // [x, y, z] Gravity Vector int16_t gyro[3]; // [x, y, z] Gyro Vector float ypr[3]; // [yaw, pitch, roll] Yaw/Pitch/Roll container and gravity vector float averagepitch[50]; // Used for averaging pitch value // For PID Controller float Kp = 8; //(P)roportional Tuning Parameter float Ki = 2; //(I)ntegral Tuning Parameter float Kd = 5; //(D)erivative Tuning Parameter float lastpitch; // Keeps track of error over time float Ky = 2; //(Y)aw Turning Parameter float iTerm; // Used to accumalate error (intergral) float targetAngle = 0; // Can be adjusted according to centre of gravity float targetYaw = 0; float PIDGain = 0; // Used for soft start (prevent jerking at initiation) // Motor Control int left_PWM, right_PWM; // Motor Speed Variables int direction_A = 0; // 0 - Forwards, 1 - Backwards int direction_B = 0; int brake_A = 1; // 1 - On, 0 - Off int brake_B = 1; // Runtime variables int initialized = 0; // Is the balancing mode on? int calibrated = 0; // Has accel-gyro auto calibrated? (0 = No, 1 = Yes) int calibrateangle = 51; // Used for calibrating COG (0-50 = Gather new data, 51 = Turned Off) int status = 0; // Are there any errors? char inchar = 0; // Hold any incoming characters float angular_rate = 0; // Used to make sure rate is ~0 when balance mode is initiated // Variables used for timing control // Aim is 10ms per cycle (100Hz) #define STD_LOOP_TIME 9 #define STD_CALIBRATE_TIME 40500 // Time the PID waits until Accel-Gyro is calibrated int lastLoopTime = STD_LOOP_TIME; int lastLoopUsefulTime = STD_LOOP_TIME; unsigned long loopStartTime = 0; unsigned long thisTime; // Used to see if calibration time is complete unsigned long lastTime; // Time since PID was called last (should be ~10ms) int looptimeslow = 0; // Average earlier deviation loop time int looptimefast = 0; // Average daley deviation from loop time int looptimeaverage = 0; // Total average deviation (See 'timekeeper()' method) // ================================================================= // === INITIAL SETUP === // ================================================================= void setup() { Wire.begin(); // Initialize serial communication Serial.begin(115200); // Initialize device Serial.println("1) Initialising I2C devices..."); mpu.initialize(); // Verify connection Serial.println("2) Testing device connections..."); Serial.println(mpu.testConnection() ? "2a) MPU6050 connection successful" : "2b) MPU6050 connection failed"); // Load and configure the DMP Serial.println("3) Initialising DMP..."); devStatus = mpu.dmpInitialize(); // Supply your own gyro offsets here, scaled for min sensitivity mpu.setXGyroOffset(0); mpu.setYGyroOffset(-53); mpu.setZGyroOffset(0); mpu.setXAccelOffset(-191); mpu.setYAccelOffset(-3822); mpu.setZAccelOffset(550); // Make sure it worked (returns 0 if so) if (devStatus == 0) { // turn on the DMP, now that it's ready Serial.println("4) Enabling DMP..."); mpu.setDMPEnabled(true); mpuIntStatus = mpu.getIntStatus(); // Set our DMP Ready flag so the main loop() function knows it's okay to use it Serial.println("5) DMP Ready! Let's Proceed..."); dmpReady = true; // Get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); } else { // ERROR! // 1 = Initial memory load failed // 2 = DMP configuration updates failed if(devStatus == 1) Serial.println("6) Initial Memory Load Failed"); else if (devStatus == 2) Serial.println("7) DMP Configuration Updates Failed"); } // Configure LED for output pinMode(LED_PIN, OUTPUT); digitalWrite(LED_PIN, LOW); // Set as input, internal pullup for G button pinMode(4, INPUT_PULLUP); // Configure Motor I/O pinMode(DIR_A, OUTPUT); // Left Motor Direction pinMode(DIR_B, OUTPUT); // Right Motor Direction pinMode(BRK_A, OUTPUT); // Left Motor Brake pinMode(BRK_B, OUTPUT); // Right Motor Brake loop(); } // ================================================================ // === PID CONTROLLER === // ================================================================ void PID(float pitch, float yaw) { // Calculate time since last time PID was called (~10ms) thisTime = millis(); double timeChange = (double)(thisTime - lastTime); // Calculate Error float error = (targetAngle - pitch); // Calculate our PID terms // PID values are multiplied/divided by 10 in order to allow the // constants to be numbers between 0-10. float pTerm = Kp * error * 10; iTerm += Ki * error * timeChange / 10; if (Ki == 0) iTerm = 0; // Prevent integral from exceeding max PWM values else if(iTerm > 255) iTerm= 255; else if(iTerm < -255) iTerm= -255; float dTerm = Kd * (pitch - lastpitch) / timeChange * 100; lastpitch = pitch; lastTime = thisTime; // Set PWM Value float PIDValue = (pTerm + iTerm - dTerm) * PIDGain; // Set a minimum speed (motors will not move below this - this helps reduce latency) if(PIDValue > 0) PIDValue = PIDValue + 10; if(PIDValue < 0) PIDValue = PIDValue - 10; // Set both motors to this speed left_PWM = right_PWM = PIDValue; // Now add yaw turn controller // First of all, see if turning left or right is faster int leftturn, rightturn; if((yaw > 0) && (targetYaw < 0)){ rightturn = yaw + abs(targetYaw); leftturn = (180 - yaw) + (180 - abs(targetYaw)); } else if ((yaw < 0) && (targetYaw > 0)){ rightturn = (180 - abs(yaw)) + (180 - targetYaw); leftturn = abs(yaw) + targetYaw; } else if (((yaw > 0) && (targetYaw > 0)) || ((yaw < 0) && (targetYaw < 0))){ rightturn = targetYaw - yaw; if (rightturn > 0){ leftturn = rightturn; rightturn = 360 - leftturn; } else if (rightturn < 0){ rightturn = abs(rightturn); leftturn = 360 - abs(rightturn); } else if (rightturn == 0){ rightturn = leftturn = 0; } } // Now add/subtract this from normal output if ((leftturn == 0) && (rightturn == 0)){ // Do nothing } else if (leftturn < rightturn){ leftturn = Ky * leftturn; if (leftturn > 50) leftturn = 50; left_PWM = left_PWM - leftturn; right_PWM = right_PWM + leftturn; } else if (rightturn < leftturn){ rightturn = Ky * rightturn; if (rightturn > 50) rightturn = 50; left_PWM = left_PWM + Ky * (rightturn); right_PWM = right_PWM - Ky * (rightturn); } else if (rightturn == leftturn){ leftturn = Ky * leftturn; if (leftturn > 50) leftturn = 50; left_PWM = left_PWM - leftturn; right_PWM = right_PWM + leftturn; } // Limits PID to max motor speed if (left_PWM > 255) left_PWM = 255; else if (left_PWM < -255) left_PWM = -255; if (right_PWM > 255) right_PWM = 255; else if (right_PWM < -255) right_PWM = -255; // If motors are to move forwards: if (left_PWM >= 0) Move(0, 0, int(left_PWM)); // '0' = Left-motor, '1' = Right-motor else Move(0, 1, (int(left_PWM) * -1)); if (right_PWM >= 0) Move(1, 0, int(right_PWM)); // '0' = Forward, '1' = Backward else Move(1, 1, (int(right_PWM) * -1)); } // ================================================================ // === MOTOR CONTROLLER === // ================================================================ void Move(int motor, int direction, int speed) { // Left Motor if (motor == 0) { // Move motor forwards if (direction == 0){ if (direction_A == 1) digitalWrite(DIR_A, HIGH); // Establishes forward direction of Channel A if (brake_A == 1) digitalWrite(BRK_A, LOW); // Disengage the Brake for Channel A analogWrite(PWM_A, speed); // Spins the motor on Channel A at full speed direction_A = 0; brake_A = 0; // Move motor backwards } else if (direction == 1){ if (direction_A == 0) digitalWrite(DIR_A, LOW); // Establishes backwards direction of Channel A if (brake_A == 1) digitalWrite(BRK_A, LOW); // Disengage the Brake for Channel A analogWrite(PWM_A, speed); // Spins the motor on Channel A at full speed direction_A = 1; brake_A = 0; } // Right Motor } else if (motor == 1){ // Move motor forwards if (direction == 0){ if (direction_B == 1) digitalWrite(DIR_B, HIGH); // Establishes forward direction of Channel B if (brake_B == 1) digitalWrite(BRK_B, LOW); // Disengage the Brake for Channel B analogWrite(PWM_B, speed); // Spins the motor on Channel B at full speed direction_B = 0; brake_B = 0; // Move motor backwards } else if (direction == 1){ if (direction_B == 0) digitalWrite(DIR_B, LOW); // Establishes backwards direction of Channel B if (brake_B == 1) digitalWrite(BRK_B, LOW); // Disengage the Brake for Channel B analogWrite(PWM_B, speed); // Spins the motor on Channel B at full speed direction_B = 1; brake_B = 0; } // Stop both motors } else if (motor = 3){ analogWrite(PWM_A, 0); analogWrite(PWM_B, 0); digitalWrite(BRK_A, HIGH); digitalWrite(BRK_B, HIGH); brake_A = 1; brake_B = 1; } } // ================================================================ // === READ SERIAL FROM GUI === // ================================================================ void readSerial() { // Initiate all of the variables int changestate = 0; // Which action needs to be taken? (See below) int no_before = 0; // Numbers before decimal point int no_after = 0; // Numbers after decimal point int minus = 0; // See if number is negative inchar = Serial.read(); // Read incoming data if (inchar == 'P') changestate = 1; else if (inchar == 'I') changestate = 2; else if (inchar == 'D') changestate = 3; // Tell robot to calibrate the Centre of Gravity else if (inchar == 'G') calibrateangle = 0; // Get loop-time status report (to make sure refresh rate ~100Hz) else if (inchar == 'S'){ Serial.print("Loop-time Faster: "); Serial.print(looptimefast); Serial.print(", Loop-time Slower: "); Serial.print(looptimeslow); Serial.print(", Loop-time Average: "); Serial.println(looptimeaverage); } // Records all of the incoming data (format: 00.000) // And converts the chars into a float number if (changestate > 0){ if (Serial.available() > 0){ // Is the number negative? inchar = Serial.read(); if(inchar == '-'){ minus = 1; inchar = Serial.read(); } no_before = inchar - '0'; if (Serial.available() > 0){ inchar = Serial.read(); if (inchar != '.'){ no_before = (no_before * 10) + (inchar - '0'); if (Serial.available() > 0){ inchar = Serial.read(); } } if (inchar == '.'){ inchar = Serial.read(); if (inchar != '0'){ no_after = (inchar - '0') * 100; } if (Serial.available() > 0){ inchar = Serial.read(); if (inchar != '0'){ no_after = no_after + ((inchar - '0') * 10); } if (Serial.available() > 0){ inchar = Serial.read(); if (inchar != '0'){ no_after = no_after + (inchar - '0'); } } } } } // Combine the chars into a single float float answer = float(no_after) / 1000; answer = answer + no_before; if (minus == 1) answer = answer * -1; // Update the PID constants if (changestate == 1) Kp = answer; else if (changestate == 2) Ki = answer; else if (changestate == 3) Kd = answer; Serial.print("Constant Set: "); Serial.println(answer, 3); } else { changestate = 0; } } } // ================================================================ // === MAIN PROGRAM LOOP === // ================================================================ void loop() { // if programming failed, don't try to do anything if (!dmpReady) return; if (digitalRead(4) == LOW){ calibrateangle = 0; lastpitch = 0; iTerm = 0; } // 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("Warning - FIFO Overflowing!"); // otherwise, check for DMP data ready interrupt (this should happen exactly once per loop: 100Hz) } else if (mpuIntStatus & 0x02) { // Wait for correct available data length, should be less than 1-2ms, if any! 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 witshout waiting for an interrupt) fifoCount -= packetSize; // Get sensor data mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGyro(gyro, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); mpu.resetFIFO(); ypr[1] = ypr[1] + (ypr[1] * 0.0220); // Correct Value for Pitch <<<<<<<<<<<< Check if this is really required angular_rate = -((float)gyro[1]/131.0); //Serial.println(ypr[0] * 180/M_PI); // If sytem has not yet finished calibrating: if (calibrated == 0) { thisTime = millis(); // If sensor has finished auto-calibration: (Pre-defined time; see #define) if (thisTime > STD_CALIBRATE_TIME){ Serial.println("Ready to Balance!"); digitalWrite(LED_PIN, HIGH); calibrated = 1; } } else { // If new centre of gravity is to be calibrated: if (calibrateangle == 0){ targetAngle = (ypr[1] * 180/M_PI); calibrateangle++; } else if (calibrateangle < 50){ targetAngle = targetAngle + (ypr[1] * 180/M_PI); calibrateangle++; // All our measurements are completed: } else if (calibrateangle == 50){ targetAngle = targetAngle / 50; calibrateangle++; Serial.print("Target Angle Set: "); Serial.println(targetAngle, 3); targetYaw = ypr[0] * 180/M_PI; // Otherwise, run the control system: } else { // If the Balance System is turned on: if (initialized == 1){ // Stop the system if it has fallen over: if (((ypr[1] * 180/M_PI) < -55) || ((ypr[1] * 180/M_PI) > 55)){ Move(3, 0, 0); // Stop the motors lastpitch = 0; iTerm = 0; initialized = 0; PIDGain = 0; Serial.println(">>>> Stopped <<<<"); // Otherwise, run the PID controller } else { if (PIDGain >= 1){ PIDGain = 1; PID(ypr[1] * 180/M_PI, ypr[0] * 180/M_PI); } else { // Soft entrance - prevents jerking as PID is initiated PIDGain += 0.02; //Serial.println(PIDGain); PID(ypr[1] * 180/M_PI, ypr[0] * 180/M_PI); } } // If the Balance System is turned off: } else { // Wait until robot is vertical and angular rate is almost zero: if (((ypr[1] * 180/M_PI) < targetAngle+0.1) && ((ypr[1] * 180/M_PI) > targetAngle-0.1) && (abs(angular_rate) < 0.3)){ Serial.println(">>>> Initializing <<<<"); initialized = 1; lastpitch = ypr[1] * 180/M_PI; targetYaw = ypr[0] * 180/M_PI; iTerm = 0; } } } } } if (Serial.available() > 0){ // If new PID values are being sent by the interface readSerial(); // Run the read serial method } // Call the timing function // Very important to keep the response consistent! timekeeper(); } // ================================================================ // === TIME KEEPER ~100Hz === // ================================================================ void timekeeper() { // Calculate time since loop began lastLoopUsefulTime = millis() - loopStartTime; // If the required loop time has not been reached, please wait! if (lastLoopUsefulTime < STD_LOOP_TIME) { delay(STD_LOOP_TIME - lastLoopUsefulTime); // Record deviation from target in milliseconds (for debugging) looptimefast += STD_LOOP_TIME - lastLoopUsefulTime; looptimefast = looptimefast / 2; // If required loop time has been exceeded: } else { // Record deviation from target in milliseconds (for debugging) looptimeslow += lastLoopUsefulTime - STD_LOOP_TIME; looptimeslow = looptimeslow / 2; } // Get average deviation from 100Hz (Neg. = Delayed, Pos. = Early) looptimeaverage = looptimefast - looptimeslow; // Update loop timer variables lastLoopTime = millis() - loopStartTime; loopStartTime = millis(); }