Diff: Rover.cpp

Revision:

1:ffef6386027b

Parent:

0:8d15dc761944

--- a/Rover.cpp	Mon Aug 16 09:46:28 2010 +0000
+++ b/Rover.cpp	Thu Aug 26 14:41:08 2010 +0000
@@ -61,8 +61,6 @@
  */
 #include "Rover.h"
 
-Serial pc2(USBTX, USBRX);
-
 Rover::Rover(PinName leftMotorPwm,
              PinName leftMotorBrake,
              PinName leftMotorDirection,
@@ -91,7 +89,7 @@
         rightController(Kc, Ti, Td, PID_RATE),
         stateTicker(),
         logTicker(),
-        imu(IMU_RATE,
+        imu(IMU_RATE_,
             GYRO_MEAS_ERROR,
             ACCELEROMETER_RATE,
             GYROSCOPE_RATE) {
@@ -150,6 +148,7 @@
     degreesTurned_    = 0.0;
     leftStopFlag_     = 0;
     rightStopFlag_    = 0;
+    logIndex          = 0;
 
     //--------
     // BEGIN!
@@ -160,7 +159,14 @@
 
 }
 
-void Rover::move(int distance) {
+void Rover::move(float distance) {
+
+    //Convert from metres into millimetres.
+    distance *= 1000;
+    //Work out how many pulses are required to go that many millimetres.
+    distance *= PULSES_PER_MM;
+    //Make sure we scale the number of pulses according to our encoding method.
+    distance /= ENCODING;
 
     positionSetPoint_ = distance;
 
@@ -183,7 +189,15 @@
 
 void Rover::turn(int degrees) {
 
-    headingSetPoint_ = abs(degrees);
+    //Correct the amount to turn based on deviation during last segment.
+    headingSetPoint_ = abs(degrees) + (endHeading_ - startHeading_);
+    
+    //In case the rover tries to [pointlessly] turn >360 degrees.
+    if (headingSetPoint_ > 359.8){
+    
+        headingSetPoint_ -= 359.8;
+    
+    }
 
     //Rotating clockwise.
     if (degrees > 0) {
@@ -210,22 +224,28 @@
 
 void Rover::startLogging(void) {
 
-    logFile = fopen("/sd/roverlog.csv", "w");
-    fprintf(logFile, "leftPulses, rightPulses, leftVelocity, rightVelocity, heading, degreesTurned\n");
-    logTicker.attach(this, &Rover::log, LOG_RATE);
+    logFile = fopen("/local/roverlog.csv", "w");
+    fprintf(logFile, "leftPulses, rightPulses, leftVelocity, rightVelocity, heading\n");
+    //logTicker.attach(this, &Rover::log, LOG_RATE);
 
 }
 
 void Rover::stopLogging(void) {
 
+    //logFile = fopen("/local/roverlog.csv", "w");
+    //fprintf(logFile, "leftPulses, rightPulses, leftVelocity, rightVelocity, heading, degreesTurned\n");
+    //fprintf(logFile, "leftVelocity, rightVelocity\n");
+    //for(int i = 0; i < 1024; i++){
+    //    fprintf(logFile, "%f, %f\n", leftVelocityBuffer[i], rightVelocityBuffer[i]);
+    //}
     fclose(logFile);
 
 }
 
 void Rover::log(void) {
 
-    fprintf(logFile, "%i,%i,%f,%f,%f,%f\n",
-            leftPulses_, rightPulses_, leftVelocity_, rightVelocity_, imu.getYaw(), degreesTurned_);
+    //fprintf(logFile, "%i,%i,%f,%f,%f,%f\n",
+    //        leftPulses_, rightPulses_, leftVelocity_, rightVelocity_, imu.getYaw(), degreesTurned_);
 
 }
 
@@ -235,6 +255,7 @@
 
             //We're not moving so don't do anything!
         case (STATE_STATIONARY):
+
             break;
 
         case (STATE_MOVING_FORWARD):
@@ -247,7 +268,7 @@
                 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
                 leftPrevPulses_ = leftPulses_;
                 leftController.setProcessValue(leftVelocity_);
-                leftPwmDuty_ = leftController.getRealOutput();
+                leftPwmDuty_ = leftController.compute();
 
             } else {
                 leftStopFlag_ = 1;
@@ -261,8 +282,8 @@
                 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
                 rightPrevPulses_ = rightPulses_;
                 rightController.setProcessValue(rightVelocity_);
-                rightPwmDuty_ = rightController.getRealOutput();
-                
+                rightPwmDuty_ = rightController.compute();
+
             } else {
                 rightStopFlag_ = 1;
             }
@@ -275,6 +296,7 @@
                 rightPwmDuty_ = 1.0;
                 leftMotors.setPwmDuty(leftPwmDuty_);
                 rightMotors.setPwmDuty(rightPwmDuty_);
+                endHeading_ = imu.getYaw();
                 enterState(STATE_STATIONARY);
             }
 
@@ -289,7 +311,7 @@
                 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
                 leftPrevPulses_ = leftPulses_;
                 leftController.setProcessValue(fabs(leftVelocity_));
-                leftPwmDuty_ = leftController.getRealOutput();
+                leftPwmDuty_ = leftController.compute();
             } else {
                 leftStopFlag_ = 1;
             }
@@ -301,7 +323,7 @@
                 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
                 rightPrevPulses_ = rightPulses_;
                 rightController.setProcessValue(fabs(rightVelocity_));
-                rightPwmDuty_ = rightController.getRealOutput();
+                rightPwmDuty_ = rightController.compute();
 
             } else {
                 rightStopFlag_ = 1;
@@ -334,13 +356,13 @@
                 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
                 leftPrevPulses_ = leftPulses_;
                 leftController.setProcessValue(leftVelocity_);
-                leftPwmDuty_ = leftController.getRealOutput();
+                leftPwmDuty_ = leftController.compute();
 
                 rightPulses_ = rightQei.getPulses();
                 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
                 rightPrevPulses_ = rightPulses_;
                 rightController.setProcessValue(fabs(rightVelocity_));
-                rightPwmDuty_ = rightController.getRealOutput();
+                rightPwmDuty_ = rightController.compute();
 
                 leftMotors.setPwmDuty(leftPwmDuty_);
                 rightMotors.setPwmDuty(rightPwmDuty_);
@@ -371,13 +393,13 @@
                 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
                 leftPrevPulses_ = leftPulses_;
                 leftController.setProcessValue(fabs(leftVelocity_));
-                leftPwmDuty_ = leftController.getRealOutput();
+                leftPwmDuty_ = leftController.compute();
 
                 rightPulses_ = rightQei.getPulses();
                 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
                 rightPrevPulses_ = rightPulses_;
                 rightController.setProcessValue(rightVelocity_);
-                rightPwmDuty_ = rightController.getRealOutput();
+                rightPwmDuty_ = rightController.compute();
 
                 leftMotors.setPwmDuty(leftPwmDuty_);
                 rightMotors.setPwmDuty(rightPwmDuty_);
@@ -405,6 +427,15 @@
 
     }
 
+    if (logIndex < 1024) {
+        headingBuffer[logIndex] = imu.getYaw();
+        leftVelocityBuffer[logIndex] = leftVelocity_;
+        rightVelocityBuffer[logIndex] = rightVelocity_;
+        leftPositionBuffer[logIndex] = leftPulses_;
+        rightPositionBuffer[logIndex] = rightPulses_;
+        logIndex++;
+    }
+
 }
 
 void Rover::enterState(State state) {
@@ -449,6 +480,18 @@
             leftStopFlag_     = 0;
             rightStopFlag_    = 0;
 
+            for (int i = 0; i < logIndex; i++) {
+                fprintf(logFile, "%i, %i, %f, %f, %f\n", leftPositionBuffer[i],
+                        rightPositionBuffer[i],
+                        leftVelocityBuffer[i],
+                        rightVelocityBuffer[i],
+                        headingBuffer[i]);
+            }
+
+            logIndex = 0;
+            
+            imu.reset();
+
             state_ = STATE_STATIONARY;
 
             break;
@@ -466,6 +509,10 @@
             leftController.setSetPoint(1000);
             rightController.setSetPoint(1000);
 
+            logIndex = 0;
+            
+            startHeading_ = imu.getYaw();
+
             state_ = STATE_MOVING_FORWARD;
 
             break;
@@ -483,6 +530,8 @@
             leftController.setSetPoint(1000);
             rightController.setSetPoint(1000);
 
+            logIndex = 0;
+
             state_ = STATE_MOVING_BACKWARD;
 
             break;
@@ -504,6 +553,8 @@
             heading_ = fabs(imu.getYaw());
             prevHeading_ = heading_;
 
+            logIndex = 0;
+
             state_ = STATE_ROTATING_CLOCKWISE;
 
             break;
@@ -525,12 +576,16 @@
             heading_ = fabs(imu.getYaw());
             prevHeading_ = heading_;
 
+            logIndex = 0;
+
             state_ = STATE_ROTATING_COUNTER_CLOCKWISE;
 
             break;
 
         default:
 
+            state_ = STATE_STATIONARY;
+
             break;
 
     }