Diff: Lab3.cpp

Revision:

8:9609a50ea076

Parent:

7:241bde733699

--- a/Lab3.cpp	Sat Feb 27 18:36:43 2016 +0000
+++ b/Lab3.cpp	Mon Feb 29 14:30:29 2016 +0000
@@ -26,6 +26,10 @@
 
     signed int R_setpoint;                // Desired Angular Speed ( rad/sec )
     signed int L_setpoint;
+    
+    signed int R_setpoint_m;              // Desired Angular Speed ( m/sec )
+    signed int L_setpoint_m;
+    
     float R_e;                            // Velocity Error
     float R_u;                            // Control Signal
     float L_e;
@@ -163,6 +167,19 @@
    
    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);
    
@@ -180,6 +197,19 @@
       
    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);
    
@@ -189,7 +219,6 @@
 
 }
 
-
 /// ***************************************************
 //                 Control Thread
 // ***************************************************
@@ -318,10 +347,15 @@
    ControlInterrupt.attach(&ControlThread, 0.0005);
    
    // Specify Setpoint ( rads/sec )
-
-    L_setpoint = InputLeft()/0.05093;  //The 0.05093 converts from a m/s input to a rad/s control calculation
-    R_setpoint = InputRight()/0.05093; //The 0.05093 converts from a m/s input to a rad/s control calculation
-    
+   // 
+   L_setpoint = InputLeft();
+   R_setpoint = InputRight();
+   
+   // Specify Setpoint ( m/sec )
+   /* 
+   L_setpoint = InputLeft_m();
+   R_setpoint = InputRight_m();*/
+        
    // Display Global Variables to Console
    Bluetooth.printf("\n\r ========= LEFT =======       ========= RIGHT =======");
    Bluetooth.printf("\n\r    US    VE     IS    CS         US    VE    IS   CS "); 
@@ -329,15 +363,23 @@
    while(1){
        
        float L_error_t = L_e;
-       float L_u_t = L_u;
+       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;
+       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 meters per second float values for display
-       float R_setpoint_m = L_setpoint*0.05093; //Setpoints in meters per second float values for display
+       float L_setpoint = L_setpoint;               //Setpoints in float values for display
+       float R_setpoint = R_setpoint;               //Setpoints in float values for display
+       
+       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
 
-       Bluetooth.printf("\n\r   %2.2f   %2.1f    %i  %2.2f      %2.2f    %2.1f  %i  %2.2f",L_setpoint_m,L_error_t*0.05093,L_integrator,L_u_t,R_setpoint_m,R_error_t*0.05093,R_integrator,R_u_t);
+       // M/S Output 
+       //Bluetooth.printf("\n\r   %2.2f   %2.1f    %i  %2.2f      %2.2f    %2.1f  %i  %2.2f",L_setpoint_m,L_error_m,L_integrator,L_u_t,R_setpoint_m,R_error_m,R_integrator,R_u_t);
+       // Rad/S Output
+       Bluetooth.printf("\n\r   %2.2f   %2.1f    %i  %2.2f      %2.2f    %2.1f  %i  %2.2f",L_setpoint,L_error_t,L_integrator,L_u_t,R_setpoint,R_error_t,R_integrator,R_u_t);
        wait(0.75);
    }