![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
testcode pid
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of robot_2 by
Diff: main.cpp
- Revision:
- 28:0460786f9573
- Parent:
- 27:0cefe83f83d3
--- a/main.cpp Sun Oct 18 23:24:30 2015 +0000 +++ b/main.cpp Mon Oct 19 00:15:39 2015 +0000 @@ -65,10 +65,10 @@ //PID variables double u1; double u2; //Output of PID controller (PWM value for motor 1 and 2) double m1_error=0; double m1_e_int=0; double m1_e_prev=0; //Error, integrated error, previous error -const double m1_kp=0.001; const double m1_ki=1; const double m1_kd=0; //Proportional, integral and derivative gains. +const double m1_kp=1; const double m1_ki=0.0125; const double m1_kd=0.1; //Proportional, integral and derivative gains. double m2_error=0; double m2_e_int=0; double m2_e_prev=0; //Error, integrated error, previous error -const double m2_kp=0.001; const double m2_ki=2; const double m2_kd=0; //Proportional, integral and derivative gains. +const double m2_kp=1; const double m2_ki=0.0125; const double m2_kd=0.1; //Proportional, integral and derivative gains. //lowpass filter 7 Hz - envelope @@ -151,7 +151,7 @@ //calibrate_arm(); //Start Calibration //calibrate_emg(); - wait(1); + wait(3); start_control(); //100 Hz control //maybe some stop commands when a button is pressed: detach from timers. @@ -272,14 +272,13 @@ { // Derivative double e_der = (error-e_prev)/ CONTROL_RATE; - //e_der = derfilter.step(e_der); + e_der = derfilter.step(e_der); e_prev = error; // Integral e_int = e_int + CONTROL_RATE * error; // PID - return kp*error + ki*e_int; + return kp*error + ki*e_int + kd * e_der; - //+ kd * e_der; } void controlMenu() @@ -325,7 +324,7 @@ dtheta2 = atan2(sintheta2,costheta2); - costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sostheta2 ) / ( pow(x,2) + pow(y,2) ); + costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) ); sintheta1 = sqrt( 1 - pow(costheta1,2) ); //pc.printf("costheta1 = %f and sostheta1 = %f \r\n",costheta1,sostheta1); @@ -345,15 +344,15 @@ u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev); //motor 1 u2=pid(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev); //motor 2 - keep_in_range(&u1,-1,1); //keep u between -1 and 1, sign = direction, PWM = 0-1 - keep_in_range(&u2,-1,1); + keep_in_range(&u1,-0.6,0.6); //keep u between -1 and 1, sign = direction, PWM = 0-1 + keep_in_range(&u2,-0.6,0.6); //send PWM and DIR to motor 1 - dir_motor1 = u1>0 ? 0 : 1; //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below. + dir_motor1 = u1>0 ? 1 : 0; //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below. pwm_motor1.write(abs(u1)); //send PWM and DIR to motor 2 - dir_motor2 = u2>0 ? 1 : 0; //conditional statement, same as if else below + dir_motor2 = u2>0 ? 0 : 1; //conditional statement, same as if else below pwm_motor2.write(abs(u2));