emg threshold calibartie toegevoegd en wat namen van variabelen veranderd in betere namen
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp
- Committer:
- mefix
- Date:
- 2016-10-31
- Revision:
- 0:3c99f1705565
- Child:
- 1:ba63033da653
File content as of revision 0:3c99f1705565:
#include "mbed.h" #include "HIDScope.h" #include "BiQuad.h" #include "MODSERIAL.h" #include "QEI.h" #include "FastPWM.h" // in gebruik: D(0(TX),1(RX),4(motor2dir),5(motor2pwm),6(motor1pwm),7(motor1dir), //8(pushbutton),9(servoPWM),10(encoder),11(encoder),12(encoder),13(encoder)) A(0,1,2)(emg) MODSERIAL pc(USBTX, USBRX); HIDScope scope(6); // the amount of scopes to send to the pc //Define objects //Define the EMG inputs AnalogIn emg1( A0 ); AnalogIn emg2( A1 ); AnalogIn emg3( A2 ); //Define motor outputs DigitalOut motor1dir(D7); //direction of motor 1, attach at m1, set to 0: cw FastPWM motor1(D6); // speed of motor 1 FastPWM motor2(D5); //speed of motor 2 DigitalOut motor2dir(D4); //direction of motor 2, attach at m2, set to 0: ccw FastPWM servo(D9); //servo pwm QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //defining encoder QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //defining encoder //Define the Tickers Ticker pos_timer; // the timer which is used to print the position every second Ticker sample_timer; // the timer which is used to decide when a sample needs to be taken Ticker control; // Ticker for processing encoder input to motor output Ticker servo_control; // Ticker for calling servo_control //Initialize all variables volatile bool sampletimer = false; // go flag volatile bool controller_go=false; volatile bool servo_go=false; double threshold = 0.04; // the threshold which the emg signals need to surpass to do something double samplefreq=0.002; // every 0.002 sec a sample will be taken this is a frequency of 500 Hz double emg02; // the first emg signal double emg12; // the second emg signal double emg22; // the third emg signal double ref_x=0.0000; // the x reference position double ref_y=0.0000; // the y reference position double old_ref_x; // the old x reference double old_ref_y; // the old y reference double speed=0.00008; // the variable with which a speed is reached of 1cm/s double theta=0.0; // angle of the arm double radius=0.0; // radius of the arm const double minRadius=0.43; // minimum radius of arm const double maxRadius=0.62; // maximum radius of arm const double min_y=-0.26; // minimum height which the spatula can reach char key; // variable to place the keyboard input double m1_pwm=0; //variable for PWM control motor 1 double m2_pwm=0; //variable for PWM control motor 2 const double m1_Kp = 35.16, m1_Ki = 108.8, m1_Kd = 2.84, m1_N = 100; // controller constants motor 1 double m1_v1 = 0, m1_v2 = 0; // Memory variables const double m1_Ts = 0.01; // Controller sample time const double m2_Kp = 9.974, m2_Ki = 16.49, m2_Kd = 1.341, m2_N = 100; // controller constants motor 2 double m2_v1 = 0, m2_v2 = 0; // Memory variables const double m2_Ts = 0.01; // Controller sample time const double pi=3.14159265359; const double res = 64.0/(1.0/131.25*2.0*pi); // resolution on gearbox shaft per pulse const double V_max=9.0; // maximum voltage supplied by trafo const double pulleyRadius=0.0398/2.0; // pulley radius double servo_pwm=0.0023; // duty cycle 1.5 ms 7.5%, min 0.5 ms 2.5%, max 2.5 ms 12.5% const double minTheta=-43.0/180.0*pi; //minimum angle robot const double maxTheta=-32.0/180.0*pi; // maximum angle to which the spatula can stabilise const double diffTheta=maxTheta-minTheta; //difference between max and min angle of theta for stabilisation const double min_servo_pwm=0.00217; // corresponds to angle of theta -32 degrees const double max_servo_pwm=0.0023; // corresponds to angle of theta -43 degrees const double res_servo=max_servo_pwm-min_servo_pwm; //resolution of servo pwm signal between min and max angle const double servo_Ts=0.02; bool z_push=false; //Define the needed Biquad chains BiQuadChain bqc11; BiQuadChain bqc13; BiQuadChain bqc21; BiQuadChain bqc23; BiQuadChain bqc31; BiQuadChain bqc33; //Define the BiQuads for the filter of the first emg signal //Notch filter BiQuad bq111(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589); BiQuad bq112(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787); BiQuad bq113(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798); //High pass filter //BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); //Old biquad values BiQuad bq121( 0.8956, -1.7911, 0.8956, 1.0000, -1.7814, 0.7941); BiQuad bq122( 0.9192, -1.8385, 0.9192, 1.0000, -1.8319, 0.8450); BiQuad bq123( 0.9649, -1.9298, 0.9649, 1.0000, -1.9266, 0.9403); //Low pass filter BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 ); //Define the BiQuads for the filter of the second emg signal //Notch filter BiQuad bq211 = bq111; BiQuad bq212 = bq112; BiQuad bq213 = bq113; //High pass filter BiQuad bq221 = bq121; BiQuad bq222 = bq122; BiQuad bq223 = bq123; //Low pass filter BiQuad bq231 = bq131; //Define the BiQuads for the filter of the third emg signal //notch filter BiQuad bq311 = bq111; BiQuad bq312 = bq112; BiQuad bq313 = bq113; //High pass filter BiQuad bq321 = bq121; BiQuad bq323 = bq122; BiQuad bq322 = bq123; //low pass filter BiQuad bq331 = bq131; void sampleflag() { if (sampletimer==true) { // this if statement is used to see if the code takes too long before it is called again pc.printf("rate too high error in sampleflag\n\r"); } //This sets the go flag for when the function sample needs to be called sampletimer=true; } void activate_controller() { if (controller_go==true) { // this if statement is used to see if the code takes too long before it is called again pc.printf("rate too high error in activate_controller()\n\r"); } controller_go=true; //activate go flag } void activate_servo_control() { if (servo_go==true) { pc.printf("error servo"); } servo_go=true; //activate go flag } void sample() { //This checks if a key is pressed and changes the variable key in the pressed key if (pc.readable()==1) { key=pc.getc(); } //Read the emg signals and filter it emg02=bqc13.step(fabs(bqc11.step(emg1.read()))); //filtered signal 0 emg12=bqc23.step(fabs(bqc21.step(emg2.read()))); //filtered signal 1 emg22=bqc33.step(fabs(bqc31.step(emg3.read()))); //filtered signal 2 //remember what the reference was old_ref_x=ref_x; old_ref_y=ref_y; //look if the emg signals go over the threshold and change the reference accordingly if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') { ref_x=ref_x-speed; ref_y=ref_y-speed; } else if (emg02>threshold&&emg12>threshold || key == 'a' || key == 'z') { ref_x=ref_x-speed; } else if (emg02>threshold&&emg22>threshold || key == 's') { ref_y=ref_y-speed; } else if (emg12>threshold&&emg22>threshold || key == 'e' ) { ref_x=ref_x+speed; ref_y=ref_y+speed; } else if (emg12>threshold || key == 'q' ) { ref_x=ref_x+speed; } else if (emg22>threshold || key == 'w') { ref_y=ref_y+speed; } if (key != 'z' && z_push) { ref_x=0.0; ref_y=0.0; Encoder1.reset(); Encoder2.reset(); z_push=false; } // convert the x and y reference to the theta and radius reference theta=atan(ref_y/(ref_x+minRadius)); radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2)); //look if the new reference is outside the possible range and revert back to the old reference if it is outside the range if (radius < minRadius) { if (key != 'z') { ref_x=old_ref_x; ref_y=old_ref_y; } else if (key == 'z') { z_push=true; } } else if ( radius > maxRadius) { ref_x=old_ref_x; ref_y=old_ref_y; } else if (ref_y<min_y) { ref_y=old_ref_y; } theta=atan(ref_y/(ref_x+minRadius)); radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2)); } double PID( double err, const double Kp, const double Ki, const double Kd, const double Ts, const double N, double &v1, double &v2 ) //discrete PIDF filter { const double a1 =-4/(N*Ts+2), a2=-(N*Ts-2)/(N*Ts+2), b0=(4*Kp + 4*Kd*N + 2*Ki*Ts+2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4), b1=(Ki*N*pow(Ts,2)-4*Kp-4*Kd*N)/(N*Ts+2), b2=(4*Kp+4*Kd*N-2*Ki*Ts-2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4); double v=err-a1*v1-a2*v2; double u=b0*v+b1*v1+b2*v2; v2=v1; v1=v; return u; } void controller() //function for executing controller action { //converting radius and theta to gearbox angle double ref_angle1=16*theta; double ref_angle2=(-radius+minRadius)/pulleyRadius; double angle1 = Encoder1.getPulses()/res; //get number of pulses (counterclockwise is positive) double angle2 = Encoder2.getPulses()/res; //get number of pulses m1_pwm = (PID(ref_angle1-angle1,m1_Kp,m1_Ki,m1_Kd,m1_Ts,m1_N,m1_v1,m1_v2))/V_max; //divide by voltage to get pwm duty cycle percentage) m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max; //limit pwm value and change motor direction when pwm becomes either negative or positive if (m1_pwm >=0.0f && m1_pwm <=1.0f) { motor1dir=0; motor1.write(m1_pwm); } else if (m1_pwm < 0.0f && m1_pwm >= -1.0f) { motor1dir=1; motor1.write(-m1_pwm); } if (m2_pwm >=0.0f && m2_pwm <=1.0f) { motor2dir=0; motor2.write(m2_pwm); } else if (m2_pwm < 0.0f && m2_pwm >= -1.0f) { motor2dir=1; motor2.write(-m2_pwm); } //hidsopce to check what the code does exactly scope.set(0,ref_angle1-angle1); //error scope.set(1,ref_angle1); scope.set(2,m1_pwm); scope.set(3,ref_angle2-angle2); scope.set(4,ref_angle2); scope.set(5,servo_pwm); scope.send(); } void servo_controller() { if (theta < maxTheta ) { servo_pwm=min_servo_pwm+(theta-minTheta)/diffTheta*res_servo; } else { servo_pwm=max_servo_pwm; } servo.pulsewidth(servo_pwm); } void my_pos() { //This function is attached to a ticker so that the reference position is printed every second. pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta); } int main() { pc.printf("RESET\n\r"); pc.baud(115200); //Attach the Biquads to the Biquad chains bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ).add( &bq122 ).add( &bq123 ); bqc13.add( &bq131); bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 ); bqc23.add( &bq231); bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 ); bqc33.add( &bq331); motor1.period(0.02f); //period of pwm signal for motor 1 motor2.period(0.02f); // period of pwm signal for motor 2 motor1dir=0; // setting direction to ccw motor2dir=0; // setting direction to ccw //Attach the 'sample' function to the timer 'sample_timer'. //this ensures that 'sample' is executed every 0.002 seconds = 500 Hz sample_timer.attach(&sampleflag, samplefreq); //Attach the function my_pos to the timer pos_timer. //This ensures that the position is printed every second. pos_timer.attach(&my_pos, 1); control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input servo_control.attach(&activate_servo_control,servo_Ts); while(1) { //Only take a sample when the go flag is true. if (sampletimer==true) { sample(); sampletimer = false; //change sampletimer to false if sample() is finished } if(controller_go) { // go flag controller(); controller_go=false; } if(servo_go) { servo_controller(); servo_go=false; } } }