Diff: MotionTest.cpp

Revision:

1:d523415c7b53

Parent:

0:4c63f2192998

Child:

2:0f93881225d2

Child:

3:9cbf571137a5

--- a/MotionTest.cpp	Fri Mar 11 17:10:59 2016 +0000
+++ b/MotionTest.cpp	Fri Mar 18 14:36:50 2016 +0000
@@ -19,6 +19,8 @@
     int SaturateAdd(int x, int y);
     float SaturateLimit(float x, float limit);
     signed int SignExtend(signed int x);
+    char GUI();
+    void Settings(char x);
 
 // ********************************************************************
 //                     GLOBAL VARIABLE DECLARATIONS
@@ -40,16 +42,18 @@
     signed int dPositionRight;
     int dTimeLeft;                      // DE0 Register 1
     int dTimeRight;
+    float Circ = 0;
+    float tTot = 0;
     
     //Motion Primitive Variables
     signed int Arc;
-    float Radius;
-    float VelDes;
-    float tcalc;
-    int tcum;
+    float Radius = 0;
+    float VelDes = 0;
+    float tcalc = 0;
+
     float O_setpoint;                     //Inner Wheel Speed
     float I_setpoint;                     //Outer Wheel Speed
-    float L; //Distance between wheels (in meters)
+    float L = 5.5*2.54/100; //Distance between wheel contacts  ; //Distance between wheels (in meters)
     float pi = 3.141259;
 // *********************************************************************
 //                     PROCESSES AND THREADS
@@ -169,65 +173,6 @@
     
 }
     
-/// ***************************************************
-//                 User Input Left
-// ***************************************************
-
-signed int InputLeft(void){
-   
-   signed int input;
-   
-   Bluetooth.printf("\n\r Please enter a desired angular speed for the left motor (rads/sec) >> ");
-   Bluetooth.scanf("%i",&input);
-   
-   Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input);
-   
-   return input;
-
-}
-
-signed int InputLeft_m(void){
-   
-   signed int input;
-   
-   Bluetooth.printf("\n\r Please enter a desired angular speed for the left motor (m/sec) >> ");
-   Bluetooth.scanf("%i",&input);
-   
-   Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input);
-   
-   return input;
-
-}
-
-/// ***************************************************
-//                 User Input Right
-// ***************************************************
-
-signed int InputRight(void){
-      
-   signed int input;
-   
-   Bluetooth.printf("\n\r Please enter a desired angular speed for the right motor (rads/sec) >> ");
-   Bluetooth.scanf("%i",&input);
-   
-   Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input);
-   
-   return input;
-
-}
-
-signed int InputRight_m(void){
-      
-   signed int input;
-   
-   Bluetooth.printf("\n\r Please enter a desired angular speed for the right motor (m/sec) >> ");
-   Bluetooth.scanf("%i",&input);
-   
-   Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input);
-   
-   return input;
-
-}
 
 /// ***************************************************
 //                 Control Thread
@@ -236,7 +181,6 @@
 void ControlThread(void){
 
     // Read Incremental Position from DE0 QEI
-
     int dummy = 0x0000;                                // Pushes dummy information which DE0 ignores, store return from QEI register
     
     dPositionLeft = SignExtend(DE0.write(dummy));
@@ -265,7 +209,7 @@
     // 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
+    //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){
@@ -344,7 +288,63 @@
         return x;
 }
 
+char GUI(){
+    char x;
+    pc.printf("\n\r (C)ircle or (L)ine? >>");
+    pc.scanf("%c", &x);
+    return x;
+    }
 
+void Settings(char x){
+    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);
+        float Circ = 2*pi*Radius;
+        float tTot = Circ/VelDes;
+        if(z == 'l' || z == 'L')
+        {
+            I_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot);    
+            if(I_setpoint>35)
+            {
+                I_setpoint = 35;
+                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);
+        }
+        else
+        {
+            O_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot);
+            if(O_setpoint>35)
+            {
+                O_setpoint = 35;
+                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);    
+        }
+    }
+    else
+    {
+        pc.printf("\n\r Desired Speed? (m) >>");
+        pc.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);
+        }
+        I_setpoint = O_setpoint;
+    }
+    
+    
+    return;
+    }
 
 // ==============================================================================================================
 // ==============================================================================================================
@@ -356,7 +356,7 @@
 
 int main(){
    
-   // Initialization
+   // Initialization 
 
    DE0_Init(0x8004);                                                
    L_MotorInit();
@@ -367,56 +367,11 @@
    
    //Motion Primitives Initialization
    Arc = 0;    // Arc Length (start at 0) 
-   L = 0.14605; //Distance between wheel contacts  
-   
-   //IF RADIUS IS TOO LOW OR VELOCITY TOO HIGH THINGS GO CRAZY 
-   Radius = 0.2; // Minimum L/2;
-   VelDes = 1.7;
-   tcum = 0;
-   float circum = 2*pi*Radius; //Arc circumference
+   Settings(GUI()); 
    
-   tcalc = circum/VelDes;      // The easiest way to get the right speeds is to determine the theoretical completion time
-   // Inner and Outer Wheel Setpioints
-   O_setpoint = ((Radius+(L/2))*2*pi)/(2*2.54*tcalc/100); //0.02*2.54*tcalc is for rads/s
-   I_setpoint = ((Radius-(L/2))*2*pi)/(2*2.54*tcalc/100); 
-   pc.printf("Out: %2.2f   In: %2.2f \n\r", O_setpoint,I_setpoint);
-   
-   // Specify Setpoint ( rads/sec )
-    
-   //L_setpoint = InputLeft();
-   //R_setpoint = InputRight();
-   
-   // Specify Setpoint ( m/sec )
-   /* 
-   L_setpoint = InputLeft_m();
-   R_setpoint = InputRight_m();*/
-        
    // Display Global Variables to Console
    while(1){
-       
-       float L_error_t = L_e;
-       float L_error_m = L_e*0.05093;                       //Multiply by 0.05093 for m/s
-       float L_u_t = L_u;     
-       
-       float R_error_t = R_e;                       //Multiply by 0.05093 for m/s
-       float R_u_t = R_u;
-       float R_error_m = R_e*0.05093;                       //Multiply by 0.05093 for m/s
-            
-       float L_setpoint_m = L_setpoint*0.05093;               //Setpoints in float values for display
-       float R_setpoint_m = R_setpoint*0.05093;               //Setpoints in float values for display
-
-       pc.printf("Radius: %2.2f m Velocity: %2.2f rad/s\n\r", Radius,VelDes);
-       if (O_setpoint > 35)
-       {
-       pc.printf("L_Set:   MAX  R_Set:   MAX \n\r");
-       O_setpoint = 35;
-       I_setpoint = O_setpoint*(Radius-L/2)/(Radius+L/2);
-       }
-       else
-       {    
-       pc.printf("L_Set:   %2.2f  R_Set:   %2.2f \n\r",O_setpoint,I_setpoint);           
-       }
+       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);
-   }
-   
+   } 
 }