Diff: MotionTest.cpp

Revision:

5:ac5819613d0c

Parent:

4:1bcb35cef400

--- a/MotionTest.cpp	Sat Mar 19 19:32:53 2016 +0000
+++ b/MotionTest.cpp	Sat Mar 19 19:50:53 2016 +0000
@@ -187,14 +187,14 @@
     // 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));
+    dPositionLeft = SignExtend(DE0.write(dummy));      //Extends the 16-bit signed ints into 32-bit containers
     dTimeLeft = DE0.write(dummy);
     dPositionRight = SignExtend(DE0.write(dummy));
     dTimeRight = DE0.write(dummy);
     
     // Computer Angular Speed and Angular Speed Error
     
-    AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft;
+    AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft;   //123 is the constant derived from the gear ratios and properties of the motor
     AngularSpeedRight = (123*dPositionRight)/dTimeRight;
     
     //CHANGED FOR OUTER AND INNER WHEELS
@@ -208,21 +208,22 @@
     float Ki_R = 0.010;
        
     // Saturate Inegrators if necessary
-    if(abs(SaturateLimit((Kp_L*L_e+Ki_L*L_integrator)/35,1))<1){
+    if(abs(SaturateLimit((Kp_L*L_e+Ki_L*L_integrator)/35,1))<1){    //Checks that we are not saturating the Left integrator before summing more error
         L_integrator = L_integrator +L_e;}
     else{
         L_integrator = L_integrator;
     }
     
-    if(abs(SaturateLimit((Kp_R*R_e+Ki_R*R_integrator)/35,1))<1){
+    if(abs(SaturateLimit((Kp_R*R_e+Ki_R*R_integrator)/35,1))<1){    //Checks that we are not saturating the Left integrator before summing more error
         R_integrator = R_integrator +R_e;}
     else{
         R_integrator = R_integrator;
     }
  
-    L_u = SaturateLimit( (Kp_L*L_e+Ki_L*L_integrator),1);
-    R_u = SaturateLimit( (Kp_R*R_e+Ki_R*R_integrator),1);
+    L_u = SaturateLimit( (Kp_L*L_e+Ki_L*L_integrator),1);           //Checks that we are not saturating the Left integrator before summing more error
+    R_u = SaturateLimit( (Kp_R*R_e+Ki_R*R_integrator),1);           //Checks that we are not saturating the Left integrator before summing more error
         
+    //Determine direction bits based on sign of required output signal
     if(L_u <=0)
         DirL = 0;
     else
@@ -232,7 +233,8 @@
         DirR = 1;
     else
         DirR = 0;
-        
+    
+    //Write final values to the PWM    
     PwmL.write(abs(L_u));
     PwmR.write(abs(R_u));
 }
@@ -258,7 +260,7 @@
 //              SaturateLimit
 // ***************************************************
 
-float SaturateLimit(float x, float limit){
+float SaturateLimit(float x, float limit){              //Ensures we don't ask for more than the PWM can give.
     
     if (x > limit){
         return limit;
@@ -290,9 +292,9 @@
 
 char GUI(){
     char x;
-    Bluetooth.printf("\n\r (C)ircle or (L)ine? >>");
-    Bluetooth.scanf("%c", &x);
-    return x;
+    Bluetooth.printf("\n\r (C)ircle or (L)ine? >>");        //Determine Trajectory Type
+    Bluetooth.scanf("%c", &x);                              
+    return x;                                               //Return trajectory type
     }
 
 // ***************************************************
@@ -301,30 +303,36 @@
 
 
 void Settings(char x){
-    char z;
-    if(x == 'c' || x == 'C')
+    char dir;                                               //Direction Variable
+    float dist;                                             //Distance Variable
+    if(x == 'c' || x == 'C')                                //If the user chooses a circle
     {
-        Bluetooth.printf("\n\rCircle Radius? (m)");
+        Bluetooth.printf("\n\rCircle Radius? (m)");         //Determine circle radius
         Bluetooth.scanf("%f", &Radius);
-        Bluetooth.printf("\n\rDesired Speed? (m)");
+        Bluetooth.printf("\n\rDesired Speed? (m)");         //Speed
         Bluetooth.scanf("%f", &VelDes);
-        Bluetooth.printf("\n\r Desired Direction? (L) or (R)");
-        Bluetooth.scanf("%c",&z);
+        Bluetooth.printf("\n\r Desired Direction? (L) or (R)"); //Direction of Travel
+        Bluetooth.scanf("%c",&dir);
+        Bluetooth.printf("\n\r Desired Distance? (m)");     //Determine maximum distance
+        Bluetooth.scanf("%c",&dist);
+        
         float Circ = 2*pi*Radius;
         float tTot = Circ/VelDes;
-        if(z == 'l' || z == 'L')
+        
+        if(dir == 'l' || dir == 'L')                        //If counterclockwise direction, the right wheel is faster
         {
-            I_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot);    
+            I_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot);    //Set right wheel speed
             if(I_setpoint>35)
             {
                 I_setpoint = 35;
                 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);
+                return
             }
-            O_setpoint = I_setpoint*(Radius-L/2)/(Radius+L/2);
         }
-        else
+        else                                                //Clockwise, the left wheel is faster
         {
-            O_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot);
+            O_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot); //Set Left wheel speed
             if(O_setpoint>35)
             {
                 O_setpoint = 35;
@@ -334,17 +342,20 @@
             I_setpoint = (Radius-L/2)*2*pi/((2*2.54/100)*tTot);    
         }
     }
-    else
+    else                                                //If the user chooses a line instead
     {
-        Bluetooth.printf("\n\r Desired Speed? (m) >>");
+        Bluetooth.printf("\n\r Desired Speed? (m) >>"); //We only need maximum speed and maximum distance
         Bluetooth.scanf("%f", &VelDes);
-        O_setpoint = VelDes/0.05094;
+        Bluetooth.printf("\n\r Desired Distance? (m)"); //Determine maximum distance
+        Bluetooth.scanf("%c",&dist);
+        O_setpoint = VelDes/0.05094;                    //Saturate if necessary
+        
         if(O_setpoint>35)
         {
             O_setpoint = 35;
             Bluetooth.printf("\n\r Speed maximum exceeded. Capping outer wheel speed at %f m/s", O_setpoint*0.05094);
         }
-        I_setpoint = O_setpoint;
+        I_setpoint = O_setpoint;                        //For a straight line both wheel speeds must be equal
     }
     
     
@@ -366,15 +377,17 @@
    L_integrator = 0;
    R_integrator = 0;
    ControlInterrupt.attach(&ControlThread, 0.0005);
+   float WAvg,VAvg;                         //Reported Speeds
    
    //Motion Primitives Initialization
    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){
-       WAvg = (AngularSpeedLeft + AngularSpeedRight);
-       VAvg = (AngularSpeedLeft + AngularSpeedRight)*0.02483*2;
+       WAvg = (AngularSpeedLeft + AngularSpeedRight);           //Angular Average
+       VAvg = (AngularSpeedLeft + AngularSpeedRight)*0.02483*2; //Velocity Average with unit conversion
        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);
    }