Diff: main.cpp

Revision:

10:4b0b4f2abacf

Parent:

9:edf01d06935e

Child:

11:66d0be7efd3f

--- a/main.cpp	Wed Nov 01 10:00:10 2017 +0000
+++ b/main.cpp	Wed Nov 01 14:39:59 2017 +0000
@@ -28,10 +28,10 @@
 Ticker controller;
 
 // ---- Motor Constants-------
-float Pwmperiod = 0.0001f;
-int potmultiplier = 36000;    // Multiplier for the pot meter reference which is normally between 0 and 1
+float Pwmperiod = 0.001f;
+int potmultiplier = 600;    // Multiplier for the pot meter reference which is normally between 0 and 1
 float gainM1 = 1/35.17;     // encoder pulses per degree theta
-float gainM2 = 0.01;      // gain for radius r
+float gainM2 = 1/109.4;      // gain for radius r
 
 volatile float motor1;
 volatile float motor2;
@@ -50,8 +50,8 @@
 double          m1_err_int = 0;
 double          m1_prev_err = 0;
 
-const float     M2_KP = 10; 
-const float     M2_KI = 0.5;
+const float     M2_KP = 50; 
+const float     M2_KI = 1.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,7 +132,7 @@
     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;
@@ -167,14 +167,14 @@
       //pc.printf("\r val motor1: %f\n", motor1);
    // }
 
-if(delta2 > 10.0){
+if(delta2 > 0.1){
         motor2DC = 0;
         
         ledr = 1;
         ledg = 1;       //Blau
         ledb = 0;
     }
-    else if (delta2<-10.0) {
+    else if (delta2<-0.1) {
         motor2DC = 1; 
         
         ledb = 1;
@@ -195,18 +195,18 @@
     //    motor2 = 0.50f;
    // }
     
-    motor1PWM = motor1*2; // + 0.50f;
-    motor2PWM = motor2; //+ 0.50f;
+    motor1PWM = motor1*5; // + 0.50f;
+    motor2PWM = motor2*1; //+ 0.50f;
     
-    //pc.printf("\r motorvalues (M1,M2):(%f,%f), error:( \n",  motor1PWM, motor2PWM);
+    pc.printf("\r motorvalues (M1,M2):(%f,%f), error:( \n",  motor1, motor2);
     //pc.printf("\r 
 }
 
 int main()
 {
     controller.attach(&Controller, M1_TS);
-    //motor1PWM.period(Pwmperiod);
-    //motor2PWM.period(Pwmperiod);
+    motor1PWM.period(Pwmperiod);
+    motor2PWM.period(Pwmperiod);
     
     while(1){
     /*