Committed for sharing.
Dependencies: mbed SelfBalancingRobot_BerkYasarYavuz_AdilBerkayTemiz Motor ledControl2 HC_SR04_Ultrasonic_Library
BalancingRobot.h@11:0a2944e7be23, 2020-01-01 (annotated)
- Committer:
- berkyavuz1997
- Date:
- Wed Jan 01 20:31:22 2020 +0000
- Revision:
- 11:0a2944e7be23
- Parent:
- 9:67f2110fce8e
Committed for sharing.
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 */ |
berkyavuz1997 | 11:0a2944e7be23 | 9 | DigitalOut leftA(D6); |
berkyavuz1997 | 11:0a2944e7be23 | 10 | DigitalOut leftB(D5); |
berkyavuz1997 | 11:0a2944e7be23 | 11 | PwmOut leftPWM(D7); |
Lauszus | 0:f5c02b620688 | 12 | |
Lauszus | 0:f5c02b620688 | 13 | /* Right motor */ |
berkyavuz1997 | 11:0a2944e7be23 | 14 | DigitalOut rightA(D4); |
berkyavuz1997 | 11:0a2944e7be23 | 15 | DigitalOut rightB(D3); |
berkyavuz1997 | 11:0a2944e7be23 | 16 | PwmOut rightPWM(D2); |
Lauszus | 0:f5c02b620688 | 17 | |
Lauszus | 0:f5c02b620688 | 18 | // Results |
Lauszus | 0:f5c02b620688 | 19 | double accYangle; |
Lauszus | 0:f5c02b620688 | 20 | double gyroYrate; |
lakshmananag | 9:67f2110fce8e | 21 | int16_t raw_accYangle; |
lakshmananag | 9:67f2110fce8e | 22 | int16_t raw_gyroYrate; |
Lauszus | 0:f5c02b620688 | 23 | |
Lauszus | 4:0b4c320bc948 | 24 | /* PID variables */ |
berkyavuz1997 | 11:0a2944e7be23 | 25 | |
berkyavuz1997 | 11:0a2944e7be23 | 26 | double Kp = 0.085; //0.100; //0.0400; // 11 - 7 |
berkyavuz1997 | 11:0a2944e7be23 | 27 | double Ki = 0.00900; //0.005;//0.00320; //1 0.001 sanki gideri var mı ne |
berkyavuz1997 | 11:0a2944e7be23 | 28 | double Kd = 0.050;//0.0125 baya iyi //0.0333; //12 |
berkyavuz1997 | 11:0a2944e7be23 | 29 | |
berkyavuz1997 | 11:0a2944e7be23 | 30 | //double Kp = 0.185; //11 - 7 almost well //0.1 |
berkyavuz1997 | 11:0a2944e7be23 | 31 | //double Ki = 0.0000; //1 //0.005 integralli ilk iyi gibi |
berkyavuz1997 | 11:0a2944e7be23 | 32 | //double Kd = 0.1; //12 //0.01 |
berkyavuz1997 | 11:0a2944e7be23 | 33 | |
lakshmananag | 9:67f2110fce8e | 34 | double PIDValue = 0.0; |
berkyavuz1997 | 11:0a2944e7be23 | 35 | double DesiredAngle; |
berkyavuz1997 | 11:0a2944e7be23 | 36 | double ConstantAngle; |
berkyavuz1997 | 11:0a2944e7be23 | 37 | unsigned int distF; |
berkyavuz1997 | 11:0a2944e7be23 | 38 | unsigned int distB; |
lakshmananag | 9:67f2110fce8e | 39 | double targetOffset = 0; |
lakshmananag | 9:67f2110fce8e | 40 | float pitchAngle = 0; |
lakshmananag | 9:67f2110fce8e | 41 | float rollAngle = 0; |
berkyavuz1997 | 11:0a2944e7be23 | 42 | double error1 = 0.0; |
lakshmananag | 9:67f2110fce8e | 43 | double lastError = 0.0; |
Lauszus | 0:f5c02b620688 | 44 | double iTerm; |
Lauszus | 0:f5c02b620688 | 45 | |
Lauszus | 0:f5c02b620688 | 46 | /* Used for timing */ |
Lauszus | 8:87dd9d5f7001 | 47 | unsigned long timer; |
Lauszus | 0:f5c02b620688 | 48 | |
Lauszus | 0:f5c02b620688 | 49 | const long STD_LOOP_TIME = 10*1000; // Fixed time loop of 10 milliseconds |
Lauszus | 0:f5c02b620688 | 50 | long lastLoopTime = STD_LOOP_TIME; |
Lauszus | 0:f5c02b620688 | 51 | long lastLoopUsefulTime = STD_LOOP_TIME; |
Lauszus | 0:f5c02b620688 | 52 | long loopStartTime; |
Lauszus | 0:f5c02b620688 | 53 | |
Lauszus | 0:f5c02b620688 | 54 | enum Motor { |
Lauszus | 0:f5c02b620688 | 55 | left, |
Lauszus | 0:f5c02b620688 | 56 | right, |
Lauszus | 0:f5c02b620688 | 57 | both, |
Lauszus | 0:f5c02b620688 | 58 | }; |
Lauszus | 0:f5c02b620688 | 59 | enum Direction { |
Lauszus | 0:f5c02b620688 | 60 | forward, |
Lauszus | 0:f5c02b620688 | 61 | backward, |
Lauszus | 0:f5c02b620688 | 62 | }; |
Lauszus | 0:f5c02b620688 | 63 | |
Lauszus | 0:f5c02b620688 | 64 | bool steerForward; |
Lauszus | 0:f5c02b620688 | 65 | bool steerBackward; |
Lauszus | 3:c3963f37d597 | 66 | bool steerStop = true; // Stop by default |
Lauszus | 0:f5c02b620688 | 67 | bool steerLeft; |
Lauszus | 0:f5c02b620688 | 68 | bool steerRotateLeft; |
Lauszus | 0:f5c02b620688 | 69 | bool steerRight; |
Lauszus | 0:f5c02b620688 | 70 | bool steerRotateRight; |
Lauszus | 0:f5c02b620688 | 71 | |
Lauszus | 4:0b4c320bc948 | 72 | bool stopped; |
Lauszus | 4:0b4c320bc948 | 73 | |
Lauszus | 4:0b4c320bc948 | 74 | const double turnSpeed = 0.1; |
Lauszus | 4:0b4c320bc948 | 75 | const double rotateSpeed = 0.2; |
Lauszus | 4:0b4c320bc948 | 76 | |
Lauszus | 7:f1d24c6119ac | 77 | uint8_t loopCounter = 0; // Used for wheel velocity and battery voltage |
Lauszus | 3:c3963f37d597 | 78 | long wheelPosition; |
Lauszus | 3:c3963f37d597 | 79 | long lastWheelPosition; |
Lauszus | 4:0b4c320bc948 | 80 | long wheelVelocity; |
Lauszus | 3:c3963f37d597 | 81 | long targetPosition; |
Lauszus | 5:fc6c7a059759 | 82 | int zoneA = 4000; // 2000 |
Lauszus | 5:fc6c7a059759 | 83 | int zoneB = 2000; // 1000 |
Lauszus | 4:0b4c320bc948 | 84 | double positionScaleA = 250; // one resolution is 464 pulses |
Lauszus | 4:0b4c320bc948 | 85 | double positionScaleB = 500; |
Lauszus | 4:0b4c320bc948 | 86 | double positionScaleC = 1000; |
lakshmananag | 9:67f2110fce8e | 87 | double velocityScaleMove = 1; //40; |
lakshmananag | 9:67f2110fce8e | 88 | double velocityScaleStop = 1; //30 - 40 - 60 |
Lauszus | 3:c3963f37d597 | 89 | |
Lauszus | 2:caec5534774d | 90 | void PID(double restAngle, double offset); |
Lauszus | 0:f5c02b620688 | 91 | double getGyroYrate(); |
Lauszus | 0:f5c02b620688 | 92 | double getAccY(); |
Lauszus | 0:f5c02b620688 | 93 | void move(Motor motor, Direction direction, float speed); |
Lauszus | 0:f5c02b620688 | 94 | void stop(Motor motor); |
lakshmananag | 9:67f2110fce8e | 95 | void receiveBluetooth(); |
Lauszus | 0:f5c02b620688 | 96 | void stopAndReset(); |
lakshmananag | 9:67f2110fce8e | 97 | void toggle_led1(); |
lakshmananag | 9:67f2110fce8e | 98 | void toggle_led2(); |
Lauszus | 0:f5c02b620688 | 99 | |
Lauszus | 0:f5c02b620688 | 100 | #endif |