Diff: MotionTest.cpp

Revision:

2:0f93881225d2

Parent:

1:d523415c7b53

--- a/MotionTest.cpp	Fri Mar 18 14:36:50 2016 +0000
+++ b/MotionTest.cpp	Fri Mar 18 16:08:50 2016 +0000
@@ -169,8 +169,7 @@
     // 1 : Backwards ( Reverse )
     
     PwmR.period_us(100);
-    PwmR.write(0);
-    
+    PwmR.write(0);   
 }
     
 
@@ -313,7 +312,7 @@
             if(I_setpoint>35)
             {
                 I_setpoint = 35;
-                printf("Setpoint limit exceeded. Capping outer wheel speed at %2.2f m/s", O_setpoint*0.05094);
+                pc.printf("Setpoint limit exceeded. Capping outer wheel speed at %2.2f m/s", O_setpoint*0.05094);
             }
             O_setpoint = I_setpoint*(Radius-L/2)/(Radius+L/2);
         }
@@ -323,7 +322,7 @@
             if(O_setpoint>35)
             {
                 O_setpoint = 35;
-                printf("Setpoint limit exceeded. Capping outer wheel speed at %2.2f m/s", O_setpoint*0.05094);
+                pc.printf("Setpoint limit exceeded. Capping outer wheel speed at %2.2f m/s", O_setpoint*0.05094);
                 
             }
             I_setpoint = (Radius-L/2)*2*pi/((2*2.54/100)*tTot);    
@@ -337,12 +336,10 @@
         if(O_setpoint>35)
         {
             O_setpoint = 35;
-            printf("\n\r Speed maximum exceeded. Capping outer wheel speed at %f m/s", O_setpoint*0.05094);
+            pc.printf("\n\r Speed maximum exceeded. Capping outer wheel speed at %f m/s", O_setpoint*0.05094);
         }
         I_setpoint = O_setpoint;
-    }
-    
-    
+    }    
     return;
     }
 
@@ -368,10 +365,15 @@
    //Motion Primitives Initialization
    Arc = 0;    // Arc Length (start at 0) 
    Settings(GUI()); 
+   float SpeedActual; 
+   unsigned int VLeft,VRight;
    
    // Display Global Variables to Console
-   while(1){
-       pc.printf("\n\r||Radius: %2.2f m||VLeft: %2.2f m/s ||VRight: %2.2f m/s ||Average: %2.2f m/s",Radius, O_setpoint*0.05094,I_setpoint*0.05094,VelDes);
-       wait(0.75);
+   while(1){ 
+       VLeft = 123*(dPositionLeft/dTimeLeft);
+       VRight = 123*(dPositionRight/dTimeRight);
+       SpeedActual = (VLeft+VRight)/2;
+       pc.printf("\n\r||VLeft: %2.2f ||VRight: %2.2f ||Radius: %2.2f",VLeft,VRight,SpeedActual);
+       wait(1);
    } 
 }