Diff: MotionTest.cpp

Revision:

4:1bcb35cef400

Parent:

3:9cbf571137a5

Child:

5:ac5819613d0c

--- a/MotionTest.cpp	Fri Mar 18 17:38:36 2016 +0000
+++ b/MotionTest.cpp	Sat Mar 19 19:32:53 2016 +0000
@@ -44,10 +44,10 @@
     int dTimeRight;
     float Circ = 0;
     float tTot = 0;
-    float AngularSpeedLeftFloat;
-    float AngularSpeedRightFloat;
-    signed int AngularSpeedLeft;
-    signed int AngularSpeedRight;
+    float AngularSpeedLeft;
+    float AngularSpeedRight;
+    //signed int AngularSpeedLeft;
+    //signed int AngularSpeedRight;
     
     //Motion Primitive Variables
     signed int Arc;
@@ -206,15 +206,7 @@
     
     float Kp_R = 2.5;
     float Ki_R = 0.010;
-    
-    //          **** MODIFICATIONS FOR MOTION PRIMITIVE ARE HERE ****
-    // In order to determine theta relative to any given position, we can just use Theta = Arc Length/Radius
-    // Radius R is determined by the user
-    // Arc Length is determined from an integration of average velocity dP/dt, which is tracked by the DE0 board
-    
-    //Arc Length (meters) variable - We have speed and time outputs already so we can use those values to calculate average arc of V
-    //Arc = (AngularSpeedLeft*dTimeLeft+AngularSpeedRight*dTimeRight)*0.05093/2; //constant of 0.05093 derived in another function using wheel radii
-      
+       
     // Saturate Inegrators if necessary
     if(abs(SaturateLimit((Kp_L*L_e+Ki_L*L_integrator)/35,1))<1){
         L_integrator = L_integrator +L_e;}
@@ -245,7 +237,7 @@
     PwmR.write(abs(R_u));
 }
 
-/// ***************************************************
+/*// *************************************************** NOT USED
 //                SaturateAdd
 // ***************************************************
 
@@ -260,7 +252,7 @@
         z = 0x80000000;}
         
     return z;
-}
+}*/
     
 /// ***************************************************
 //              SaturateLimit
@@ -292,6 +284,10 @@
         return x;
 }
 
+// ***************************************************
+//                   Startup GUI
+// ***************************************************
+
 char GUI(){
     char x;
     Bluetooth.printf("\n\r (C)ircle or (L)ine? >>");
@@ -299,6 +295,11 @@
     return x;
     }
 
+// ***************************************************
+//                   Setpoint Collection
+// ***************************************************
+
+
 void Settings(char x){
     char z;
     if(x == 'c' || x == 'C')
@@ -348,10 +349,7 @@
     
     
     return;
-    }
-
-// ==============================================================================================================
-// ==============================================================================================================
+}
 
 
 // *********************************************************************
@@ -370,16 +368,14 @@
    ControlInterrupt.attach(&ControlThread, 0.0005);
    
    //Motion Primitives Initialization
-   Arc = 0;    // Arc Length (start at 0) 
-   Settings(GUI()); 
-   
+   Arc = 0;                                 // Arc Length (start at 0) 
+   Settings(GUI());                         // Ask user for input and pass to Settings
+   float WAvg,VAvg;
    // Display Global Variables to Console
    while(1){
-       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);
+       WAvg = (AngularSpeedLeft + AngularSpeedRight);
+       VAvg = (AngularSpeedLeft + AngularSpeedRight)*0.02483*2;
+       Bluetooth.printf("\n\r||SetLeft: %2.3f ||WLeft: %2.3f rad/s ||SetRight: %2.3f ||WRight: %2.3f rad/s||WAverage: %2.3f||VAverage: %2.3f",O_setpoint,AngularSpeedLeft,I_setpoint,AngularSpeedRight,WAvg,VAvg);
        wait(0.75);
    } 
 }