Diff: main.cpp

Revision:

8:55ca92c0e39d

Parent:

7:e14e28d8cae3

Child:

9:07189a75e979

--- a/main.cpp	Mon Oct 05 13:28:12 2015 +0000
+++ b/main.cpp	Mon Oct 05 13:35:20 2015 +0000
@@ -44,7 +44,7 @@
 volatile float leftPwmDutyPrev = 1.0;
 volatile float leftPwmDutyChange;
 volatile float leftVelocity = 0.0;
-//Velocity to reach.
+
 float request;
 float request_pos;
 float request_neg;
@@ -64,33 +64,26 @@
     leftController.setMode(AUTO_MODE);
 
 
-    //Open results file.
-    //fp = fopen("/local/pidtest.csv", "w");
-    
-    endTimer.start();
-
-    //Set velocity set point.
-    
-    
-    
-    //Run for 3 seconds.
+    endTimer.start(); //Run for 100 seconds.
     while (endTimer.read() < 100){
+        // get 'emg' signal
         request_pos = pot1.read();
         request_neg = pot2.read();
         
+        // determine if forward or backward signal is stronger and set reference
         if (request_pos > request_neg){
             request = request_pos; 
         } 
         else {
             request = -request_neg;
         }
+        leftController.setSetPoint(request);
+        
         // Velocity calculation
-        leftController.setSetPoint(request);
         leftPulses = leftQei.getPulses();
         leftVelocity = ((float)(leftPulses - leftPrevPulses))/ (30000.0* RATE);
         leftPrevPulses = leftPulses;
-        
-        
+                
         // PID control
         leftController.setProcessValue(leftVelocity);
         leftPwmDuty = leftController.compute();
@@ -103,13 +96,13 @@
             leftMotor = leftPwmDuty;
         }
         
+        // User feadback
         scope.set(0, request);
         scope.set(1, leftPwmDuty);
         scope.set(2, leftVelocity);
         scope.send();
         pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
 
-        //fprintf(fp, "%f,%f\n", leftVelocity, goal);
         wait(RATE);
     }