Diff: main.cpp

Revision:

10:f60f9849980a

Parent:

9:334b1596637b

Child:

11:03d979f1517f

--- a/main.cpp	Wed Oct 26 14:00:22 2016 +0000
+++ b/main.cpp	Wed Oct 26 15:27:21 2016 +0000
@@ -28,20 +28,20 @@
 volatile float l2 = 0.420;
 volatile float q1_v;
 volatile float q2_v;
- float q1_ref = 0;
- float q2_ref = 0;
- float q1_error = 0;
- float q2_error = 0;
- float q1_error_prev = 0;
- float q2_error_prev = 0;
- float q1DerivativeError = 0;
- float q2DerivativeError = 0;
- float q1IntError = 0;
- float q2IntError = 0;
- float TotalError1= 0;
- float TotalError2= 0;
- float motorValue1Out = 0.0;
- float motorValue2Out = 0.0;
+volatile float q1_ref = 0;
+volatile float q2_ref = 0;
+volatile float q1_error = 0;
+volatile float q2_error = 0;
+volatile float q1_error_prev = 0;
+volatile float q2_error_prev = 0;
+volatile float q1DerivativeError = 0;
+volatile float q2DerivativeError = 0;
+volatile float q1IntError = 0;
+volatile float q2IntError = 0;
+volatile float TotalError1= 0;
+volatile float TotalError2= 0;
+float motorValue1Out = 0.0;
+float motorValue2Out = 0.0;
 volatile float ctrlOutput_M1 = 0;
 volatile float ctrlOutput_M2 = 0;
 volatile float vx;
@@ -60,10 +60,11 @@
 
 void update_encoder()
 {
-    q1 = wheel_M1.getPulses()/(1334.355/2);
-    q2 = wheel_M2.getPulses()/(1334.355/2);
+    //q1 = wheel_M1.getPulses()/(1334.355/2);
+    //q2 = wheel_M2.getPulses()/(1334.355/2);
     //pc.printf("q1 = %f \tq1_ref = %f \tPID1 = %f \tq2 = %f \tq2_ref = %f \tPID2 = %f \ttotalerror1 = %f \ttotalerror2 = %f\n\r",q1, q1_ref, ctrlOutput_M1,q2,q2_ref,ctrlOutput_M2,TotalError1,TotalError2);
-    pc.printf("vx = %f \tvy = %f \tq1_v = %f \tq2_v = %f\n\r",vx,vy,q1_v,q2_v);
+    //pc.printf("vx = %f \tvy = %f \tq1_v = %f \tq2_v = %f \tq1 = %f \tq2 = %f \tpwm_M1 = %f \tpwm_M2 = %f\n\r",vx,vy,q1_v,q2_v,q1,q2,pwm_M1.read(),pwm_M2.read());
+    pc.printf("q1_err = %0.9f \tq2_err = %0.9f \tq1IntErr = %0.9f \tq2IntErr = %0.9f \tTotErr1 = %0.9f \tTotErr2 = %0.9f\n\r",q1_error,q2_error,q1IntError,q2IntError,TotalError1,TotalError2);
 }
 
 volatile bool go_flag_initialize = false;
@@ -86,8 +87,8 @@
 void begin_hoeken()
 {
     wait(3);
-    float q1_ref = wheel_M1.getPulses()/(1334.355/2);
-    float q2_ref = wheel_M2.getPulses()/(1334.355/2);
+    q1_ref = wheel_M1.getPulses()/(1334.355/2);
+    q2_ref = wheel_M2.getPulses()/(1334.355/2);
     active_PID_ticker = true;
 }
 
