Diff: BalancingRobot.h

Revision:

9:67f2110fce8e

Parent:

8:87dd9d5f7001

diff -r 87dd9d5f7001 -r 67f2110fce8e BalancingRobot.h
--- a/BalancingRobot.h	Fri Apr 13 19:45:25 2012 +0000
+++ b/BalancingRobot.h	Thu Aug 25 22:50:49 2016 +0000
@@ -6,51 +6,36 @@
 BusOut LEDs(LED1, LED2, LED3, LED4);
 
 /* Left motor */
-DigitalOut leftA(p21);
-DigitalOut leftB(p22);
+DigitalOut leftA(p5);
+DigitalOut leftB(p6);
 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 
+DigitalOut rightA(p7);
+DigitalOut rightB(p8);
+PwmOut rightPWM(p24);
 
-// 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;
+/* Encoder variables */
+float LeftWheelPosition = 0.0;
+float RightWheelPosition = 0.0;
 
 // Results
 double accYangle;
 double gyroYrate;
-double pitch;
+int16_t raw_accYangle;
+int16_t raw_gyroYrate;
 
 /* PID variables */
-double Kp = 8; //11 - 7
-double Ki = 2; //1
-double Kd = 9; //12
-double targetAngle = 90;
-
-double lastError;
+double Kp = 0.5; //11 - 7
+double Ki = 0.00001; //1
+double Kd = 0.01; //12
+double PIDValue = 0.0;
+double targetAngle = 1.05;
+double targetOffset = 0;
+float pitchAngle = 0;
+float rollAngle = 0;
+double error = 0.0;
+double lastError = 0.0;
 double iTerm;
 
 /* Used for timing */
@@ -84,8 +69,6 @@
 const double turnSpeed = 0.1;
 const double rotateSpeed = 0.2;
 
-double targetOffset = 0;
-
 uint8_t loopCounter = 0; // Used for wheel velocity and battery voltage
 long wheelPosition;
 long lastWheelPosition;
@@ -96,19 +79,17 @@
 double positionScaleA = 250; // one resolution is 464 pulses
 double positionScaleB = 500; 
 double positionScaleC = 1000;
-double velocityScaleMove = 40;
-double velocityScaleStop = 30;//30 - 40 - 60
+double velocityScaleMove = 1; //40;
+double velocityScaleStop = 1; //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 receiveBluetooth();
 void stopAndReset();
+void toggle_led1();
+void toggle_led2();
 
 #endif
\ No newline at end of file