Lakshmanan Gopalakrishnan / Mbed 2 deprecated Balancing_Robot_2

First trial

Dependencies:   MPU6050 Motor ledControl2 mbed

Fork of BalancingRobotPS3 by Kristian Lauszus

BalancingRobot.h@0:f5c02b620688, 2012-02-14 (annotated)

Committer:
Lauszus
Date:
Tue Feb 14 10:48:01 2012 +0000
Revision:
0:f5c02b620688
Child:
1:01295228342f
First release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Lauszus 0:f5c02b620688 1 #ifndef _balancingrobot_h_
Lauszus 0:f5c02b620688 2 #define _balancingrobot_h_
Lauszus 0:f5c02b620688 3
Lauszus 0:f5c02b620688 4 #include "mbed.h"
Lauszus 0:f5c02b620688 5
Lauszus 0:f5c02b620688 6 #define RAD_TO_DEG 57.295779513082320876798154814105 // 180/pi
Lauszus 0:f5c02b620688 7
Lauszus 0:f5c02b620688 8 DigitalOut onboardLED1(LED1);
Lauszus 0:f5c02b620688 9 DigitalOut onboardLED2(LED2);
Lauszus 0:f5c02b620688 10 DigitalOut onboardLED3(LED3);
Lauszus 0:f5c02b620688 11 DigitalOut onboardLED4(LED4);
Lauszus 0:f5c02b620688 12
Lauszus 0:f5c02b620688 13 /* Left motor */
Lauszus 0:f5c02b620688 14 DigitalOut leftA(p21);
Lauszus 0:f5c02b620688 15 DigitalOut leftB(p22);
Lauszus 0:f5c02b620688 16 PwmOut leftPWM(p23);
Lauszus 0:f5c02b620688 17 //AnalogIn leftCurrentSense(p15); // Not used
Lauszus 0:f5c02b620688 18
Lauszus 0:f5c02b620688 19 /* Right motor */
Lauszus 0:f5c02b620688 20 DigitalOut rightA(p24);
Lauszus 0:f5c02b620688 21 DigitalOut rightB(p25);
Lauszus 0:f5c02b620688 22 PwmOut rightPWM(p26);
Lauszus 0:f5c02b620688 23 //AnalogIn rightCurrentSense(p16); // Not used
Lauszus 0:f5c02b620688 24
Lauszus 0:f5c02b620688 25 /* IMU */
Lauszus 0:f5c02b620688 26 AnalogIn gyroY(p17);
Lauszus 0:f5c02b620688 27 AnalogIn accX(p18);
Lauszus 0:f5c02b620688 28 AnalogIn accY(p19);
Lauszus 0:f5c02b620688 29 AnalogIn accZ(p20);
Lauszus 0:f5c02b620688 30
Lauszus 0:f5c02b620688 31 // Zero voltage values for the sensors - [0] = gyroY, [1] = accX, [2] = accY, [3] = accZ
Lauszus 0:f5c02b620688 32 double zeroValues[4];
Lauszus 0:f5c02b620688 33
Lauszus 0:f5c02b620688 34 /* Kalman filter variables and constants */
Lauszus 0:f5c02b620688 35 const float Q_angle = 0.001; // Process noise covariance for the accelerometer - Sw
Lauszus 0:f5c02b620688 36 const float Q_gyro = 0.003; // Process noise covariance for the gyro - Sw
Lauszus 0:f5c02b620688 37 const float R_angle = 0.03; // Measurent noise covariance - Sv
Lauszus 0:f5c02b620688 38
Lauszus 0:f5c02b620688 39 double angle = 180; // It starts at 180 degrees
Lauszus 0:f5c02b620688 40 double bias = 0;
Lauszus 0:f5c02b620688 41 double P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
Lauszus 0:f5c02b620688 42 double dt, y, S;
Lauszus 0:f5c02b620688 43 double K_0, K_1;
Lauszus 0:f5c02b620688 44
Lauszus 0:f5c02b620688 45 // Results
Lauszus 0:f5c02b620688 46 double accYangle;
Lauszus 0:f5c02b620688 47 double gyroYrate;
Lauszus 0:f5c02b620688 48 double pitch;
Lauszus 0:f5c02b620688 49
Lauszus 0:f5c02b620688 50 /* PID values */
Lauszus 0:f5c02b620688 51 // Motors
Lauszus 0:f5c02b620688 52 double Kp = 11;
Lauszus 0:f5c02b620688 53 double Ki = 2;
Lauszus 0:f5c02b620688 54 double Kd = 12;
Lauszus 0:f5c02b620688 55 double targetAngle = 90;
Lauszus 0:f5c02b620688 56
Lauszus 0:f5c02b620688 57 double lastError;
Lauszus 0:f5c02b620688 58 double iTerm;
Lauszus 0:f5c02b620688 59
Lauszus 0:f5c02b620688 60 /* Used for timing */
Lauszus 0:f5c02b620688 61 long timer;
Lauszus 0:f5c02b620688 62
Lauszus 0:f5c02b620688 63 const long STD_LOOP_TIME = 10*1000; // Fixed time loop of 10 milliseconds
Lauszus 0:f5c02b620688 64 long lastLoopTime = STD_LOOP_TIME;
Lauszus 0:f5c02b620688 65 long lastLoopUsefulTime = STD_LOOP_TIME;
Lauszus 0:f5c02b620688 66 long loopStartTime;
Lauszus 0:f5c02b620688 67
Lauszus 0:f5c02b620688 68 enum Motor {
Lauszus 0:f5c02b620688 69 left,
Lauszus 0:f5c02b620688 70 right,
Lauszus 0:f5c02b620688 71 both,
Lauszus 0:f5c02b620688 72 };
Lauszus 0:f5c02b620688 73 enum Direction {
Lauszus 0:f5c02b620688 74 forward,
Lauszus 0:f5c02b620688 75 backward,
Lauszus 0:f5c02b620688 76 };
Lauszus 0:f5c02b620688 77
Lauszus 0:f5c02b620688 78 bool steerForward;
Lauszus 0:f5c02b620688 79 bool steerBackward;
Lauszus 0:f5c02b620688 80 bool steerLeft;
Lauszus 0:f5c02b620688 81 bool steerRotateLeft;
Lauszus 0:f5c02b620688 82 bool steerRight;
Lauszus 0:f5c02b620688 83 bool steerRotateRight;
Lauszus 0:f5c02b620688 84
Lauszus 0:f5c02b620688 85 void calibrateSensors();
Lauszus 0:f5c02b620688 86 void PID(double restAngle);
Lauszus 0:f5c02b620688 87 double kalman(double newAngle, double newRate, double looptime);
Lauszus 0:f5c02b620688 88 double getGyroYrate();
Lauszus 0:f5c02b620688 89 double getAccY();
Lauszus 0:f5c02b620688 90 void move(Motor motor, Direction direction, float speed);
Lauszus 0:f5c02b620688 91 void stop(Motor motor);
Lauszus 0:f5c02b620688 92 void processing();
Lauszus 0:f5c02b620688 93 void receiveSerial();
Lauszus 0:f5c02b620688 94 void stopAndReset();
Lauszus 0:f5c02b620688 95
Lauszus 0:f5c02b620688 96 #endif