Lakshmanan Gopalakrishnan / Mbed 2 deprecated Balancing_Robot_2

First trial

Dependencies:   MPU6050 Motor ledControl2 mbed

Fork of BalancingRobotPS3 by Kristian Lauszus

BalancingRobot.h@3:c3963f37d597, 2012-02-26 (annotated)

Committer:
Lauszus
Date:
Sun Feb 26 13:11:10 2012 +0000
Revision:
3:c3963f37d597
Parent:
2:caec5534774d
Child:
4:0b4c320bc948
Implemented encoders

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