Kristian Lauszus
/
BalancingRobotPS3
Code for my balancing robot, controlled with a PS3 controller via bluetooth
BalancingRobot.h
- Committer:
- Lauszus
- Date:
- 2012-03-04
- Revision:
- 5:fc6c7a059759
- Parent:
- 4:0b4c320bc948
- Child:
- 7:f1d24c6119ac
File content as of revision 5:fc6c7a059759:
#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); /* Right motor */ DigitalOut rightA(p24); DigitalOut rightB(p25); PwmOut rightPWM(p26); /* IMU */ AnalogIn gyroY(p17); AnalogIn accX(p18); AnalogIn accY(p19); AnalogIn accZ(p20); /* Battery voltage */ AnalogIn batteryVoltage(p15); // The analog pin is connected to a 56k-10k voltage divider DigitalOut buzzer(p5); // Connected to a BC547 transistor - there is a protection diode at the buzzer // 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; // Measurement 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 variables */ double Kp = 8; //11 - 7 double Ki = 2; //2 double Kd = 9; //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; bool stopped; const double turnSpeed = 0.1; const double rotateSpeed = 0.2; double targetOffset = 0; uint8_t loopCounter = 0; // Used for wheel velocity long wheelPosition; long lastWheelPosition; long wheelVelocity; long targetPosition; int zoneA = 4000; // 2000 int zoneB = 2000; // 1000 double positionScaleA = 250; // one resolution is 464 pulses double positionScaleB = 500; double positionScaleC = 1000; double velocityScaleMove = 40; double velocityScaleStop = 30;//30 - 40 - 60 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(); #endif