2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 21:d6a46315d5f5
- Parent:
- 20:0ede3818e08e
- Child:
- 22:1ba637601dc3
--- a/main.cpp Mon Oct 12 14:02:20 2015 +0000 +++ b/main.cpp Tue Oct 13 10:02:48 2015 +0000 @@ -3,18 +3,28 @@ #include "MODSERIAL.h" #include "biquadFilter.h" #include "QEI.h" +#include "math.h" + +/*-------------------------------------------------------------------------------------------------------------------- +-------------------------------- 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 4200 counts per revolution of the output shaft (X2) -#define ENCODER2_CPR 4200 //64 X4, 32 X4 counts per revolution of motor shaft, gearbox 1:131.25 +#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), -//Objects +/*-------------------------------------------------------------------------------------------------------------------- +---- OBJECTS --------------------------------------------------------------------------------------------------------- +--------------------------------------------------------------------------------------------------------------------*/ + MODSERIAL pc(USBTX,USBRX); //serial communication DigitalIn button(PTA1); //buttons DigitalIn button1(PTB9); // +//EMG shields AnalogIn emg1(A0); //Analog input - Biceps EMG AnalogIn emg2(A1); //Analog input - Triceps EMG AnalogIn emg3(A2); //Analog input - Flexor EMG @@ -27,27 +37,90 @@ // 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, -PwmOut pwm_motor1(D6); //D4 and D5 = motor 2, D7 and D6 = motor 1 -PwmOut pwm_motor2(D5); //D4 and D5 = motor 2, D7 and D6 = motor 1 + +//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 //Filters -//biquadFilter filter(a1, a2, b0, b1, b2); coefficients -biquadFilter filter1( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass -biquadFilter filter2( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass -biquadFilter filter3( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass -biquadFilter filter4( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass +//Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered. +biquadFilter highpass( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // removes DC and movement artefacts +biquadFilter notch1( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // removes 49-51 Hz power interference +biquadFilter notch2( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // +biquadFilter lowpass( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // EMG envelope +biquadFilter derfilter( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // derivative filter + +/*-------------------------------------------------------------------------------------------------------------------- +---- DECLARE VARIABLES ----------------------------------------------------------------------------------------------- +--------------------------------------------------------------------------------------------------------------------*/ + +//EMG variables +double biceps_power; double bicepsMVC = 0; +double triceps_power; double tricepsMVC = 0; +double flexor_power; double flexorMVC = 0; +double extens_power; double extensorMVC = 0; + + + +/*-------------------------------------------------------------------------------------------------------------------- +---- DECLARE FUNCTION NAMES------------------------------------------------------------------------------------------- +--------------------------------------------------------------------------------------------------------------------*/ +void keep_in_range(float * in, float min, float max); +void sample_filter(void); +void control(); +void calibrate_emg(int muscle); +void calibrate_arm(void); +void start_sampling(void); +void stop_sampling(void); +void start_control(void); +void stop_control(void); +double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev); -void keep_in_range(float * in, float min, float max); -void sample(void); +void mainMenu(); +void caliMenu(); + +/*-------------------------------------------------------------------------------------------------------------------- +---- MAIN LOOP ------------------------------------------------------------------------------------------------------- +--------------------------------------------------------------------------------------------------------------------*/ + +int main() +{ + // make a menu, user has to initiated next step + mainMenu(); + caliMenu(); // Menu function + calibrate_arm(); //Start Calibration + start_sampling(); //500 Hz EMG + calibrate_emg(1); + start_control(); //100 Hz control + + //maybe some stop commands when a button is pressed: detach from timers. + //stop_control(); + //stop_sampling(); + + while(1) { + scope.set(0,emg_biceps); + scope.set(1,emg_triceps); + scope.set(2,emg_flexor); + scope.set(3,emg_extens); + scope.send(); + } + //end of while loop +} +//end of main + +/*-------------------------------------------------------------------------------------------------------------------- +---- FUNCTIONS ------------------------------------------------------------------------------------------------------- +--------------------------------------------------------------------------------------------------------------------*/ //Start sampling void start_sampling(void) { - sample_timer.attach(&sample,SAMPLE_RATE); + sample_timer.attach(&sample_filter,SAMPLE_RATE); //500 Hz EMG } //stop sampling @@ -56,18 +129,35 @@ sample_timer.detach(); } -//Sample function -void sample(void) +//Sample and Filter +void sample_filter(void) { - double emg_value1 = emg1.read(); - double emg_value2 = emg2.read(); - double emg_value3 = emg3.read(); - double emg_value4 = emg4.read(); - scope.set(0,emg_value1); - scope.set(1,emg_value2); - scope.set(2,emg_value3); - scope.set(3,emg_value4); - scope.send(); + double emg_biceps = emg1.read(); //Biceps + double emg_triceps = emg2.read(); //Triceps + double emg_flexor = emg3.read(); //Flexor + double emg_extens = emg4.read(); //Extensor + + //Filter: high-pass -> notch -> rectify -> lowpass or moving average + biceps_power = highpass.step(emg_biceps); triceps_power = highpass.step(emg_triceps); flexor_power = highpass.step(emg_flexor); extens_power = highpass.step(emg_extens); + biceps_power = notch1.step(biceps_power); triceps_power = notch1.step(triceps_power); flexor_power = notch1.step(flexor_power); extens_power = notch1.step(extens_power); + biceps_power = notch2.step(biceps_power); triceps_power = notch2.step(triceps_power); flexor_power = notch2.step(flexor_power); extens_power = notch2.step(extens_power); + biceps_power = abs(biceps_power); triceps_power = abs(triceps_power); flexor_power = abs(flexor_power); extens_power = abs(extens_power); + biceps_power = lowpass.step(biceps_power); triceps_power = lowpass.step(triceps_power); flexor_power = lowpass.step(flexor_power); extens_power = lowpass.step(extens_power); + + /* alternative for lowpass filter: moving average + window=30; //30 samples + int i=0; //buffer index + bicepsbuffer[i]=biceps_power //fill array + + i++; + if(i==window){ + i=0; + } + + */ + + + } //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position. @@ -75,30 +165,44 @@ { } -//EMG MVC measurement -void calibrate_emg(void) +//EMG Maximum Voluntary Contraction measurement +void calibrate_emg(int muscle) { + if(muscle==1){ + start_sampling(); + + for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples + if(biceps_power>bicepsMVC){ + bicepsMVC=biceps_power; + } + } + stop_sampling(); + } + + if(muscle==2){ + start_sampling(); + + for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples + if(triceps_power>tricepsMVC){ + tricepsMVC=triceps_power; + } + } + stop_sampling(); + } } -//use biquadFilter library, output filtered EMG (combine with sample?) -double filter(double u) -{ - double test; - return test; -} //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 = biquad( e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2 ); - e_prev = e; + 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; + e_int = e_int+CONTROL_RATE*error; // PID - return Kp ∗ e + Ki ∗ e_int + Kd ∗ e_der; + return kp*error + ki*e_int + kd * e_der; } @@ -114,14 +218,46 @@ //inverse kinematics (pos error to angle error) //PID controller - pid(); - //send PWM and DIR to motors + double error=0;double kp=0;double ki=0;double kd=0;double e_int=0;double e_prev=0; + u1=pid(error,kp,ki,kd,e_int,e_prev); //motor 1 + u2=pid(error,kp,ki,kd,e_int,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); + + //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_motor1 = u2>0 ? 1 : 0; //conditional statement, same as if else below + pwm_motor1.write(abs(u2)); + + /*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));*/ + } //Start control void start_control(void) { - control_timer.attach(&control,SAMPLE_RATE); + control_timer.attach(&control,SAMPLE_RATE); //100 Hz control } //stop control @@ -130,25 +266,14 @@ control_timer.detach(); } -int main() + +void calibrate() { - // make a menu, user has to initiated next step - - calibrate_arm(); //Start Calibration - calibrate_emg(); - start_sampling(); //500 Hz EMG - start_control(); //100 Hz control - - //maybe some stop commands when a button is pressed: detach from timers. - //stop_control(); - //stop_sampling(); - - while(1) { - - - }//end of while loop -}//end of main + +} + +//keeps input limited between min max void keep_in_range(float * in, float min, float max) { *in > min ? *in < max? : *in = max: *in = min;