![](/media/cache/profiles/7d5430cf85f78c4b7aa09813b14bce0d.50x50_q85.jpg)
First trial
Dependencies: MPU6050 Motor ledControl2 mbed
Fork of BalancingRobotPS3 by
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?
User | Revision | Line number | New 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 |