Diff: main.cpp

Revision:

1:13d8940f0fd4

Parent:

0:77ad62c61c78

Child:

2:2563d1d8461f

--- a/main.cpp	Mon Oct 23 14:26:28 2017 +0000
+++ b/main.cpp	Fri Oct 27 08:59:41 2017 +0000
@@ -40,17 +40,17 @@
 double m1_f_v1  = 0  , m1_f_v2  = 0 ;
 //End of constant for the PID
 
-/*Get reference position in different way-----------------START
+//Get reference position in different way-----------------START
 float Get_X_Position(){
-    double x = potMeter1 * potmultiplier;
-    return x;
+    double X = potMeter1 * potmultiplier;
+    return X;
 }
 
 float Get_Y_Position(){
-    double y = potMeter2 * potmultiplier;
-    return y;
+    double Y = potMeter2 * potmultiplier;
+    return Y;
 }
------------------------------------------------------------END*/
+//-----------------------------------------------------------END
 
 //Start PID part ------------------------------------------START
 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2){
@@ -78,32 +78,35 @@
 
 
 void Controller(){
-    double x = potMeter1 * potmultiplier;
-    double y = potMeter2 * potmultiplier;
+    //double x = potMeter1 * potmultiplier;
+    double x = Get_X_Position();
+    
+    double y = Get_Y_Position();
+    //double y = potMeter2 * potmultiplier;
     
     double reference_motor1 = atan(y/x); // reference for Theta
     double reference_motor2 = sqrt((x*x+y*y));  // reference for radius
     
-    double position_motor1 = RAD_PER_PULSE*Encoder1.getPulses(); // current position for theta
-    double position_motor2 = RAD_PER_PULSE*Encoder2.getPulses(); // current position for the radius
+    int position_motor1 = RAD_PER_PULSE*Encoder1.getPulses(); // current position for theta
+    int position_motor2 = RAD_PER_PULSE*Encoder2.getPulses(); // current position for the radius
     
     double motor1 = PID(reference_motor1 - position_motor1, 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 motor2 = PID(reference_motor2 - position_motor2, 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);
 
     pc.baud(115200);
-    pc.printf("\r Position(X,Y)=(%f,%f), Ref(Theta,R): (%f,%f), Pos(Theta,R):(%f,%f), Motor Value(M1,M2):(%f,%f).\n",x, y, reference_motor1, reference_motor2, position_motor1, position_motor2, motor1, motor2);
+    pc.printf("\r Position(X,Y)=(%f,%f), Ref(Theta,R): (%f,%f), Pos(Theta,R):(%i,%i), Motor Value(M1,M2):(%f,%f).\n",x, y, reference_motor1, reference_motor2, position_motor1, position_motor2, motor1, motor2);
     
     motor1PWM = motor1;
     motor2PWM = motor2;
 
-    if(motor1 > 0.5){
+    if(motor1 > 0.3){
         motor1DC = 1;
         
         ledr = 1;
         ledg = 1;       //Blau
         ledb = 0;
     }
-    else if (motor1<-0.5) {
+    else if (motor1<-0.3) {
         motor1DC = 0; 
         
         ledb = 1;