First trial
Dependencies: MPU6050 Motor ledControl2 mbed
Fork of BalancingRobotPS3 by
Diff: BalancingRobot.h
- Revision:
- 0:f5c02b620688
- Child:
- 1:01295228342f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BalancingRobot.h Tue Feb 14 10:48:01 2012 +0000 @@ -0,0 +1,96 @@ +#ifndef _balancingrobot_h_ +#define _balancingrobot_h_ + +#include "mbed.h" + +#define RAD_TO_DEG 57.295779513082320876798154814105 // 180/pi + +DigitalOut onboardLED1(LED1); +DigitalOut onboardLED2(LED2); +DigitalOut onboardLED3(LED3); +DigitalOut onboardLED4(LED4); + +/* Left motor */ +DigitalOut leftA(p21); +DigitalOut leftB(p22); +PwmOut leftPWM(p23); +//AnalogIn leftCurrentSense(p15); // Not used + +/* Right motor */ +DigitalOut rightA(p24); +DigitalOut rightB(p25); +PwmOut rightPWM(p26); +//AnalogIn rightCurrentSense(p16); // Not used + +/* IMU */ +AnalogIn gyroY(p17); +AnalogIn accX(p18); +AnalogIn accY(p19); +AnalogIn accZ(p20); + +// Zero voltage values for the sensors - [0] = gyroY, [1] = accX, [2] = accY, [3] = accZ +double zeroValues[4]; + +/* Kalman filter variables and constants */ +const float Q_angle = 0.001; // Process noise covariance for the accelerometer - Sw +const float Q_gyro = 0.003; // Process noise covariance for the gyro - Sw +const float R_angle = 0.03; // Measurent noise covariance - Sv + +double angle = 180; // It starts at 180 degrees +double bias = 0; +double P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0; +double dt, y, S; +double K_0, K_1; + +// Results +double accYangle; +double gyroYrate; +double pitch; + +/* PID values */ +// Motors +double Kp = 11; +double Ki = 2; +double Kd = 12; +double targetAngle = 90; + +double lastError; +double iTerm; + +/* Used for timing */ +long timer; + +const long STD_LOOP_TIME = 10*1000; // Fixed time loop of 10 milliseconds +long lastLoopTime = STD_LOOP_TIME; +long lastLoopUsefulTime = STD_LOOP_TIME; +long loopStartTime; + +enum Motor { + left, + right, + both, +}; +enum Direction { + forward, + backward, +}; + +bool steerForward; +bool steerBackward; +bool steerLeft; +bool steerRotateLeft; +bool steerRight; +bool steerRotateRight; + +void calibrateSensors(); +void PID(double restAngle); +double kalman(double newAngle, double newRate, double looptime); +double getGyroYrate(); +double getAccY(); +void move(Motor motor, Direction direction, float speed); +void stop(Motor motor); +void processing(); +void receiveSerial(); +void stopAndReset(); + +#endif \ No newline at end of file