testcode pid
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of testPID by
Diff: main.cpp
- Revision:
- 30:ec66691d226d
- Parent:
- 28:0460786f9573
- Child:
- 31:98a1155f5edb
diff -r e0f7af88563f -r ec66691d226d main.cpp --- a/main.cpp Mon Oct 19 00:16:39 2015 +0000 +++ b/main.cpp Tue Oct 20 13:13:25 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=1; const double m1_ki=0.0125; const double m1_kd=0.1; //Proportional, integral and derivative gains. +const double m1_kp=0.01; const double m1_ki=0.125; const double m1_kd=0.5; //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=1; const double m2_ki=0.0125; const double m2_kd=0.1; //Proportional, integral and derivative gains. +const double m2_kp=0.01; const double m2_ki=0.125; const double m2_kd=0.5; //Proportional, integral and derivative gains. //lowpass filter 7 Hz - envelope @@ -97,7 +97,7 @@ //PID filter (lowpass ??? Hz) biquadFilter derfilter( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // derivative filter - +biquadFilter derfilter2( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); /*-------------------------------------------------------------------------------------------------------------------- ---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------ --------------------------------------------------------------------------------------------------------------------*/ @@ -117,6 +117,7 @@ //Reusable PID controller, previous and integral error need to be passed by reference double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev); +double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev); //Menu functions void mainMenu(); @@ -128,81 +129,89 @@ ---- MAIN LOOP ------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ +volatile bool looptimerflag; + +void setlooptimerflag(void) +{ + looptimerflag = true; +} + int main() { pc.baud(115200); //terminal baudrate red=1; green=1; blue=1; //Make sure debug LEDS are off //Set PwmOut frequency to 10k Hz??? - //pwm_motor1.period(0.01); - //pwm_motor2.period(0.01); - - dir_motor1.write(1); //for motor 1 - 1 = counterclockwise - dir_motor2.write(1); //for motor 2 - 1 = clockwise - pwm_motor1=0; - pwm_motor2=0; + pwm_motor1.period(0.0001); + pwm_motor2.period(0.0001); - clearTerminal(); //Clear the putty window + //VARIABLES + AnalogIn potmeter(A4); + AnalogIn potmeter2(A5); + //DigitalIn button(D8); + //MODSERIAL pc(USBTX,USBRX); - // make a menu, user has to initiate next step - mainMenu(); - //caliMenu(); // Menu function - //calibrate_arm(); //Start Calibration + //Encoder motor1(D13,D12); // channel A and B from encoder, true - getSpeed works + //PwmOut pwm_motor1(D6); // D4 and D5 = motor 2, D7 and D6 = motor 2 + //DigitalOut dir_motor1(D7); // - //calibrate_emg(); - wait(3); - start_control(); //100 Hz control + //Encoder motor2(D10,D9); // channel A and B from encoder, true - getSpeed works + // PwmOut pwm_motor2(D5); // D4 and D5 = motor 2, D7 and D6 = motor 2 + // DigitalOut dir_motor2(D4); // - //maybe some stop commands when a button is pressed: detach from timers. - //stop_control(); - //stop_sampling(); - char c; - pc.printf("entering while loop \r\n"); - while(1) { + // MOTOR1 + double goal; + double pwm_to_motor; + // MOTOR2 + double goal2; + double pwm_to_motor2; + + //CODE + pc.baud(115200); - if( pc.readable() ){ - c = pc.getc(); - switch (c) - { - case '1' : - x = x + 0.01; - //controlMenu(); - //running=false; - break; - - case '2' : - x-=0.01; - //controlMenu(); - //running=false; - break; - - case '3' : - y+=0.01; - //controlMenu(); - //running=false; - break; - - - case '4' : - y-=0.01; - //controlMenu(); - //running=false; - break; - - case 'q' : - pc.printf("Quit"); - //running = false; - break; - }//end switch - pc.printf("Reference position: %f and %f \r\n",x,y); - pc.printf("Current position: %f and %f \r\n",current_x,current_y); - pc.printf("Current angles: %f and %f \r\n",theta1,theta2); - pc.printf("Error in angles: %f and %f \r\n",dtheta1,dtheta2); - pc.printf("PID output: %f and %f \r\n",u1,u2); - pc.printf("----------------------------------------\r\n\n"); - } - //end if + //pwm_motor1.write(0.2f); // Speed + //dir_motor1.write(1); // Direction + + Ticker looptimer; + looptimer.attach(setlooptimerflag,0.01); + + while (1) { + + while(looptimerflag != true); + looptimerflag = false; + + //MOTOR1 + goal = (potmeter.read()-0.5)*4200; + //pc.printf("setpoint: %f, %d, %f \n\r", goal, motor1.getPosition(),motor1.getSpeed()); + double error1 = (goal - Encoder1.getPulses()); + pwm_to_motor = pid(error1,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev); + + if(pwm_to_motor > 0) + dir_motor1 = 1; //=clockwise + else + dir_motor1 = 0; //=counterclockwise + + pwm_motor1.write(abs(pwm_to_motor)); + + //MOTOR2 + goal2 = (potmeter2.read()-0.5)*4200; + + double error2=(goal2 - Encoder2.getPulses()); + pwm_to_motor2 = pid2(error2,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev); + + if(pwm_to_motor2 > 0) + dir_motor2 = 0; //=counterclockwise + else + dir_motor2 = 1; //=clockwise + + pwm_motor2.write(abs(pwm_to_motor2)); + //pc.printf("setpoint: %f, %d, %f \n\r", goal2, motor2.getPosition()); + + + pc.printf("goal: %f, pulses: %d \n\r", goal, Encoder1.getPulses()); + pc.printf("goal2: %f, pulses2: %d \n\r", goal2, Encoder2.getPulses()); + } //end of while loop } @@ -277,7 +286,20 @@ // Integral e_int = e_int + CONTROL_RATE * error; // PID - return kp*error + ki*e_int + kd * e_der; + return kp*error + ki*e_int; //+ kd * e_der; + +} + +double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev) +{ + // Derivative + double e_der = (error-e_prev)/ CONTROL_RATE; + e_der = derfilter2.step(e_der); + e_prev = error; + // Integral + e_int = e_int + CONTROL_RATE * error; + // PID + return kp*error + ki*e_int; //+ kd * e_der; }