![](/media/cache/profiles/7d5430cf85f78c4b7aa09813b14bce0d.50x50_q85.jpg)
First trial
Dependencies: MPU6050 Motor ledControl2 mbed
Fork of BalancingRobotPS3 by
BalancingRobot.h@9:67f2110fce8e, 2016-08-25 (annotated)
- Committer:
- lakshmananag
- Date:
- Thu Aug 25 22:50:49 2016 +0000
- Revision:
- 9:67f2110fce8e
- Parent:
- 8:87dd9d5f7001
First trrial
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 */ |
lakshmananag | 9:67f2110fce8e | 9 | DigitalOut leftA(p5); |
lakshmananag | 9:67f2110fce8e | 10 | DigitalOut leftB(p6); |
Lauszus | 0:f5c02b620688 | 11 | PwmOut leftPWM(p23); |
Lauszus | 0:f5c02b620688 | 12 | |
Lauszus | 0:f5c02b620688 | 13 | /* Right motor */ |
lakshmananag | 9:67f2110fce8e | 14 | DigitalOut rightA(p7); |
lakshmananag | 9:67f2110fce8e | 15 | DigitalOut rightB(p8); |
lakshmananag | 9:67f2110fce8e | 16 | PwmOut rightPWM(p24); |
Lauszus | 4:0b4c320bc948 | 17 | |
lakshmananag | 9:67f2110fce8e | 18 | /* Encoder variables */ |
lakshmananag | 9:67f2110fce8e | 19 | float LeftWheelPosition = 0.0; |
lakshmananag | 9:67f2110fce8e | 20 | float RightWheelPosition = 0.0; |
Lauszus | 0:f5c02b620688 | 21 | |
Lauszus | 0:f5c02b620688 | 22 | // Results |
Lauszus | 0:f5c02b620688 | 23 | double accYangle; |
Lauszus | 0:f5c02b620688 | 24 | double gyroYrate; |
lakshmananag | 9:67f2110fce8e | 25 | int16_t raw_accYangle; |
lakshmananag | 9:67f2110fce8e | 26 | int16_t raw_gyroYrate; |
Lauszus | 0:f5c02b620688 | 27 | |
Lauszus | 4:0b4c320bc948 | 28 | /* PID variables */ |
lakshmananag | 9:67f2110fce8e | 29 | double Kp = 0.5; //11 - 7 |
lakshmananag | 9:67f2110fce8e | 30 | double Ki = 0.00001; //1 |
lakshmananag | 9:67f2110fce8e | 31 | double Kd = 0.01; //12 |
lakshmananag | 9:67f2110fce8e | 32 | double PIDValue = 0.0; |
lakshmananag | 9:67f2110fce8e | 33 | double targetAngle = 1.05; |
lakshmananag | 9:67f2110fce8e | 34 | double targetOffset = 0; |
lakshmananag | 9:67f2110fce8e | 35 | float pitchAngle = 0; |
lakshmananag | 9:67f2110fce8e | 36 | float rollAngle = 0; |
lakshmananag | 9:67f2110fce8e | 37 | double error = 0.0; |
lakshmananag | 9:67f2110fce8e | 38 | double lastError = 0.0; |
Lauszus | 0:f5c02b620688 | 39 | double iTerm; |
Lauszus | 0:f5c02b620688 | 40 | |
Lauszus | 0:f5c02b620688 | 41 | /* Used for timing */ |
Lauszus | 8:87dd9d5f7001 | 42 | unsigned long timer; |
Lauszus | 0:f5c02b620688 | 43 | |
Lauszus | 0:f5c02b620688 | 44 | const long STD_LOOP_TIME = 10*1000; // Fixed time loop of 10 milliseconds |
Lauszus | 0:f5c02b620688 | 45 | long lastLoopTime = STD_LOOP_TIME; |
Lauszus | 0:f5c02b620688 | 46 | long lastLoopUsefulTime = STD_LOOP_TIME; |
Lauszus | 0:f5c02b620688 | 47 | long loopStartTime; |
Lauszus | 0:f5c02b620688 | 48 | |
Lauszus | 0:f5c02b620688 | 49 | enum Motor { |
Lauszus | 0:f5c02b620688 | 50 | left, |
Lauszus | 0:f5c02b620688 | 51 | right, |
Lauszus | 0:f5c02b620688 | 52 | both, |
Lauszus | 0:f5c02b620688 | 53 | }; |
Lauszus | 0:f5c02b620688 | 54 | enum Direction { |
Lauszus | 0:f5c02b620688 | 55 | forward, |
Lauszus | 0:f5c02b620688 | 56 | backward, |
Lauszus | 0:f5c02b620688 | 57 | }; |
Lauszus | 0:f5c02b620688 | 58 | |
Lauszus | 0:f5c02b620688 | 59 | bool steerForward; |
Lauszus | 0:f5c02b620688 | 60 | bool steerBackward; |
Lauszus | 3:c3963f37d597 | 61 | bool steerStop = true; // Stop by default |
Lauszus | 0:f5c02b620688 | 62 | bool steerLeft; |
Lauszus | 0:f5c02b620688 | 63 | bool steerRotateLeft; |
Lauszus | 0:f5c02b620688 | 64 | bool steerRight; |
Lauszus | 0:f5c02b620688 | 65 | bool steerRotateRight; |
Lauszus | 0:f5c02b620688 | 66 | |
Lauszus | 4:0b4c320bc948 | 67 | bool stopped; |
Lauszus | 4:0b4c320bc948 | 68 | |
Lauszus | 4:0b4c320bc948 | 69 | const double turnSpeed = 0.1; |
Lauszus | 4:0b4c320bc948 | 70 | const double rotateSpeed = 0.2; |
Lauszus | 4:0b4c320bc948 | 71 | |
Lauszus | 7:f1d24c6119ac | 72 | uint8_t loopCounter = 0; // Used for wheel velocity and battery voltage |
Lauszus | 3:c3963f37d597 | 73 | long wheelPosition; |
Lauszus | 3:c3963f37d597 | 74 | long lastWheelPosition; |
Lauszus | 4:0b4c320bc948 | 75 | long wheelVelocity; |
Lauszus | 3:c3963f37d597 | 76 | long targetPosition; |
Lauszus | 5:fc6c7a059759 | 77 | int zoneA = 4000; // 2000 |
Lauszus | 5:fc6c7a059759 | 78 | int zoneB = 2000; // 1000 |
Lauszus | 4:0b4c320bc948 | 79 | double positionScaleA = 250; // one resolution is 464 pulses |
Lauszus | 4:0b4c320bc948 | 80 | double positionScaleB = 500; |
Lauszus | 4:0b4c320bc948 | 81 | double positionScaleC = 1000; |
lakshmananag | 9:67f2110fce8e | 82 | double velocityScaleMove = 1; //40; |
lakshmananag | 9:67f2110fce8e | 83 | double velocityScaleStop = 1; //30 - 40 - 60 |
Lauszus | 3:c3963f37d597 | 84 | |
Lauszus | 2:caec5534774d | 85 | void PID(double restAngle, double offset); |
Lauszus | 0:f5c02b620688 | 86 | double getGyroYrate(); |
Lauszus | 0:f5c02b620688 | 87 | double getAccY(); |
Lauszus | 0:f5c02b620688 | 88 | void move(Motor motor, Direction direction, float speed); |
Lauszus | 0:f5c02b620688 | 89 | void stop(Motor motor); |
lakshmananag | 9:67f2110fce8e | 90 | void receiveBluetooth(); |
Lauszus | 0:f5c02b620688 | 91 | void stopAndReset(); |
lakshmananag | 9:67f2110fce8e | 92 | void toggle_led1(); |
lakshmananag | 9:67f2110fce8e | 93 | void toggle_led2(); |
Lauszus | 0:f5c02b620688 | 94 | |
Lauszus | 0:f5c02b620688 | 95 | #endif |