Diff: Lab3.cpp

Revision:

5:e33e69d4eecf

Parent:

4:43aef502bb73

Child:

6:1d16cc833e0d

diff -r 43aef502bb73 -r e33e69d4eecf Lab3.cpp
--- a/Lab3.cpp	Wed Feb 24 14:44:32 2016 +0000
+++ b/Lab3.cpp	Thu Feb 25 17:13:30 2016 +0000
@@ -13,7 +13,8 @@
     void DE0_Init(int);
     void L_MotorInit(void);
     void R_MotorInit(void);
-    signed int UserInput(void);
+    signed int InputLeft(void);
+    signed int InputRight(void);
     void ControlThread(void);
     int SaturateAdd(int x, int y);
     float SaturateLimit(float x, float limit);
@@ -23,8 +24,8 @@
 //                     GLOBAL VARIABLE DECLARATIONS
 // ********************************************************************
 
-    signed int R_setpoint;                // Desired Angular Speed ( rad/sec )
-    signed int L_setpoint;
+    signed int R_setpoint=0;                // Desired Angular Speed ( rad/sec )
+    signed int L_setpoint=0;
     float R_e;                            // Velocity Error
     float R_u;                            // Control Signal
     float L_e;
@@ -155,16 +156,34 @@
 }
     
 /// ***************************************************
-//                 User Input
+//                 User Input Left
 // ***************************************************
 
-signed int UserInput(void){
+signed int InputLeft(void){
    
    signed int input;
    
-   printf("\n\r Please enter a desired angular speed (rad/sec) >> ");
+   printf("\n\r Please enter a desired angular speed for the left motor (rad/sec) >> ");
    scanf("%i",&input);
-   printf("\n\r Your number was >> %i\n\r",input);
+   
+   printf("\n\r Your setpoint is >> %i\n\r",input);
+   
+   return input;
+
+}
+
+/// ***************************************************
+//                 User Input Right
+// ***************************************************
+
+signed int InputRight(void){
+      
+   signed int input;
+   
+   printf("\n\r Please enter a desired angular speed for the right motor (rad/sec) >> ");
+   scanf("%i",&input);
+   
+   printf("\n\r Your setpoint is >> %i\n\r",input);
    
    return input;
 
@@ -197,29 +216,37 @@
     float Kp_L = 2.5;
     float Ki_L = 0.010;
     
-    
     float Kp_R = 2.5;
     float Ki_R = 0.010;
     
     
-    if(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){
         L_integrator = L_integrator +L_e;}
     else{
         L_integrator = L_integrator;
     }
     
-    if(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){
         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);
     
-    PwmL.write(L_u);
-    PwmR.write(R_u);
+    if(L_u <=0)
+        DirL = 0;
+    else
+        DirL = 1;
+    
+    if(R_u <=0)
+        DirR = 1;
+    else
+        DirR = 0;
+        
+    PwmL.write(abs(L_u));
+    PwmR.write(abs(R_u));
 }
 
 /// ***************************************************
@@ -292,8 +319,8 @@
    
    // Specify Setpoint ( rads/sec )
 
-    L_setpoint = UserInput();
-    R_setpoint = UserInput();
+    L_setpoint = InputLeft();
+    R_setpoint = InputRight();
     
    // Display Global Variables to Console