Diff: main.cpp

Revision:

14:54cbd8f0efe4

Parent:

13:b5868fd8ffe9

Child:

15:65f295a49a4b

diff -r b5868fd8ffe9 -r 54cbd8f0efe4 main.cpp
--- a/main.cpp	Thu Nov 02 14:47:48 2017 +0000
+++ b/main.cpp	Thu Nov 02 15:43:46 2017 +0000
@@ -44,7 +44,7 @@
 
 double Y;
 double Y_maxTreshold = 480;
-double Y_minTreshold = 0;
+double Y_minTreshold = 20;
 const double Y_Callibrate  = 300;
 
 
@@ -54,12 +54,16 @@
 int     potmultiplier = 600;    // Multiplier for the pot meter reference which is normally between 0 and 1
 const float gearM1 = 6.2;
 const double   gainM1 = 1/38.17;       // encoder pulses per degree theta
-const double   gainM2 = 1/107.8;       // pulses per mm
+const double   gainM2 = 1/100.8;       // pulses per mm
 
 double motor1;
 double motor2;
-double pos_M1;
-double pos_M2;
+
+double reference_motor1 = 0;     // reference for Theta
+double reference_motor2 = 0;
+double pos_M1 = 0; // start angle theta
+double pos_M2 = 0; // start radius
+
 
 //Start constants PID ------------------------------- 
 const double M1_TS = 0.01;      // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
@@ -283,6 +287,11 @@
         else{          
         }
     double pos_M1 = gainM1*Encoder1.getPosition(); // current position for theta
+    double countM1 = Encoder1.getPosition();
+    
+    pc.baud(115200);
+    pc.printf("\r counts encoder: %f\n",countM1);
+    
     return pos_M1;       
 }
 double motor2_Position(){ //output R
@@ -313,11 +322,11 @@
     double x = Get_X_Position();
     double y = Get_Y_Position();
      
-    double reference_motor1 = (atan(y/x)*180)/pi;       // reference for Theta
-    double reference_motor2 = sqrt((x*x+y*y));          // reference for radius
+    reference_motor1 = (atan(y/x)*180)/pi;       // reference for Theta
+    reference_motor2 = sqrt((x*x+y*y));          // reference for radius
     
-    double pos_M1 = motor1_Position(); // current position for theta   
-    double pos_M2 = motor2_Position(); // current position for the radius
+    pos_M1 = motor1_Position(); // current position for theta   
+    pos_M2 = motor2_Position(); // current position for the radius
       
     double delta1 = PID1(reference_motor1 - pos_M1, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
     double delta2 = PID2(reference_motor2 - pos_M2, M2_KP, M2_KI, M2_KD, M1_TS, m2_err_int, m2_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
@@ -327,7 +336,7 @@
 
     pc.baud(115200);
     //pc.printf("\r DesPosition(X,Y):(%f,%f), posError(dTheta, dError):(%f,%f), (delta1,delta2):(%f,%f)\n",x,y, dTheta ,dRadius,delta1, delta2);
-    pc.printf("\r pos(M1,M2):(%f,%f)\n", pos_M1, pos_M2);
+    //pc.printf("\r pos(M1,M2):(%f,%f)\n", pos_M1, pos_M2);
     
     //motor1PWM = motor1;
     //motor2PWM = motor2;