Diff: main.cpp

Revision:

12:ff7947a79149

Parent:

11:03d979f1517f

Child:

13:fe2efec19eda

--- a/main.cpp	Fri Oct 28 10:36:46 2016 +0000
+++ b/main.cpp	Fri Oct 28 14:17:12 2016 +0000
@@ -30,6 +30,8 @@
 volatile float q2_v;
 volatile float q1_ref = 0;
 volatile float q2_ref = 0;
+volatile float q1_ref_prev = 0;
+volatile float q2_ref_prev = 0;
 volatile float q1_error = 0;
 volatile float q2_error = 0;
 volatile float q1_error_prev = 0;
@@ -38,8 +40,8 @@
 volatile float q2DerivativeError = 0;
 volatile float q1IntError = 0;
 volatile float q2IntError = 0;
-volatile float TotalError1= 0;
-volatile float TotalError2= 0;
+volatile float q1_total_error= 0;
+volatile float q2_total_error= 0;
 float motorValue1Out = 0.0;
 float motorValue2Out = 0.0;
 volatile float ctrlOutput_M1 = 0;
@@ -63,9 +65,9 @@
 {
     //q1 = wheel_M1.getPulses()/(1334.355/2);
     //q2 = wheel_M2.getPulses()/(1334.355/2);
-    //pc.printf("q1 = %f \tq1_ref = %f \tq2 = %f \tq2_ref = %f \ttotalerr1 = %f \ttotalerr2 = %f\n\r",q1, q1_ref,q2,q2_ref,TotalError1,TotalError2);
-    pc.printf("vx = %f \tvy = %f \tq1_r = %f \tq2_r = %f \tq1 = %f \tq2 = %f \tPID_M1 = %f \tPID_M2 = %f\n\r",vx,vy,q1_ref,q2_ref,q1,q2,ctrlOutput_M1,ctrlOutput_M2);
-    //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);
+    //pc.printf("q1 = %f \tq1_ref = %f \tq2 = %f \tq2_ref = %f \ttotalerr1 = %f \ttotalerr2 = %f\n\r",q1, q1_ref,q2,q2_ref,q1_total_error,q2_total_error);
+    //pc.printf("vx = %f \tvy = %f \tq1_v = %f \tq2_v = %f \tq1 = %f \tq2 = %f \tPID_M1 = %f \tPID_M2 = %f\n\r",vx,vy,q1_v,q2_v,q1,q2,ctrlOutput_M1,ctrlOutput_M2);
+    //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,q1_total_error,q2_total_error);
 }
 
 volatile bool go_flag_initialize = false;
@@ -105,19 +107,20 @@
     }
     pwm_M1 = 0;
 
