Diff: Lab3.cpp

Revision:

4:43aef502bb73

Parent:

2:82e4eac97f0a

Child:

5:e33e69d4eecf

diff -r 82e4eac97f0a -r 43aef502bb73 Lab3.cpp
--- a/Lab3.cpp	Fri Feb 19 21:40:48 2016 +0000
+++ b/Lab3.cpp	Wed Feb 24 14:44:32 2016 +0000
@@ -23,12 +23,18 @@
 //                     GLOBAL VARIABLE DECLARATIONS
 // ********************************************************************
 
-    signed int setpoint;                // Desired Angular Speed ( rad/sec )
-    float e;                            // Velocity Error
-    float u;                            // Control Signal
+    signed int R_setpoint;                // Desired Angular Speed ( rad/sec )
+    signed int L_setpoint;
+    float R_e;                            // Velocity Error
+    float R_u;                            // Control Signal
+    float L_e;
+    float L_u;
     int L_integrator;                   // Left Integrator State
+    int R_integrator;
     signed int dPositionLeft;           // DE0 Register 0
+    signed int dPositionRight;
     int dTimeLeft;                      // DE0 Register 1
+    int dTimeRight;
     
 // *********************************************************************
 //                     PROCESSES AND THREADS
@@ -117,14 +123,14 @@
 
 void L_MotorInit(void){
 
-   DirL = 1;                                   // Defaults to 0.
+   DirL = 1;                                   // Defaults to 1
 
     // Direction bit logic output
     // 0 : Backwards ( Reverse ) 
     // 1 : Forwards  ( Advance )
 
     PwmL.period_us(100);
-    PwmL.write(0.7);
+    PwmL.write(0);
     
 }
     
@@ -137,7 +143,7 @@
     
 void R_MotorInit(void){
     
-   DirR = 1;                                   // Defaults to 0.
+   DirR = 0;                                   // Defaults to 0.
     
     // Direction bit logic output
     // 0 : Forwards  ( Advance ) 
@@ -177,26 +183,43 @@
     
     dPositionLeft = SignExtend(DE0.write(dummy));
     dTimeLeft = DE0.write(dummy);
+    dPositionRight = SignExtend(DE0.write(dummy));
+    dTimeRight = DE0.write(dummy);
     
     // Computer Angular Speed and Angular Speed Error
     
     signed int AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft;
+    signed int AngularSpeedRight = (123*dPositionRight)/dTimeRight;
     
-    e = setpoint - AngularSpeedLeft;
+    L_e = L_setpoint - AngularSpeedLeft;
+    R_e = R_setpoint - AngularSpeedRight;
+    
+    float Kp_L = 2.5;
+    float Ki_L = 0.010;
     
-    float Kp = 2.5;
-    float Ki = 0.010;
+    
+    float Kp_R = 2.5;
+    float Ki_R = 0.010;
     
-    if(SaturateLimit((Kp*e+Ki*L_integrator)/35,1)<1){
-        L_integrator = L_integrator +e;}
+    
+    if(SaturateLimit((Kp_L*L_e+Ki_L*L_integrator)/35,1)<1){
+        L_integrator = L_integrator +L_e;}
     else{
         L_integrator = L_integrator;
     }
     
+    if(SaturateLimit((Kp_R*R_e+Ki_R*R_integrator)/35,1)<1){
+        R_integrator = R_integrator +R_e;}
+    else{
+        R_integrator = R_integrator;
+    }
     
-    u = SaturateLimit( (Kp*e+Ki*L_integrator),1);
-
-    PwmL.write(u);
+    
+    L_u = SaturateLimit( (Kp_L*L_e+Ki_L*L_integrator),1);
+    R_u = SaturateLimit( (Kp_R*R_e+Ki_R*R_integrator),1);
+    
+    PwmL.write(L_u);
+    PwmR.write(R_u);
 }
 
 /// ***************************************************
@@ -260,27 +283,38 @@
    
    // Initialization
 
-   DE0_Init(0x8002);                                                            
+   DE0_Init(0x8004);                                                            
    L_MotorInit();
+   R_MotorInit();
    L_integrator = 0;
+   R_integrator = 0;
    ControlInterrupt.attach(&ControlThread, 0.0005);
    
-   
    // Specify Setpoint ( rads/sec )
 
-    setpoint = UserInput();
-   
+    L_setpoint = UserInput();
+    R_setpoint = UserInput();
+    
    // Display Global Variables to Console
    
    while(1){
        
-       float error_t = e;
-       float u_t = u;
+       float L_error_t = L_e;
+       float L_u_t = L_u;
+       
+       float R_error_t = R_e;
+       float R_u_t = R_u;
        
-       printf("\n\r US : %i",setpoint);      // User defined setpoint
-       printf("\n\r VE : %2.2f",error_t);             // Displays signed Velocity Error
+       printf("\n\r LEFT");
+       printf("\n\r US : %i",L_setpoint);      // User defined setpoint
+       printf("\n\r VE : %2.2f",L_error_t);    // Displays signed Velocity Error
        printf("\n\r IS : %i",L_integrator);  // Displays currently state of the left integrator
-       printf("\n\r CS : %2.4f",u_t);             // Displays control signal
+       printf("\n\r CS : %2.4f",L_u_t);        // Displays control signal
+       printf("\n\r RIGHT");
+       printf("\n\r US : %i",R_setpoint);      // User defined setpoint
+       printf("\n\r VE : %2.2f",R_error_t);    // Displays signed Velocity Error
+       printf("\n\r IS : %i",R_integrator);  // Displays currently state of the left integrator
+       printf("\n\r CS : %2.4f",R_u_t);        // Displays control signal
        printf("\n\r\n\r");
        wait(0.75);
    }