Diff: BalancingRobot.cpp

Revision:

7:f1d24c6119ac

Parent:

6:defe36ce2346

Child:

9:67f2110fce8e

--- a/BalancingRobot.cpp	Tue Mar 06 22:38:41 2012 +0000
+++ b/BalancingRobot.cpp	Mon Apr 09 21:40:47 2012 +0000
@@ -28,39 +28,39 @@
     xbee.baud(115200);
     ps3.baud(115200);
     debug.baud(115200);
-
+    
     /* Set PWM frequency */
     leftPWM.period(0.00005); // The motor driver can handle a pwm frequency up to 20kHz (1/20000=0.00005)
     rightPWM.period(0.00005);
-
+    
     /* Calibrate the gyro and accelerometer relative to ground */
     calibrateSensors();
-
+    
     xbee.printf("Initialized\n");
     processing(); // Send output to processing application
-
+    
     /* Setup timing */
     t.start();
     loopStartTime = t.read_us();
     timer = loopStartTime;
-
+    
     while (1) {
         /* Calculate pitch */
         accYangle = getAccY();
         gyroYrate = getGyroYrate();
-
+        
         // See my guide for more info about calculation the angles and the Kalman filter: http://arduino.cc/forum/index.php/topic,58048.0.html
         pitch = kalman(accYangle, gyroYrate, t.read_us() - timer); // calculate the angle using a Kalman filter
         timer = t.read_us();
-
+        
         //debug.printf("Pitch: %f, accYangle: %f\n",pitch,accYangle);
-
+        
         /* Drive motors */
         if (pitch < 60 || pitch > 120) // Stop if falling or laying down
             stopAndReset();
         else
             PID(targetAngle,targetOffset);
-
+        
         /* Update wheel velocity every 100ms */
         loopCounter++;
         if (loopCounter%10 == 0) { // If remainder is equal 0, it must be 10,20,30 etc.
@@ -69,20 +69,20 @@
             wheelVelocity = wheelPosition - lastWheelPosition;
             lastWheelPosition = wheelPosition;
             //xbee.printf("WheelVelocity: %i\n",wheelVelocity);
-
+            
             if (abs(wheelVelocity) <= 20 && !stopped) { // Set new targetPosition if breaking
                 targetPosition = wheelPosition;
                 stopped = true;
                 //xbee.printf("Stopped\n");
             }
         }
-
+        
         /* Check for incoming serial data */
         if (ps3.readable()) // Check if there's any incoming data
             receivePS3();
         if (xbee.readable()) // For setting the PID values
             receiveXbee();
-
+        
         /* Read battery voltage every 1s */
         if (loopCounter == 100) {
             loopCounter = 0; // Reset loop counter
@@ -90,16 +90,16 @@
             analogVoltage *= 6.6; // The analog pin is connected to a 56k-10k voltage divider
             xbee.printf("analogVoltage: %f - timer: %i\n",analogVoltage,t.read_ms());
             if (analogVoltage < 9 && pitch > 60 && pitch < 120) // Set buzzer on, if voltage gets critical low
-                buzzer = 1; // The mbed resets at aproximatly 1V           
+                buzzer = 1; // The mbed resets at aproximatly 8.5V           
         }
-
+        
         /* Use a time fixed loop */
         lastLoopUsefulTime = t.read_us() - loopStartTime;
         if (lastLoopUsefulTime < STD_LOOP_TIME)
             wait_us(STD_LOOP_TIME - lastLoopUsefulTime);
         lastLoopTime = t.read_us() - loopStartTime; // only used for debugging
         loopStartTime = t.read_us();
-
+        
         //debug.printf("%i,%i\n",lastLoopUsefulTime,lastLoopTime);
     }
 }
@@ -123,9 +123,9 @@
             restAngle -= (double)positionError/positionScaleB;
         else // Inside zone C
             restAngle -= (double)positionError/positionScaleC;
-
+        
         restAngle -= (double)wheelVelocity/velocityScaleStop;
-
+        
         if (restAngle < 80) // Limit rest Angle
             restAngle = 80;
         else if (restAngle > 100)
@@ -138,11 +138,11 @@
     iTerm += Ki * error;
     double dTerm = Kd * (error - lastError);
     lastError = error;
-
+    
     double PIDValue = pTerm + iTerm + dTerm;
-
+    
     //debug.printf("Pitch: %5.2f\tPIDValue:  %5.2f\tpTerm: %5.2f\tiTerm: %5.2f\tdTerm: %5.2f\tKp: %5.2f\tKi: %5.2f\tKd: %5.2f\n",pitch,PIDValue,pTerm,iTerm,dTerm,Kp,Ki,Kd);
