Diff: main.cpp

Revision:

11:66d0be7efd3f

Parent:

10:4b0b4f2abacf

Child:

12:02eba9a294d2

--- a/main.cpp	Wed Nov 01 14:39:59 2017 +0000
+++ b/main.cpp	Wed Nov 01 15:30:59 2017 +0000
@@ -44,14 +44,14 @@
 const float     RAD_PER_PULSE = (2*pi)/4200;
 const float     CONTROLLER_TS = 0.01;     //TIME INTERVAL/ hZ
 
-const float     M1_KP = 20; 
+const float     M1_KP = 10; 
 const float     M1_KI = 0.5;
 const float     M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
 double          m1_err_int = 0;
 double          m1_prev_err = 0;
 
-const float     M2_KP = 50; 
-const float     M2_KI = 1.5;
+const float     M2_KP = 10; 
+const float     M2_KI = 0.5;
 const float     M2_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
 double          m2_err_int = 0;
 double          m2_prev_err = 0;
@@ -132,19 +132,19 @@
     double dRadius  = reference_motor2 - pos_M2;
 
     pc.baud(115200);
-    //pc.printf("\r DesPosition(X,Y):(%f,%f), pos Error(dTheta, dError):(%f,%f)\n",x,y, dTheta ,dRadius);
+    pc.printf("\r DesPosition(X,Y):(%f,%f), pos Error(dTheta, dError):(%f,%f)\n",x,y, dTheta ,dRadius);
     
     //motor1PWM = motor1;
     //motor2PWM = motor2;
 
-    if(delta1 > 10.0){
+    if(delta1 > 0.5){
         motor1DC = 0;
         
         ledr = 1;
         ledg = 1;       //Blau
         ledb = 0;
     }
-    else if (delta1< -10.0) {
+    else if (delta1< -0.5) {
         motor1DC = 1; 
         
         ledb = 1;
@@ -161,20 +161,20 @@
     }
     
     motor1 = abs(delta1)/1000.0;
-    //if(motor1 >= 0.50f) {
-      //  motor1 = 0.50f;
+        if(motor1 >= 0.50) {
+        motor1 = 0.50;
       //pc.baud(115200);
       //pc.printf("\r val motor1: %f\n", motor1);
-   // }
+    }
 
-if(delta2 > 0.1){
+if(delta2 > 2.0){
         motor2DC = 0;
         
         ledr = 1;
         ledg = 1;       //Blau
         ledb = 0;
     }
-    else if (delta2<-0.1) {
+    else if (delta2<-2.0) {
         motor2DC = 1; 
         
         ledb = 1;
@@ -191,14 +191,15 @@
     }
     
     motor2 = abs(delta2)/1000.0;
-    //if(motor2 >= 0.50f) {
-    //    motor2 = 0.50f;
-   // }
+    if(motor2 >= 0.50) {
+        motor2 = 0.50;
+    }
     
-    motor1PWM = motor1*5; // + 0.50f;
-    motor2PWM = motor2*1; //+ 0.50f;
+    motor1PWM = motor1 + 0.20;
+    motor2PWM = motor2 + 0.20;
     
-    pc.printf("\r motorvalues (M1,M2):(%f,%f), error:( \n",  motor1, motor2);
+    //pc.printf("\r delta(1,2):(%f,%f)\n", delta1,delta2);
+    //pc.printf("\r motorvalues (M1,M2):(%f,%f),\n",  motor1, motor2);
     //pc.printf("\r 
 }