Diff: main.cpp

Revision:

12:d7bb475bb82d

Parent:

9:07189a75e979

Child:

13:40141b362092

--- a/main.cpp	Tue Oct 06 10:20:50 2015 +0000
+++ b/main.cpp	Tue Oct 06 10:51:02 2015 +0000
@@ -32,14 +32,25 @@
                 request = -request_neg;
             }
             leftController.setSetPoint(request);
+            rightController.setSetPoint(request);
             
+            // *******************
             // Velocity calculation
+            // left
             leftPulses = leftQei.getPulses();
             leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE;
             leftVelocity = leftVelocity/30000; // scale to 0 - 1, max velocity = 30000
             leftPrevPulses = leftPulses;
-                    
+            
+            // right
+            rightPulses = rightQei.getPulses();
+            rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE;
+            rightVelocity = rightVelocity/30000; // scale to 0 - 1, max velocity = 30000
+            rightPrevPulses = rightPulses;
+            
+            // ***********
             // PID control
+            // left
             leftController.setProcessValue(leftVelocity);
             leftPwmDuty = leftController.compute();
             if (leftPwmDuty < 0){
@@ -51,14 +62,22 @@
                 leftMotor = leftPwmDuty;
             }
             
-            // testing the right motor
-            rightMotor = leftMotor;
-            rightDirection=leftDirection;
+            // right
+            rightController.setProcessValue(rightVelocity);
+            rightPwmDuty = rightController.compute();
+            if (rightPwmDuty < 0){
+                rightDirection = 0;
+                rightMotor = -rightPwmDuty;
+            }
+            else {
+                rightDirection = 1;
+                rightMotor = rightPwmDuty;
+            }
             
-            // User feadback
+            // User feedback
             scope.set(0, request);
-            scope.set(1, leftPwmDuty);
-            scope.set(2, leftVelocity);
+            scope.set(1, rightPwmDuty);
+            scope.set(2, rightVelocity);
             scope.send();
             pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
         
@@ -68,8 +87,6 @@
 
     //Stop motors.
     leftMotor  = 0;
+    rightMotor = 0;
     
-    //Close results file.
-    //fclose(fp);
-
 }