-    while (q2 > -80*2*3.1415/360) {
+    while (q2 > -90*2*3.1415/360) {
         q2 = wheel_M2.getPulses()/(1334.355/2);
         pwm_M2 = 0.05;
         wait(0.005f);
     }
     pwm_M2 = 0;
-    ledg = !ledg;
     begin_hoeken();
 }
 
 
 void biceps()
 {
+    q1_ref_prev = 0;
+    q2_ref_prev = 0;
     q1IntError = 0;
     q2IntError = 0;
     q1_error_prev = 0;
@@ -133,6 +136,8 @@
 
 void triceps()
 {
+    q1_ref_prev = 0;
+    q2_ref_prev = 0;
     q1IntError = 0;
     q2IntError = 0;
     q1_error_prev = 0;
@@ -156,10 +161,17 @@
     } else {
         vx = 0;
         vy = 0;
+        q1_ref = q1;
+        q2_ref = q2;
+        q1_error = 0;
+        q2_error = 0;
         q1IntError = 0;
         q2IntError = 0;
         q1_error_prev = 0;
         q2_error_prev = 0;
+        q1_total_error = 0;
+        q1_total_error = 0;
+        pc.printf("\n\rSWITCH!!! \n\n\r");
     }
 
     if (translatie_richting == 1) {
@@ -197,22 +209,51 @@
     q1_v = J_1 * vx + J_2 * vy;
     q2_v = J_3 * vx + J_4 * vy;
 
+    //pc.printf("q1 = %f \tq2 = %f \tq1_v = %f \tq2_v = %f\n\r", q1,q2,q1_v,q2_v);
+
     if ( (q1 > (90*2*3.1415/360)) && (q1_v > 0 ) ) {                // WAARDES VINDEN 0.8726 (50 graden)
         q1_v = 0;
         q2_v = 0;
+        q1_ref_prev = 0;
+        q2_ref_prev = 0;
+        q1IntError = 0;
+        q2IntError = 0;
+        q1_error_prev = 0;
+        q2_error_prev = 0;
     } else if ( (q1 < -(90*2*3.1415/360)) && (q1_v < 0) ) {
         q1_v = 0;
         q2_v = 0;
+        q1_ref_prev = 0;
+        q2_ref_prev = 0;
+        q1IntError = 0;
+        q2IntError = 0;
+        q1_error_prev = 0;
+        q2_error_prev = 0;
     } else if ( (q2 < (-140*2*3.1415/360)) && (q2_v < 0) ) {        // WAARDES VINDEN -2.4434 (-140 graden) --> werkelijke max -2.672452
         q1_v = 0;
         q2_v = 0;
-    } else if ( (q2 > 0) && (q2_v > 0) ) {
+        q1_ref_prev = 0;
+        q2_ref_prev = 0;
+        q1IntError = 0;
+        q2IntError = 0;
+        q1_error_prev = 0;
+        q2_error_prev = 0;
+    } else if ( (q2 >= 0) && (q2_v > 0) ) {
         q1_v = 0;
         q2_v = 0;
+        q1_ref_prev = 0;
+        q2_ref_prev = 0;
+        q1IntError = 0;
+        q2IntError = 0;
+        q1_error_prev = 0;
+        q2_error_prev = 0;
     }
-
-    q1_ref = q1 + q1_v*TS;
-    q2_ref = q2 + q2_v*TS;
+    
+    q1_ref_prev = q1_ref;
+    q2_ref_prev = q2_ref;
+    
+    q1_ref = q1_ref_prev + q1_v*TS;
+    q2_ref = q2_ref_prev + q2_v*TS;
 }
 
 void PID(float q1,float q1_ref,float q2,float q2_ref,float TS,float &motorValue1Out, float &motorValue2Out)
@@ -230,12 +271,12 @@
     q2DerivativeError = (q2_error - q2_error_prev)/TS;  // derivative of error in radians
     float Kd = 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
+    q1_total_error = (q1_error * Kp) + (q1IntError * Ki) + (q1DerivativeError * Kd);         //total controller output = motor input
+    q2_total_error = (q2_error * Kp) + (q2IntError * Ki) + (q2DerivativeError * Kd);         //total controller output = motor input
 
-    motorValue1Out = TotalError1/MotorGain_M1;
-    motorValue2Out = TotalError2/MotorGain_M2;
-
+    motorValue1Out = q1_total_error/MotorGain_M1;
+    motorValue2Out = q2_total_error/MotorGain_M2;
+    
     q1_error_prev = q1_error;
     q2_error_prev = q2_error;
 }
@@ -246,6 +287,8 @@
     ctrlOutput_M1 = motorValue1Out;
     ctrlOutput_M2 = motorValue2Out;
 
+    pc.printf("q1_err = %0.9f \tq2_err = %0.9f \tq1IntErr = %0.9f \tq2IntErr = %0.9f  \tq1_ref = %0.9f \tq2_ref = %0.9f \tq1 = %f \tq2 = %f\n\r",q1_error,q2_error,q1IntError,q2IntError,q1_ref,q2_ref,q1,q2);
+
     if (ctrlOutput_M1 < 0) {
         dir_M1 = 1;
     } else {
@@ -282,12 +325,13 @@
     knop_biceps.rise(&biceps);
     knop_triceps.rise(&triceps);
     knop_switch.rise(&switcher);
-
+    
+    // initialize -> beginposities
+    initialize();
+    
     // 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);