Diff: main.cpp

Revision:

11:03d979f1517f

Parent:

10:f60f9849980a

Child:

12:ff7947a79149

--- a/main.cpp	Wed Oct 26 15:27:21 2016 +0000
+++ b/main.cpp	Fri Oct 28 10:36:46 2016 +0000
@@ -24,8 +24,8 @@
 volatile float q2 = 0;
 volatile float q1_begin;
 volatile float q2_begin;
-volatile float l1 = 0.3626;
-volatile float l2 = 0.420;
+const float l1 = 0.3626;
+const float l2 = 0.420;
 volatile float q1_v;
 volatile float q2_v;
 volatile float q1_ref = 0;
@@ -49,7 +49,8 @@
 volatile bool translatie_richting = true;  //true is verticaal, false is horizontaal
 
 const float TS = 0.02;
-const float MotorGain = 8.4;       // bij pwm = 1 draait (losse) motor met 8.4 rad/s
+const float MotorGain_M1 = 4.3;         // bij pwm = 1 draait (losse) motor met 4.3 rad/s -> gemeten
+const float MotorGain_M2 = 4.7;         // gemeten
 
 Ticker update_encoder_ticker;
 volatile bool go_flag_update_encoder = false;
@@ -62,9 +63,9 @@
 {
     //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 \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);
+    //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);
 }
 
 volatile bool go_flag_initialize = false;
@@ -97,15 +98,17 @@
 {
     dir_M1 = 0; //ccw
     dir_M2 = 1; //cw
-    while (q1 < 20*2*3.1415/360) {
+    while (q1 < 40*2*3.1415/360) {
         q1 = wheel_M1.getPulses()/(1334.355/2);
-        pwm_M1 = 0.01;
+        pwm_M1 = 0.05;
+        wait(0.005f);
     }
     pwm_M1 = 0;
 
-    while (q2 > -45*2*3.1415/360) {
+    while (q2 > -80*2*3.1415/360) {
         q2 = wheel_M2.getPulses()/(1334.355/2);
-        pwm_M2 = 0.01;
+        pwm_M2 = 0.05;
+        wait(0.005f);
     }
     pwm_M2 = 0;
     ledg = !ledg;
@@ -119,10 +122,10 @@
     q2IntError = 0;
     q1_error_prev = 0;
     q2_error_prev = 0;
-    if (translatie_richting == true) {      // verticaal / up
+    if (translatie_richting == true) {      // verticaal / up / blauw
         vx = 0;
         vy = 0.1;
-    } else {                        // horizontaal / right
+    } else {                        // horizontaal / right / rood
         vx = 0.1;
         vy = 0;
     }
@@ -134,10 +137,10 @@
     q2IntError = 0;
     q1_error_prev = 0;
     q2_error_prev = 0;
-    if (translatie_richting == true) {      // verticaal / down
+    if (translatie_richting == true) {      // verticaal / down / blauw
         vx = 0;
         vy = -0.1;
-    } else {                        // horizontaal / left
+    } else {                        // horizontaal / left / rood
         vx = -0.1;
         vy = 0;
     }
@@ -221,17 +224,17 @@
 
     q1IntError = q1IntError + q1_error*TS;             // integrated error in radians
     q2IntError = q2IntError + q2_error*TS;             // integrated error in radians
-    float Ki = 0.1;
+    float Ki = 1;
 
     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;
+    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
 
-    motorValue1Out = TotalError1/MotorGain;
-    motorValue2Out = TotalError2/MotorGain;
+    motorValue1Out = TotalError1/MotorGain_M1;
+    motorValue2Out = TotalError2/MotorGain_M2;
 
     q1_error_prev = q1_error;
     q2_error_prev = q2_error;
@@ -270,6 +273,9 @@
 
 int main()
 {
+    ledr = 1;
+    ledg = 1;
+    ledb = 0;
     pc.baud(115200);
     wheel_M1.reset();
     wheel_M2.reset();