first publish not working
Dependencies: MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter
Diff: main.cpp
- Revision:
- 10:4b0b4f2abacf
- Parent:
- 9:edf01d06935e
- Child:
- 11:66d0be7efd3f
diff -r edf01d06935e -r 4b0b4f2abacf main.cpp --- 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){ /*