Diff: MotionTest.cpp

Revision:

3:9cbf571137a5

Parent:

1:d523415c7b53

Child:

4:1bcb35cef400

--- a/MotionTest.cpp	Fri Mar 18 14:36:50 2016 +0000
+++ b/MotionTest.cpp	Fri Mar 18 17:38:36 2016 +0000
@@ -44,6 +44,10 @@
     int dTimeRight;
     float Circ = 0;
     float tTot = 0;
+    float AngularSpeedLeftFloat;
+    float AngularSpeedRightFloat;
+    signed int AngularSpeedLeft;
+    signed int AngularSpeedRight;
     
     //Motion Primitive Variables
     signed int Arc;
@@ -190,8 +194,8 @@
     
     // Computer Angular Speed and Angular Speed Error
     
-    signed int AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft;
-    signed int AngularSpeedRight = (123*dPositionRight)/dTimeRight;
+    AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft;
+    AngularSpeedRight = (123*dPositionRight)/dTimeRight;
     
     //CHANGED FOR OUTER AND INNER WHEELS
     L_e = O_setpoint - AngularSpeedLeft;
@@ -290,8 +294,8 @@
 
 char GUI(){
     char x;
-    pc.printf("\n\r (C)ircle or (L)ine? >>");
-    pc.scanf("%c", &x);
+    Bluetooth.printf("\n\r (C)ircle or (L)ine? >>");
+    Bluetooth.scanf("%c", &x);
     return x;
     }
 
@@ -299,12 +303,12 @@
     char z;
     if(x == 'c' || x == 'C')
     {
-        pc.printf("\n\rCircle Radius? (m)");
-        pc.scanf("%f", &Radius);
-        pc.printf("\n\rDesired Speed? (m)");
-        pc.scanf("%f", &VelDes);
-        pc.printf("\n\r Desired Direction? (L) or (R)");
-        pc.scanf("%c",&z);
+        Bluetooth.printf("\n\rCircle Radius? (m)");
+        Bluetooth.scanf("%f", &Radius);
+        Bluetooth.printf("\n\rDesired Speed? (m)");
+        Bluetooth.scanf("%f", &VelDes);
+        Bluetooth.printf("\n\r Desired Direction? (L) or (R)");
+        Bluetooth.scanf("%c",&z);
         float Circ = 2*pi*Radius;
         float tTot = Circ/VelDes;
         if(z == 'l' || z == 'L')
@@ -313,7 +317,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);
+                Bluetooth.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 +327,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);
+                Bluetooth.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);    
@@ -331,13 +335,13 @@
     }
     else
     {
-        pc.printf("\n\r Desired Speed? (m) >>");
-        pc.scanf("%f", &VelDes);
+        Bluetooth.printf("\n\r Desired Speed? (m) >>");
+        Bluetooth.scanf("%f", &VelDes);
         O_setpoint = VelDes/0.05094;
         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);
+            Bluetooth.printf("\n\r Speed maximum exceeded. Capping outer wheel speed at %f m/s", O_setpoint*0.05094);
         }
         I_setpoint = O_setpoint;
     }
@@ -371,7 +375,11 @@
    
    // 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);
+       signed int SpeedLeft = AngularSpeedLeft;
+       signed int SpeedRight = AngularSpeedRight;
+       float Avg = (SpeedLeft + SpeedRight)*0.02483/2;
+       
+       Bluetooth.printf("\n\r||SetLeft: %2.2f ||WLeft: %d rad/s ||SetRight: %2.2f ||WRight: %d rad/s||WAverage: %d||VAverage: %2.2f",O_setpoint,SpeedLeft,I_setpoint,SpeedRight,(SpeedLeft + SpeedRight)/2,Avg);
        wait(0.75);
    } 
 }