first publish not working
Dependencies: MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter
Diff: main.cpp
- Revision:
- 9:edf01d06935e
- Parent:
- 8:b932f8b71d3a
- Child:
- 10:4b0b4f2abacf
diff -r b932f8b71d3a -r edf01d06935e main.cpp --- 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 }