Lakshmanan Gopalakrishnan
First trial
Dependencies: MPU6050 Motor ledControl2 mbed
Fork of
BalancingRobotPS3
by 
Kristian Lauszus
BalancingRobot.h@5:fc6c7a059759, 2012-03-04 (annotated)
- Committer:
- Lauszus
- Date:
- Sun Mar 04 02:11:58 2012 +0000
- Revision:
- 5:fc6c7a059759
- Parent:
- 4:0b4c320bc948
- Child:
- 7:f1d24c6119ac
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 | #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 | |
| Lauszus | 0:f5c02b620688 | 13 | /* Right motor */ |
| Lauszus | 0:f5c02b620688 | 14 | DigitalOut rightA(p24); |
| Lauszus | 0:f5c02b620688 | 15 | DigitalOut rightB(p25); |
| Lauszus | 0:f5c02b620688 | 16 | PwmOut rightPWM(p26); |
| Lauszus | 0:f5c02b620688 | 17 | |
| Lauszus | 0:f5c02b620688 | 18 | /* IMU */ |
| Lauszus | 0:f5c02b620688 | 19 | AnalogIn gyroY(p17); |
| Lauszus | 0:f5c02b620688 | 20 | AnalogIn accX(p18); |
| Lauszus | 0:f5c02b620688 | 21 | AnalogIn accY(p19); |
| Lauszus | 0:f5c02b620688 | 22 | AnalogIn accZ(p20); |
| Lauszus | 0:f5c02b620688 | 23 | |
| Lauszus | 4:0b4c320bc948 | 24 | /* Battery voltage */ |
| Lauszus | 4:0b4c320bc948 | 25 | AnalogIn batteryVoltage(p15); // The analog pin is connected to a 56k-10k voltage divider |
| Lauszus | 4:0b4c320bc948 | 26 | DigitalOut buzzer(p5); // Connected to a BC547 transistor - there is a protection diode at the buzzer |
| Lauszus | 4:0b4c320bc948 | 27 | |
| Lauszus | 0:f5c02b620688 | 28 | // Zero voltage values for the sensors - [0] = gyroY, [1] = accX, [2] = accY, [3] = accZ |
| Lauszus | 0:f5c02b620688 | 29 | double zeroValues[4]; |
| Lauszus | 0:f5c02b620688 | 30 | |
| Lauszus | 0:f5c02b620688 | 31 | /* Kalman filter variables and constants */ |
| Lauszus | 0:f5c02b620688 | 32 | const float Q_angle = 0.001; // Process noise covariance for the accelerometer - Sw |
| Lauszus | 0:f5c02b620688 | 33 | const float Q_gyro = 0.003; // Process noise covariance for the gyro - Sw |
| Lauszus | 4:0b4c320bc948 | 34 | const float R_angle = 0.03; // Measurement noise covariance - Sv |
| Lauszus | 0:f5c02b620688 | 35 | |
| Lauszus | 0:f5c02b620688 | 36 | double angle = 180; // It starts at 180 degrees |
| Lauszus | 0:f5c02b620688 | 37 | double bias = 0; |
| Lauszus | 0:f5c02b620688 | 38 | double P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0; |
| Lauszus | 0:f5c02b620688 | 39 | double dt, y, S; |
| Lauszus | 0:f5c02b620688 | 40 | double K_0, K_1; |
| Lauszus | 0:f5c02b620688 | 41 | |
| Lauszus | 0:f5c02b620688 | 42 | // Results |
| Lauszus | 0:f5c02b620688 | 43 | double accYangle; |
| Lauszus | 0:f5c02b620688 | 44 | double gyroYrate; |
| Lauszus | 0:f5c02b620688 | 45 | double pitch; |
| Lauszus | 0:f5c02b620688 | 46 | |
| Lauszus | 4:0b4c320bc948 | 47 | /* PID variables */ |
| Lauszus | 5:fc6c7a059759 | 48 | double Kp = 8; //11 - 7 |
| Lauszus | 5:fc6c7a059759 | 49 | double Ki = 2; //2 |
| Lauszus | 5:fc6c7a059759 | 50 | double Kd = 9; //12 |
| Lauszus | 0:f5c02b620688 | 51 | double targetAngle = 90; |
| Lauszus | 0:f5c02b620688 | 52 | |
| Lauszus | 0:f5c02b620688 | 53 | double lastError; |
| Lauszus | 0:f5c02b620688 | 54 | double iTerm; |
| Lauszus | 0:f5c02b620688 | 55 | |
| Lauszus | 0:f5c02b620688 | 56 | /* Used for timing */ |
| Lauszus | 0:f5c02b620688 | 57 | long timer; |
| Lauszus | 0:f5c02b620688 | 58 | |
| Lauszus | 0:f5c02b620688 | 59 | const long STD_LOOP_TIME = 10*1000; // Fixed time loop of 10 milliseconds |
| Lauszus | 0:f5c02b620688 | 60 | long lastLoopTime = STD_LOOP_TIME; |
| Lauszus | 0:f5c02b620688 | 61 | long lastLoopUsefulTime = STD_LOOP_TIME; |
| Lauszus | 0:f5c02b620688 | 62 | long loopStartTime; |
| Lauszus | 0:f5c02b620688 | 63 | |
| Lauszus | 0:f5c02b620688 | 64 | enum Motor { |
| Lauszus | 0:f5c02b620688 | 65 | left, |
| Lauszus | 0:f5c02b620688 | 66 | right, |
| Lauszus | 0:f5c02b620688 | 67 | both, |
| Lauszus | 0:f5c02b620688 | 68 | }; |
| Lauszus | 0:f5c02b620688 | 69 | enum Direction { |
| Lauszus | 0:f5c02b620688 | 70 | forward, |
| Lauszus | 0:f5c02b620688 | 71 | backward, |
| Lauszus | 0:f5c02b620688 | 72 | }; |
| Lauszus | 0:f5c02b620688 | 73 | |
| Lauszus | 0:f5c02b620688 | 74 | bool steerForward; |
| Lauszus | 0:f5c02b620688 | 75 | bool steerBackward; |
| Lauszus | 3:c3963f37d597 | 76 | bool steerStop = true; // Stop by default |
| Lauszus | 0:f5c02b620688 | 77 | bool steerLeft; |
| Lauszus | 0:f5c02b620688 | 78 | bool steerRotateLeft; |
| Lauszus | 0:f5c02b620688 | 79 | bool steerRight; |
| Lauszus | 0:f5c02b620688 | 80 | bool steerRotateRight; |
| Lauszus | 0:f5c02b620688 | 81 | |
| Lauszus | 4:0b4c320bc948 | 82 | bool stopped; |
| Lauszus | 4:0b4c320bc948 | 83 | |
| Lauszus | 4:0b4c320bc948 | 84 | const double turnSpeed = 0.1; |
| Lauszus | 4:0b4c320bc948 | 85 | const double rotateSpeed = 0.2; |
| Lauszus | 4:0b4c320bc948 | 86 | |
| Lauszus | 2:caec5534774d | 87 | double targetOffset = 0; |
| Lauszus | 2:caec5534774d | 88 | |
| Lauszus | 3:c3963f37d597 | 89 | uint8_t loopCounter = 0; // Used for wheel velocity |
| Lauszus | 3:c3963f37d597 | 90 | long wheelPosition; |
| Lauszus | 3:c3963f37d597 | 91 | long lastWheelPosition; |
| Lauszus | 4:0b4c320bc948 | 92 | long wheelVelocity; |
| Lauszus | 3:c3963f37d597 | 93 | long targetPosition; |
| Lauszus | 5:fc6c7a059759 | 94 | int zoneA = 4000; // 2000 |
| Lauszus | 5:fc6c7a059759 | 95 | int zoneB = 2000; // 1000 |
| Lauszus | 4:0b4c320bc948 | 96 | double positionScaleA = 250; // one resolution is 464 pulses |
| Lauszus | 4:0b4c320bc948 | 97 | double positionScaleB = 500; |
| Lauszus | 4:0b4c320bc948 | 98 | double positionScaleC = 1000; |
| Lauszus | 4:0b4c320bc948 | 99 | double velocityScaleMove = 40; |
| Lauszus | 5:fc6c7a059759 | 100 | double velocityScaleStop = 30;//30 - 40 - 60 |
| Lauszus | 3:c3963f37d597 | 101 | |
| Lauszus | 0:f5c02b620688 | 102 | void calibrateSensors(); |
| Lauszus | 2:caec5534774d | 103 | void PID(double restAngle, double offset); |
| Lauszus | 0:f5c02b620688 | 104 | double kalman(double newAngle, double newRate, double looptime); |
| Lauszus | 0:f5c02b620688 | 105 | double getGyroYrate(); |
| Lauszus | 0:f5c02b620688 | 106 | double getAccY(); |
| Lauszus | 0:f5c02b620688 | 107 | void move(Motor motor, Direction direction, float speed); |
| Lauszus | 0:f5c02b620688 | 108 | void stop(Motor motor); |
| Lauszus | 0:f5c02b620688 | 109 | void processing(); |
| Lauszus | 2:caec5534774d | 110 | void receivePS3(); |
| Lauszus | 2:caec5534774d | 111 | void receiveXbee(); |
| Lauszus | 0:f5c02b620688 | 112 | void stopAndReset(); |
| Lauszus | 0:f5c02b620688 | 113 | |
| Lauszus | 0:f5c02b620688 | 114 | #endif |