Committed for sharing.
Dependencies: mbed SelfBalancingRobot_BerkYasarYavuz_AdilBerkayTemiz Motor ledControl2 HC_SR04_Ultrasonic_Library
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?
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 | //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 |