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