testcode pid
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of testPID by
main.cpp
- Committer:
- Vigilance88
- Date:
- 2015-10-20
- Revision:
- 31:98a1155f5edb
- Parent:
- 30:ec66691d226d
- Child:
- 32:054900bfb0a5
File content as of revision 31:98a1155f5edb:
#include "mbed.h" #include "HIDScope.h" #include "MODSERIAL.h" #include "biquadFilter.h" #include "QEI.h" #include "math.h" #include <string> /*-------------------------------------------------------------------------------------------------------------------- -------------------------------- BIOROBOTICS GROUP 14 ---------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ //Define important constants in memory #define PI 3.14159265 #define SAMPLE_RATE 0.002 //500 Hz EMG sample rate #define CONTROL_RATE 0.01 //100 Hz Control rate #define ENCODER1_CPR 4200 //encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft #define ENCODER2_CPR 4200 //gearbox 1:131.25 -> 4200 counts per revolution of the output shaft (X2), #define PWM_PERIOD 0.0001 //10k Hz pwm motor frequency. Higher -> too hot, lower -> motor doesnt respond correctly /*-------------------------------------------------------------------------------------------------------------------- ---- OBJECTS --------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ MODSERIAL pc(USBTX,USBRX); //serial communication //Debug legs DigitalOut red(LED_RED); DigitalOut green(LED_GREEN); DigitalOut blue(LED_BLUE); Ticker sample_timer; //Ticker for EMG sampling Ticker control_timer; //Ticker for control loop HIDScope scope(4); //Scope 4 channels // AnalogIn potmeter(A4); //potmeters // AnalogIn potmeter2(A5); // //Encoders QEI Encoder1(D13,D12,NC,32); //channel A and B from encoder, counts = Encoder.getPulses(); QEI Encoder2(D10,D9,NC,32); //channel A and B from encoder, //Speed and Direction of motors - D4 (dir) and D5(speed) = motor 2, D7(dir) and D6(speed) = motor 1 PwmOut pwm_motor1(D6); //PWM motor 1 PwmOut pwm_motor2(D5); //PWM motor 2 DigitalOut dir_motor1(D7); //Direction motor 1 DigitalOut dir_motor2(D4); //Direction motor 2 //Limit Switches InterruptIn shoulder_limit(D2); //using FRDM buttons InterruptIn elbow_limit(D3); //using FRDM buttons //Other buttons DigitalIn button1(PTA4); //using FRDM buttons DigitalIn button2(PTC6); //using FRDM buttons /*-------------------------------------------------------------------------------------------------------------------- ---- DECLARE VARIABLES ----------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ //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.01; const double m1_ki=0.00125; 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=0.01; const double m2_ki=0.00125; const double m2_kd=0.5; //Proportional, integral and derivative gains. //lowpass filter 7 Hz - envelope const double low_b0 = 0.000119046743110057; const double low_b1 = 0.000238093486220118; const double low_b2 = 0.000119046743110057; const double low_a1 = -1.968902268531908; const double low_a2 = 0.9693784555043481; //Forward Kinematics const double l1 = 0.25; const double l2 = 0.25; double current_x; double current_y; double theta1; double theta2; double dtheta1; double dtheta2; double rpc; double x=0.5; double y=0; double x_error; double y_error; double costheta1; double sintheta1; double costheta2; double sintheta2; /*-------------------------------------------------------------------------------------------------------------------- ---- Filters --------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ //Using biquadFilter library //Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered. //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 ------------------------------------------------------------------------------------------ --------------------------------------------------------------------------------------------------------------------*/ void control(); //Control - reference -> error -> pid -> motor output void calibrate_arm(void); //Calibration of the arm with limit switches void start_sampling(void); //Attaches the sample_filter function to a 500Hz ticker void stop_sampling(void); //Stops sample_filter void start_control(void); //Attaches the control function to a 100Hz ticker void stop_control(void); //Stops control function //Keeps the input between min and max value void keep_in_range(double * in, double min, double max); //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(); void caliMenu(); void clearTerminal(); void controlMenu(); /*-------------------------------------------------------------------------------------------------------------------- ---- 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.0001); pwm_motor2.period(0.0001); //VARIABLES AnalogIn potmeter(A4); AnalogIn potmeter2(A5); //DigitalIn button(D8); //MODSERIAL pc(USBTX,USBRX); //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); // //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); // // MOTOR1 double goal; double pwm_to_motor; // MOTOR2 double goal2; double pwm_to_motor2; //CODE pc.baud(115200); //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 } //end of main /*-------------------------------------------------------------------------------------------------------------------- ---- FUNCTIONS ------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position. void calibrate_arm(void) { pc.printf("Calibrate_arm() entered\r\n"); bool calibrating = true; bool done1 = false; bool done2 = false; pc.printf("To start arm calibration, press any key =>"); pc.getc(); pc.printf("\r\n Calibrating... \r\n"); dir_motor1=1; //cw dir_motor2=0; //cw pwm_motor1.write(0.2); //move upper arm slowly cw while(calibrating){ red=0; blue=0; //Debug light is purple during arm calibration if(done1==true){ pwm_motor2.write(0.2); //move forearm slowly cw } //when limit switches are hit, stop motor and reset encoder if(shoulder_limit.read() < 0.5){ //polling pwm_motor1.write(0); Encoder1.reset(); done1 = true; pc.printf("Shoulder Limit hit - shutting down motor 1\r\n"); } if(elbow_limit.read() < 0.5){ //polling pwm_motor2.write(0); Encoder2.reset(); done2 = true; pc.printf("Elbow Limit hit - shutting down motor 2. \r\n"); } if(done1 && done2){ calibrating=false; //Leave while loop when both limits are reached red=1; blue=1; //Turn debug light off when calibration complete } }//end while //TO DO: //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 / (2*pi/4200) ) //Encoder1.setPulses(100); //edited QEI library: added setPulses() //Encoder2.setPulses(100); //edited QEI library: added setPulses() //pc.printf("Elbow Limit hit - shutting down motor 2. Current pulsecount: %i \r\n",Encoder1.getPulses()); wait(1); pc.printf("Arm Calibration Complete\r\n"); } //Input error and Kp, Kd, Ki, output control signal double pid(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 = derfilter.step(e_der); e_prev = error; // Integral e_int = e_int + CONTROL_RATE * error; // PID 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; } void controlMenu() { pc.printf("1) Move Arm Left\r\n"); pc.printf("2) Move Arm Right\r\n"); pc.printf("3) Move Arm Up\r\n"); pc.printf("4) Move Arm Down\r\n"); pc.printf("q) Exit \r\n"); pc.printf("Please make a choice => \r\n"); } //Analyze filtered EMG, calculate reference position from EMG, compare reference position with current position,convert to angles, send error through pid(), send PWM and DIR to motors void control() { //Current position - Encoder counts -> current angle -> Forward Kinematics rpc=(2*PI)/ENCODER1_CPR; //radians per count (resolution) - 2pi divided by 4200 theta1 = Encoder1.getPulses() * rpc; //multiply resolution with number of counts theta2 = Encoder2.getPulses() * rpc; current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2); current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2); //pc.printf("Calculated current position: x = %f and y = %f \r\n",current_x,current_y); //pc.printf("X is %f and Y is %f \r\n",x,y); //calculate error (refpos-currentpos) currentpos = forward kinematics x_error = x - current_x; y_error = y - current_y; //pc.printf("X error is %f and Y error is %f \r\n",x_error,y_error); //inverse kinematics (refpos to refangle) costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ; sintheta2 = sqrt( 1 - pow(costheta2,2) ); //pc.printf("costheta2 = %f and sostheta2 = %f \r\n",costheta2,sostheta2); dtheta2 = atan2(sintheta2,costheta2); 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); dtheta1 = atan2(sintheta1,costheta1); //Angle error m1_error = dtheta1-theta1; m2_error = dtheta2-theta2; //pc.printf("m1 error is %f and m2 error is %f \r\n",m1_error,m2_error); //PID controller 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,-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 ? 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 ? 0 : 1; //conditional statement, same as if else below pwm_motor2.write(abs(u2)); // } //if //} //while /*if(u1 > 0) { dir_motor1 = 0; else{ dir_motor1 = 1; } } pwm_motor1.write(abs(u1)); if(u2 > 0) { dir_motor1 = 1; else{ dir_motor1 = 0; } } pwm_motor1.write(abs(u2));*/ } void mainMenu() { //Title Box pc.putc(201); for(int j=0;j<33;j++){ pc.putc(205); } pc.putc(187); pc.printf("\n\r"); pc.putc(186); pc.printf(" BioRobotics M9 - Group 14 "); pc.putc(186); pc.printf("\n\r"); pc.putc(186); pc.printf(" Robot powered ON "); pc.putc(186); pc.printf("\n\r"); pc.putc(200); for(int k=0;k<33;k++){ pc.putc(205); } pc.putc(188); pc.printf("\n\r"); //endbox } void caliMenu(){}; //Start control void start_control(void) { control_timer.attach(&control,0.01); //100 Hz control //Debug LED will be blue when control is on. If sampling and control are on -> blue + green = cyan. blue=0; pc.printf("||- started control -|| \r\n"); } //stop control void stop_control(void) { control_timer.detach(); //Debug LED will be off when control is off blue=1; pc.printf("||- stopped control -|| \r\n"); } void calibrate() { } //Clears the putty (or other terminal) window void clearTerminal() { pc.putc(27); pc.printf("[2J"); // clear screen pc.putc(27); // ESC pc.printf("[H"); // cursor to home } //keeps input limited between min max void keep_in_range(double * in, double min, double max) { *in > min ? *in < max? : *in = max: *in = min; }