Berk & Ay / Mbed 2 deprecated Self-Balancing-Robot-BerkYasarYavuz_AdilBerkayTemiz

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?

UserRevisionLine numberNew 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