Diff: Lab3.cpp

Revision:

3:30244b9e5351

Parent:

2:82e4eac97f0a

--- a/Lab3.cpp	Fri Feb 19 21:40:48 2016 +0000
+++ b/Lab3.cpp	Sat Feb 20 15:20:03 2016 +0000
@@ -26,9 +26,14 @@
     signed int setpoint;                // Desired Angular Speed ( rad/sec )
     float e;                            // Velocity Error
     float u;                            // Control Signal
+    float e_r;                          // Velocity Error - Right Motor
+    float u_r;                          // Control Signal - Right Motor
     int L_integrator;                   // Left Integrator State
+    int R_integrator;                   // Right Integrator State
     signed int dPositionLeft;           // DE0 Register 0
     int dTimeLeft;                      // DE0 Register 1
+    signed int dPositionRight;          // DE0 Register 2
+    int dTimeRight;                     // DE0 Register 3
     
 // *********************************************************************
 //                     PROCESSES AND THREADS
@@ -171,17 +176,20 @@
 
 void ControlThread(void){
 
+// ***********  LEFT MOTOR CONTROL
+
     // 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));
     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;
-    
     e = setpoint - AngularSpeedLeft;
     
     float Kp = 2.5;
@@ -193,10 +201,25 @@
         L_integrator = L_integrator;
     }
     
+// ***********  RIGHT MOTOR CONTROL
     
     u = SaturateLimit( (Kp*e+Ki*L_integrator),1);
-
     PwmL.write(u);
+    
+    signed int AngularSpeedRight = (123*dPositionRight)/dTimeRight;
+    e_r = setpoint - AngularSpeedRight;
+    
+    float Kp_r = 1;
+    float Ki_r = 0;
+    
+    if(SaturateLimit((Kp_r*e_r+Ki_r*R_integrator)/35,1)<1){
+        R_integrator = R_integrator +e;}
+    else{
+        R_integrator = R_integrator;
+    }
+    
+    u_r = SaturateLimit( (Kp_r*e+Ki_r*R_integrator),1);
+    PwmR.write(u);
 }
 
 /// ***************************************************
@@ -260,9 +283,11 @@
    
    // Initialization
 
-   DE0_Init(0x8002);                                                            
+   DE0_Init(0x8004);                            //Initialize for 4 registers                                                      
    L_MotorInit();
    L_integrator = 0;
+   R_MotorInit();
+   R_integrator = 0;
    ControlInterrupt.attach(&ControlThread, 0.0005);
    
    
@@ -274,13 +299,13 @@
    
    while(1){
        
-       float error_t = e;
-       float u_t = u;
+       float error_t = e_r;
+       float u_t = u_r;
        
        printf("\n\r US : %i",setpoint);      // User defined setpoint
-       printf("\n\r VE : %2.2f",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 VE : %2.2f",error_t);             // Displays signed Velocity Error of the right motor
+       printf("\n\r IS : %i",R_integrator);  // Displays currently state of the right integrator
+       printf("\n\r CS : %2.4f",u_t);             // Displays control signal to right motor
        printf("\n\r\n\r");
        wait(0.75);
    }