![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
first publish not working
Dependencies: MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter
Diff: main.cpp
- Revision:
- 14:54cbd8f0efe4
- Parent:
- 13:b5868fd8ffe9
- Child:
- 15:65f295a49a4b
--- 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;