Lakshmanan Gopalakrishnan / Mbed 2 deprecated Balancing_Robot_2

First trial

Dependencies:   MPU6050 Motor ledControl2 mbed

Fork of BalancingRobotPS3 by Kristian Lauszus

BalancingRobot.h@10:8050817ae610, 2016-08-25 (annotated)

Committer:
lakshmananag
Date:
Thu Aug 25 22:55:36 2016 +0000
Revision:
10:8050817ae610
Parent:
9:67f2110fce8e 
First trial

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 */
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