![](/media/cache/profiles/7d5430cf85f78c4b7aa09813b14bce0d.50x50_q85.jpg)
First trial
Dependencies: MPU6050 Motor ledControl2 mbed
Fork of BalancingRobotPS3 by
Diff: BalancingRobot.h
- Revision:
- 4:0b4c320bc948
- Parent:
- 3:c3963f37d597
- Child:
- 5:fc6c7a059759
--- a/BalancingRobot.h Sun Feb 26 13:11:10 2012 +0000 +++ b/BalancingRobot.h Fri Mar 02 09:06:47 2012 +0000 @@ -9,13 +9,11 @@ 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); @@ -23,13 +21,17 @@ 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; // Measurent noise covariance - Sv +const float R_angle = 0.03; // Measurement noise covariance - Sv double angle = 180; // It starts at 180 degrees double bias = 0; @@ -42,8 +44,7 @@ double gyroYrate; double pitch; -/* PID values */ -// Motors +/* PID variables */ double Kp = 11; double Ki = 2; double Kd = 12; @@ -78,15 +79,25 @@ 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; -long wheelVelocity; -double positionScale = 1000; // one resolution is 464 pulses -double velocityScale = 40; +int zoneA = 2000; +int zoneB = 1000; +double positionScaleA = 250; // one resolution is 464 pulses +double positionScaleB = 500; +double positionScaleC = 1000; +double velocityScaleMove = 40; +double velocityScaleStop = 40;//30 void calibrateSensors(); void PID(double restAngle, double offset); @@ -99,6 +110,5 @@ void receivePS3(); void receiveXbee(); void stopAndReset(); -void processing(); #endif \ No newline at end of file