testcode pid
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of testPID by
Diff: main.cpp
- Revision:
- 26:fe3a5469dd6b
- Parent:
- 25:49ccdc98639a
- Child:
- 27:0cefe83f83d3
diff -r 49ccdc98639a -r fe3a5469dd6b main.cpp --- a/main.cpp Fri Oct 16 13:46:39 2015 +0000 +++ b/main.cpp Sun Oct 18 13:13:07 2015 +0000 @@ -4,6 +4,7 @@ #include "biquadFilter.h" #include "QEI.h" #include "math.h" +#include <string> /*-------------------------------------------------------------------------------------------------------------------- -------------------------------- BIOROBOTICS GROUP 14 ---------------------------------------------------------------- @@ -15,14 +16,12 @@ #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 -DigitalIn button(PTA1); //buttons -DigitalIn button1(PTB9); // //Debug legs DigitalOut red(LED_RED); @@ -49,33 +48,56 @@ //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 -DigitalIn shoulder_limit(PTA4); //using FRDM buttons -DigitalIn elbow_limit(PTC6); //using FRDM buttons +InterruptIn shoulder_limit(D3); //using FRDM buttons +InterruptIn elbow_limit(D4); //using FRDM buttons + +//Other buttons +DigitalIn button1(PTA4); //using FRDM buttons +DigitalIn button2(PTC6); //using FRDM buttons + +/*Text colors ASCII code: Set Attribute Mode <ESC>[{attr1};...;{attrn}m + +\ 0 3 3 - ESC +[ 3 0 m - black +[ 3 1 m - red +[ 3 2 m - green +[ 3 3 m - yellow +[ 3 4 m - blue +[ 3 5 m - magenta +[ 3 6 m - cyan +[ 3 7 m - white +[ 0 m - reset attributes + +Put the text you want to color between GREEN_ and _GREEN +*/ +string GREEN_ = "\033[32m"; //esc - green +string _GREEN = "\033[0m"; //esc - reset /*-------------------------------------------------------------------------------------------------------------------- ---- DECLARE VARIABLES ----------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ -//EMG variables +//EMG variables: raw EMG - filtered EMG - maximum voluntary contraction double emg_biceps; double biceps_power; double bicepsMVC = 0; double emg_triceps; double triceps_power; double tricepsMVC = 0; double emg_flexor; double flexor_power; double flexorMVC = 0; double emg_extens; double extens_power; double extensMVC = 0; -int muscle; -double calibrate_time; +int muscle; //Muscle selector for MVC measurement +double calibrate_time; //Elapsed time for each MVC measurement //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; const double m1_ki=0; const double m1_kd=0; //Proportional, integral and derivative gains. -double m2_error=0; double m2_e_int=0; double m2_e_prev=0; //Error, integrated error, previous error +double m2_error=0; double m2_e_int=0; double m2_e_prev=0; //Error, integrated error, previous error const double m2_kp=0; const double m2_ki=0; const double m2_kd=0; //Proportional, integral and derivative gains. //highpass filter 20 Hz @@ -119,7 +141,7 @@ const double notch2_a1 = -1.61244708381 * notch2gain; const double notch2_a2 = 0.97415116257 * notch2gain; -//lowpass filter 7 Hz - envelop +//lowpass filter 7 Hz - envelope const double low_b0 = 0.000119046743110057; const double low_b1 = 0.000238093486220118; const double low_b2 = 0.000119046743110057; @@ -127,52 +149,64 @@ const double low_a2 = 0.9693784555043481; - /*-------------------------------------------------------------------------------------------------------------------- ---- Filters --------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ //Using biquadFilter library //Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered. +//Biceps biquadFilter highpass( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts biquadFilter notch1( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference biquadFilter notch2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // biquadFilter lowpass( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope +//Triceps biquadFilter highpass2( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts -biquadFilter notch1_2( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference -biquadFilter notch2_2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // +biquadFilter notch1_2( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference +biquadFilter notch2_2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // biquadFilter lowpass2( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope +//Flexor biquadFilter highpass3( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts -biquadFilter notch1_3( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference -biquadFilter notch2_3( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // +biquadFilter notch1_3( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference +biquadFilter notch2_3( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // biquadFilter lowpass3( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope +//Extensor biquadFilter highpass4( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts -biquadFilter notch1_4( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference -biquadFilter notch2_4( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // +biquadFilter notch1_4( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference +biquadFilter notch2_4( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // biquadFilter lowpass4( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope +//PID filter (lowpass ??? Hz) biquadFilter derfilter( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // derivative filter /*-------------------------------------------------------------------------------------------------------------------- ---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------ --------------------------------------------------------------------------------------------------------------------*/ + +void sample_filter(void); //Sampling and filtering +void control(); //Control - reference -> error -> pid -> motor output +void calibrate_emg(); //Instructions + measurement of MVC of each muscle +void emg_mvc(); //Helper funcion for storing MVC value +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); -void sample_filter(void); -void control(); -void calibrate_emg(); -void emg_mvc(); -void calibrate_arm(void); -void start_sampling(void); -void stop_sampling(void); -void start_control(void); -void stop_control(void); + +//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); +//Menu functions void mainMenu(); void caliMenu(); +void clearTerminal(); + /*-------------------------------------------------------------------------------------------------------------------- ---- MAIN LOOP ------------------------------------------------------------------------------------------------------- @@ -180,10 +214,17 @@ int main() { - pc.baud(115200); - red=1; green=1; blue=1; + 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(PWM_PERIOD); + pwm_motor2.period(PWM_PERIOD); + + clearTerminal(); //Clear the putty window + // make a menu, user has to initiate next step - //mainMenu(); + mainMenu(); //caliMenu(); // Menu function //calibrate_arm(); //Start Calibration @@ -244,31 +285,35 @@ //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position. void calibrate_arm(void) { - //const double hit = 0.5; - dir_motor1=1; //ccw - dir_motor2=1; //ccw - pwm_motor1.write(0.2f); //move upper arm slowly ccw - pwm_motor2.write(0.2f); //move forearm slowly ccw + red=0; blue=0; //Debug light is purple during arm calibration + bool calibrating = true; + bool done1 = false; + bool done2 = false; + dir_motor1=1; //cw + dir_motor2=1; //cw + pwm_motor1.write(0.2f); //move upper arm slowly cw + pwm_motor2.write(0.2f); //move forearm slowly cw + + while(calibrating){ - if(shoulder_limit.read() < 0.5){ //when limit switches are hit, stop motor and reset encoder - pwm_motor1.write(0); - Encoder1.reset(); - } - if(elbow_limit.read() < 0.5){ - pwm_motor2.write(0); - Encoder2.reset(); - } + //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; + } + if(elbow_limit.read() < 0.5){ //polling + pwm_motor2.write(0); + Encoder2.reset(); + done2 = true; + } + if(done1 && done2){ + calibrating=false; //Leave while loop when both limits are reached + red=1; blue=1; //Turn debug light off when calibration complete + } - /* while(shoulder_limit != hit){ - pwm_motor1.write(0.4); - } - Encoder1.reset(); - - while(elbow_limit != hit){ - pwm_motor2.write(0.4); - } - Encoder2.reset(); - */ + }//end while + } //EMG Maximum Voluntary Contraction measurement @@ -284,7 +329,8 @@ if(muscle==1){ if(biceps_power>bicepsMVC){ - printf("+ "); + //printf("+ "); + printf("%s+ %s",GREEN_,_GREEN); bicepsMVC=biceps_power; } else @@ -294,22 +340,31 @@ if(muscle==2){ if(triceps_power>tricepsMVC){ + printf("%s+ %s",GREEN_,_GREEN); tricepsMVC=triceps_power; } + else + printf("- "); } if(muscle==3){ if(flexor_power>flexorMVC){ + printf("%s+ %s",GREEN_,_GREEN); flexorMVC=flexor_power; } + else + printf("- "); } if(muscle==4){ if(extens_power>extensMVC){ + printf("%s+ %s",GREEN_,_GREEN); extensMVC=extens_power; } + else + printf("- "); } //} @@ -324,12 +379,11 @@ { Ticker timer; - pc.printf("|-- Robot Started --| \r\n"); pc.printf("Testcode calibration \r\n"); wait(1); pc.printf("+ means current sample is higher than stored MVC\r\n"); pc.printf("- means current sample is lower than stored MVC\r\n"); - wait(3); + wait(2); pc.printf(" Biceps is first. "); wait(1); pc.printf(" Press any key to begin... "); wait(1); char input; @@ -344,13 +398,15 @@ timer.attach(&emg_mvc,0.002); wait(5); timer.detach(); - pc.printf("\r\n MVC = %f \r\n",bicepsMVC); - - pc.printf("Calibrate_emg() exited \r\n"); - pc.printf("Measured time: %f seconds \r\n",calibrate_time); + + pc.printf("\r\n Measurement complete."); wait(1); + pc.printf("\r\n Biceps MVC = %f \r\n",bicepsMVC); wait(1); + pc.printf("Calibrate_emg() exited \r\n"); wait(1); + pc.printf("Measured time: %f seconds \r\n\n",calibrate_time); calibrate_time=0; // Triceps: + muscle=2; pc.printf(" Triceps is next "); wait(1); pc.printf(" Press any key to begin... "); wait(1); input=pc.getc(); @@ -370,9 +426,11 @@ calibrate_time=0; //Flexor: - + muscle=3; //Extensor: + muscle=4; + //Stop sampling, detach ticker stop_sampling(); } @@ -440,14 +498,37 @@ } -void mainMenu(){}; +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 sampling void start_sampling(void) { sample_timer.attach(&sample_filter,SAMPLE_RATE); //500 Hz EMG - blue=0; green=0; + + //Debug LED will be green when sampling is active + green=0; pc.printf("||- started sampling -|| \r\n"); } @@ -455,7 +536,9 @@ void stop_sampling(void) { sample_timer.detach(); - blue=1; green=1; + + //Debug LED will be turned off when sampling stops + green=1; pc.printf("||- stopped sampling -|| \r\n"); } @@ -463,12 +546,20 @@ void start_control(void) { control_timer.attach(&control,SAMPLE_RATE); //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"); } @@ -477,6 +568,14 @@ } +//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)