Diff: main.cpp

Revision:

9:edf01d06935e

Parent:

8:b932f8b71d3a

Child:

10:4b0b4f2abacf

--- a/main.cpp	Wed Nov 01 00:26:33 2017 +0000
+++ b/main.cpp	Wed Nov 01 10:00:10 2017 +0000
@@ -29,7 +29,7 @@
 
 // ---- Motor Constants-------
 float Pwmperiod = 0.0001f;
-int potmultiplier = 800;    // Multiplier for the pot meter reference which is normally between 0 and 1
+int potmultiplier = 36000;    // 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
 
@@ -44,7 +44,7 @@
 const float     RAD_PER_PULSE = (2*pi)/4200;
 const float     CONTROLLER_TS = 0.01;     //TIME INTERVAL/ hZ
 
-const float     M1_KP = 10; 
+const float     M1_KP = 20; 
 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;
@@ -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, delta1 ,delta2);
+    pc.printf("\r DesPosition(X,Y):(%f,%f), pos Error(dTheta, dError):(%f,%f)\n",x,y, dTheta ,dRadius);
     
     //motor1PWM = motor1;
     //motor2PWM = motor2;
@@ -160,10 +160,12 @@
         ledg = 1;
     }
     
-    motor1 = abs(delta1)/1000.0f;
-    if(motor1 >= 0.50f) {
-        motor1 = 0.50f;
-    }
+    motor1 = abs(delta1)/1000.0;
+    //if(motor1 >= 0.50f) {
+      //  motor1 = 0.50f;
+      //pc.baud(115200);
+      //pc.printf("\r val motor1: %f\n", motor1);
+   // }
 
 if(delta2 > 10.0){
         motor2DC = 0;
@@ -188,15 +190,15 @@
         ledg = 1;
     }
     
-    motor2 = abs(delta2)/1000.0f;
-    if(motor2 >= 0.50f) {
-        motor2 = 0.50f;
-    }
+    motor2 = abs(delta2)/1000.0;
+    //if(motor2 >= 0.50f) {
+    //    motor2 = 0.50f;
+   // }
     
-    motor1PWM = motor1 + 0.50f;
-    motor2PWM = motor2 + 0.50f;
+    motor1PWM = motor1*2; // + 0.50f;
+    motor2PWM = motor2; //+ 0.50f;
     
-    //pc.printf("\r motorvalues (M1,M2):(%f,%f), error:( \n", , motor1PWM, motor2PWM);
+    //pc.printf("\r motorvalues (M1,M2):(%f,%f), error:( \n",  motor1PWM, motor2PWM);
     //pc.printf("\r 
 }