![](/media/cache/profiles/7d5430cf85f78c4b7aa09813b14bce0d.50x50_q85.jpg)
First trial
Dependencies: MPU6050 Motor ledControl2 mbed
Fork of BalancingRobotPS3 by
BalancingRobot.h
- Committer:
- Lauszus
- Date:
- 2012-02-26
- Revision:
- 3:c3963f37d597
- Parent:
- 2:caec5534774d
- Child:
- 4:0b4c320bc948
File content as of revision 3:c3963f37d597:
#ifndef _balancingrobot_h_ #define _balancingrobot_h_ #define RAD_TO_DEG 57.295779513082320876798154814105 // 180/pi BusOut LEDs(LED1, LED2, LED3, 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 steerStop = true; // Stop by default bool steerLeft; bool steerRotateLeft; bool steerRight; bool steerRotateRight; double targetOffset = 0; uint8_t loopCounter = 0; // Used for wheel velocity long wheelPosition; long lastWheelPosition; long targetPosition; long wheelVelocity; double positionScale = 1000; // one resolution is 464 pulses double velocityScale = 40; void calibrateSensors(); void PID(double restAngle, double offset); 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 receivePS3(); void receiveXbee(); void stopAndReset(); void processing(); #endif