@@ -114,22 +115,30 @@
 
 void biceps()
 {
-    if (translatie_richting) {      // verticaal / up
+    q1IntError = 0;
+    q2IntError = 0;
+    q1_error_prev = 0;
+    q2_error_prev = 0;
+    if (translatie_richting == true) {      // verticaal / up
         vx = 0;
-        vy = 0.02;
+        vy = 0.1;
     } else {                        // horizontaal / right
-        vx = 0.02;
+        vx = 0.1;
         vy = 0;
     }
 }
 
 void triceps()
 {
-    if (translatie_richting) {      // verticaal / down
+    q1IntError = 0;
+    q2IntError = 0;
+    q1_error_prev = 0;
+    q2_error_prev = 0;
+    if (translatie_richting == true) {      // verticaal / down
         vx = 0;
-        vy = -0.02;
+        vy = -0.1;
     } else {                        // horizontaal / left
-        vx = -0.02;
+        vx = -0.1;
         vy = 0;
     }
 
@@ -137,11 +146,17 @@
 
 void switcher()
 {
-    if ( (vx == 0) && (vy == 0) ) {
-        translatie_richting = !translatie_richting;
+    if ( (vx == 0) && (vy == 0) && (translatie_richting == true) ) {
+        translatie_richting = false;
+    } else if ( (vx == 0) && (vy == 0) && (translatie_richting == false) ) {
+        translatie_richting = true;
     } else {
         vx = 0;
         vy = 0;
+        q1IntError = 0;
+        q2IntError = 0;
+        q1_error_prev = 0;
+        q2_error_prev = 0;
     }
 
     if (translatie_richting == 1) {
@@ -192,7 +207,7 @@
         q1_v = 0;
         q2_v = 0;
     }
-    
+
     q1_ref = q1 + q1_v*TS;
     q2_ref = q2 + q2_v*TS;
 }
@@ -200,37 +215,24 @@
 void PID(float q1,float q1_ref,float q2,float q2_ref,float TS,float &motorValue1Out, float &motorValue2Out)
 {
     // linear feedback control
-    float q1_error = q1_ref - q1; //referencePosition1 - Position1;             // proportional angular error in radians
-    float q2_error = q2_ref - q2; //referencePosition1 - Position1;             // proportional angular error in radians
-    float Kp = 1;
+    q1_error = q1_ref - q1; //referencePosition1 - Position1;             // proportional angular error in radians
+    q2_error = q2_ref - q2; //referencePosition1 - Position1;             // proportional angular error in radians
+    float Kp = 10;
 
-    float q1IntError = q1IntError + q1_error*TS;             // integrated error in radians
-    float q2IntError = q2IntError + q2_error*TS;             // integrated error in radians
+    q1IntError = q1IntError + q1_error*TS;             // integrated error in radians
+    q2IntError = q2IntError + q2_error*TS;             // integrated error in radians
     float Ki = 0.1;
-    
-    float q1DerivativeError = (q1_error - q1_error_prev)/TS;  // derivative of error in radians
-    float q2DerivativeError = (q2_error - q2_error_prev)/TS;  // derivative of error in radians
+
+    q1DerivativeError = (q1_error - q1_error_prev)/TS;  // derivative of error in radians
+    q2DerivativeError = (q2_error - q2_error_prev)/TS;  // derivative of error in radians
     float Kd = 0.0;
-    
-    TotalError1 = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd;         //total controller output = motor input
-    TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd;         //total controller output = motor input
-    
-    if (TotalError1 < 0) {
-        TotalError1=0;
-        }
-    else {
-        TotalError1=TotalError1;
-        }
-    if (TotalError2 < 0) {
-        TotalError2=0;
-        }
-    else {
-        TotalError2=TotalError2;
-        }
-    
-    motorValue1Out = TotalError1/MotorGain;       
+
+    TotalError1 = (q1_error * Kp) + (q1IntError * Ki) + (q1DerivativeError * Kd);         //total controller output = motor input
+    TotalError2 = (q2_error * Kp) + (q2IntError * Ki) + (q2DerivativeError * Kd);         //total controller output = motor input
+
+    motorValue1Out = TotalError1/MotorGain;
     motorValue2Out = TotalError2/MotorGain;
-    
+
     q1_error_prev = q1_error;
     q2_error_prev = q2_error;
 }
@@ -240,7 +242,7 @@
     PID(q1,q1_ref,q2,q2_ref,TS,motorValue1Out,motorValue2Out);
     ctrlOutput_M1 = motorValue1Out;
     ctrlOutput_M2 = motorValue2Out;
-    
+
     if (ctrlOutput_M1 < 0) {
         dir_M1 = 1;
     } else {
@@ -274,19 +276,19 @@
     knop_biceps.rise(&biceps);
     knop_triceps.rise(&triceps);
     knop_switch.rise(&switcher);
-    
+
     // flag functions/tickers
     update_encoder_ticker.attach(&flag_update_encoder, TS);
     update_ref_ticker.attach(&flag_update_ref, TS);
     // initialize -> beginposities
     initialize();
-    
+
     if (active_PID_ticker == true) {
-    PIDcontrol.attach(&flag_controller, TS);
+        PIDcontrol.attach(&flag_controller, TS);
     }
-    
+
     while(1) {
-        
+
         // update encoder
         if (go_flag_update_encoder == true) {
             go_flag_update_encoder = false;