-
+    
     /* Steer robot sideways */
     double PIDLeft;
     double PIDRight;
@@ -163,7 +163,7 @@
         PIDRight = PIDValue;
     }
     PIDLeft *= 0.95; // compensate for difference in the motors
-
+    
     /* Set PWM Values */
     if (PIDLeft >= 0)
         move(left, forward, PIDLeft);
@@ -184,7 +184,7 @@
         i++;
     }
     //debug.printf("Input: %s\n",input);
-
+    
     // Set all false
     steerForward = false;
     steerBackward = false;
@@ -193,7 +193,7 @@
     steerRotateLeft = false;
     steerRight = false;
     steerRotateRight = false;
-
+    
     /* For remote control */
     if (input[0] == 'F') { // Forward
         strtok(input, ","); // Ignore 'F'
@@ -220,7 +220,7 @@
         stopped = false;
         targetPosition = wheelPosition;
     }
-
+    
     else if (input[0] == 'T') { // Set the target angle
         strtok(input, ","); // Ignore 'T'
         targetAngle = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
@@ -240,7 +240,7 @@
         i++;
     }
     //debug.printf("Input: %s\n",input);
-
+    
     if (input[0] == 'T') { // Set the target angle
         strtok(input, ","); // Ignore 'T'
         targetAngle = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
@@ -268,41 +268,41 @@
     lastError = 0;
     iTerm = 0;
     targetPosition = wheelPosition;
-    buzzer= 0;
+    buzzer = 0;
 }
 double kalman(double newAngle, double newRate, double dtime) {
     // KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
     // with slightly modifications by Kristian Lauszus
     // See http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf and http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf for more information
     dt = dtime / 1000000; // Convert from microseconds to seconds
-
+    
     // Discrete Kalman filter time update equations - Time Update ("Predict")
     // Update xhat - Project the state ahead
     angle += dt * (newRate - bias);
-
+    
     // Update estimation error covariance - Project the error covariance ahead
     P_00 += -dt * (P_10 + P_01) + Q_angle * dt;
     P_01 += -dt * P_11;
     P_10 += -dt * P_11;
     P_11 += +Q_gyro * dt;
-
+    
     // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
     // Calculate Kalman gain - Compute the Kalman gain
     S = P_00 + R_angle;
     K_0 = P_00 / S;
     K_1 = P_10 / S;
-
+    
     // Calculate angle and resting rate - Update estimate with measurement zk
     y = newAngle - angle;
     angle += K_0 * y;
     bias += K_1 * y;
-
+    
     // Calculate estimation error covariance - Update the error covariance
     P_00 -= K_0 * P_00;
     P_01 -= K_0 * P_01;
     P_10 -= K_1 * P_00;
     P_11 -= K_1 * P_01;
-
+    
     return angle;
 }
 double getGyroYrate() {
@@ -316,15 +316,15 @@
     double accYval = (accY.read() - zeroValues[2]) / 0.1;
     accYval--;//-1g when lying down
     double accZval = (accZ.read() - zeroValues[3]) / 0.1;
-
-    double R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2)); // Calculate the force vector
+    
+    double R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2)); // Calculate the length of force vector
     double angleY = acos(accYval / R) * RAD_TO_DEG;
-
+    
     return angleY;
 }
 void calibrateSensors() {
     LEDs = 0xF; // Turn all onboard LEDs on
-
+    
     double adc[4] = {0,0,0,0};
     for (uint8_t i = 0; i < 100; i++) { // Take the average of 100 readings
         adc[0] += gyroY.read();
@@ -337,7 +337,7 @@
     zeroValues[1] = adc[1] / 100; // Accelerometer X-axis
     zeroValues[2] = adc[2] / 100; // Accelerometer Y-axis
     zeroValues[3] = adc[3] / 100; // Accelerometer Z-axis
-
+    
     LEDs = 0x0; // Turn all onboard LEDs off
 }
 void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f)
@@ -365,13 +365,13 @@
         if (direction == forward) {
             leftA = 0;
             leftB = 1;
-
+            
             rightA = 0;
             rightB = 1;
         } else if (direction == backward) {
             leftA = 1;
             leftB = 0;
-
+            
             rightA = 1;
             rightB = 0;
         }
@@ -390,7 +390,7 @@
         leftPWM = 1;
         leftA = 1;
         leftB = 1;
-
+        
         rightPWM = 1;
         rightA = 1;
         rightB = 1;