Final Jorine Aangepast
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of Project_script_union_final by
Revision 36:650a9245bc44, committed 2018-11-06
- Comitter:
- JorineOosting
- Date:
- Tue Nov 06 10:47:38 2018 +0000
- Parent:
- 35:63c890ac71ff
- Commit message:
- Final Aangepast Jorine
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Nov 05 16:45:17 2018 +0000 +++ b/main.cpp Tue Nov 06 10:47:38 2018 +0000 @@ -6,14 +6,14 @@ #include "QEI.h" //ATTENTION: set mBed to version 151 -// set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder) +// set QEI to version 0 // set MODSERIAL to version 44 // set HIDScope to version 7 // set biquadFilter to version 7 -AnalogIn emg0_in (A0); //First raw EMG signal input -AnalogIn emg1_in (A1); //Second raw EMG signal input -AnalogIn emg2_in (A2); //Third raw EMG signal input +AnalogIn emg0_in (A0); //First raw EMG signal input: calve muscle +AnalogIn emg1_in (A1); //Second raw EMG signal input: biceps muscle 1 +AnalogIn emg2_in (A2); //Third raw EMG signal input: biceps muscle 2 InterruptIn button1 (D10); InterruptIn button2 (D11); @@ -29,11 +29,11 @@ DigitalOut ledg (LED_GREEN); -MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off +MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off QEI encoder2 (D9, D8, NC, 8400,QEI::X4_ENCODING); QEI encoder1 (D12, D13, NC, 8400,QEI::X4_ENCODING); -//HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered +//HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered //Tickers Ticker func_tick; @@ -43,6 +43,8 @@ //Global variables + +//Ticker frequencies const float T = 0.002f; //Ticker period EMG, engine control const float T2 = 0.2f; //Ticker print function @@ -51,8 +53,8 @@ double emg0_raw, emg1_raw, emg2_raw; double emg0_filt_x, emg1_filt_x, emg2_filt_x; const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated -double sum, sum1, sum2, sum3; //variables used to sum elements in array -double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg +double sum, sum1, sum2, sum3; //Variables used to sum elements in array +double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MovAg double movAg0, movAg1, movAg2; //Outcome of MovAg //Calibration variables @@ -60,7 +62,7 @@ int emg_cal = 0; //If emg_cal is set to 1, motors can begin to work in this code const int sizeCal = 1500; //Size of the dataset used for calibration double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //Arrays to put the dataset of the calibration in -double Mean0,Mean1,Mean2; //Average of maximum contraction +double Mean0,Mean1,Mean2; //Average of maximum contraction: Threshold values double Threshold0, Threshold1, Threshold2; //Biquad //Variables for the biquad band filters @@ -92,8 +94,8 @@ double encoder_radians1=0; //Inital encoder value motor 1 double err_integral1 = 0; //Initial error integral value motor 1 double err_prev1, err_prev2; //Variables called previous error motor 1 and motor 2 -double err1, err2; //Variables called error motor 1 and motor 2 -BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); //Low_pass differential term Sample frequency 500 Hz, cutoff 20Hz low pass +double err1, err2; //Variables called current error motor 1 and motor 2 +BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); //Lowpass differential term: Sample frequency 500 Hz, cutoff 20Hz low pass double Kp2 = 20.0; / //Motor 2 double Ki2 = 1.02; @@ -104,30 +106,25 @@ BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); // Inverse Kinematica variables -//const double L1 = 0.208; // Height table to joint 1 -//const double L2 = 0.288; // Height table to joint 2 -const double L3 = 0.212; // Length arm -//const double L4 = 0.089; // Distance backside base to joint 1 -//const double L5 = 0.030; // Distance from joint 1 to joint 2 -const double r_trans = 0.035; // Calculate translation to shaft rotation +//const double L1 = 0.208; //Height of the base assembly +//const double L2 = 0.288; //Height of joint 2 +const double L3 = 0.212; //Length of the rotating arm +const double r_trans = 0.035; //Radius of translational gear // Variërende variabelen inverse kinematics: -double q1ref = 0.0; // Current motor angle of joint 1 as determined out of reference signal -double q2ref = 0.0; // Current motor angle of joint 2 as determined out of reference signal -double v_x; // Desired velocity of end effector in x direction --> Determined by EMG signals -double v_y; // Desired velocity of end effector in y direction --> Determined by EMG signals - -//double Lq1; // Translational distance due to motor rotation joint 1 -//double Cq2; // Joint angle of the system (corrected for gear ratio 1:5) +double q1ref = 0.0; //Current motor angle of joint 1, initial value = 0 +double q2ref = 0.0; //Current motor angle of joint 2, initial value = 0 +double v_x; //Desired velocity of end effector in x direction --> Determined by EMG signals +double v_y; //Desired velocity of end effector in y direction --> Determined by EMG signals -double q1_dot=0.0; // Required angular velocity of motor 1 to reach v_des -double q2_dot=0.0; // Required angular velocity of motor 2 to reach v_des +double q1_dot=0.0; //Required angular velocity of motor 1 to reach v_des +double q2_dot=0.0; //Required angular velocity of motor 2 to reach v_des -double q1_ii=0.0; // Reference signal for motorangle q1ref -double q2_ii=0.0; // Reference signal for motorangle q2ref +double q1_ii=0.0; //New reference angle for joint 1, becomes new q1ref +double q2_ii=0.0; //New reference angke for joint 2, becomes new q2ref -double q1_motor; -double q2_motor; +double q1_motor; //Reference motor angle 1, input PID control +double q2_motor; //Reference motor angle 2, input PID control //--------------Functions----------------------------------------------------------------------------------------------------------------------------// @@ -189,7 +186,7 @@ wait(0.001f); } Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set - Threshold0 = Mean0*0.5; //Threshold calculation calve = 0.8*mean + Threshold0 = Mean0*0.5; //Threshold calculation calve = 0.5*mean break; //Stop. Threshold is calculated. } case 1: //If calibration state 1: @@ -277,7 +274,7 @@ sum3 += StoreArray2[a]; } - movAg0 = sum1/windowsize; //calculates an average in the array + movAg0 = sum1/windowsize; //Calculates an average in the array movAg1 = sum2/windowsize; movAg2 = sum3/windowsize; } @@ -300,7 +297,7 @@ double u_i1 = Ki1 * err_integral1; //Integral term times the integral // Derivative part - double err_derivative1 = (err1 - err_prev1)/T; //error - previous error /T + double err_derivative1 = (err1 - err_prev1)/T; //Error - previous error /T double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1); //Filter the derivative term for stabilization double u_d1 = Kd1 * filtered_err_derivative1; //Derivative term times the derivative error err_prev1 = err1; //Sets the current error to previous error (remember) @@ -327,22 +324,22 @@ // Sum all parts and return it u2 = u_k2 + u_i2 + u_d2; } -void engine_control1() //Engine 1 is translational engine, connected with left side pins +void engine_control1() //Engine 1 is translational joint, connected with left side pins { encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0; - err1 = q1_motor - encoder_radians1; //Calculate error between desired angle 1 and current angle 1 + err1 = q1_motor - encoder_radians1; //Calculate error between reference angle 1 and current angle 1 PID_control1(); //PID 1 controller function call - pwmpin1 = fabs(u1); //Motor 1 speed set - directionpin1.write(u1<0); //Direction motor 1 set + pwmpin1 = fabs(u1); //Set speed motor 1 + directionpin1.write(u1<0); //Set direction motor 1 } -void engine_control2() //Engine 2 is rotational engine, connected with right side wires +void engine_control2() //Engine 2 is rotational joint, connected with right side wires { encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0; - err2 = q2_motor - encoder_radians2; //Calculate error between desired angle 2 and current angle 2 + err2 = q2_motor - encoder_radians2; //Calculate error between reference angle 2 and current angle 2 PID_control2(); //PID 2 controller function call - pwmpin2 = fabs(u2); //Motor 2 speed set - directionpin2.write(u2>0); //Direction motor 2 set + pwmpin2 = fabs(u2); //Set speed motor 2 + directionpin2.write(u2>0); //Set direction motor 2 } @@ -354,14 +351,14 @@ q1_dot = (v_x*cos(q2ref) + v_y*sin(q2ref))/cos(q2ref); //Calculate desired angular velocity of motor 1 q2_dot = v_y/(L3*cos(q2ref)); //Calculate desired angular velocity of motor 2 - q1_ii = q1ref + q1_dot*T; //Adds the desired angle of motor 1 to the reference angle - q2_ii = q2ref + q2_dot*T; //Adds the desired angle of motor 2 to the reference angle + q1_ii = q1ref + q1_dot*T; //Calculate new reference angle of joint 1, from current angle and desired angular velocity times ticker time + q2_ii = q2ref + q2_dot*T; //Calculate new reference angle of joint 2, from current angle and desired angular velocity times ticker time - q1ref = q1_ii; //Makes new qref - q2ref = q2_ii; //Makes new qref + q1ref = q1_ii; //Replace qref by newly calculated reference angle + q2ref = q2_ii; - q1_motor = -q1ref/r_trans; //Sets the angle at which motor 1 needs to go, with ratio rotation/translation - q2_motor = q2ref*5.0; //Sets the angle at which motor 2 needs to go, scaled by 5 + q1_motor = -q1ref/r_trans; //Calculate reference motor angle 1, corrected for translational joint --> input PID control + q2_motor = q2ref*5.0; //Calculate reference motor angle 2, corrected for gear ratio 1:5 ---> input PID control engine_control1(); //Call engine_control 1 function engine_control2(); //Call engine_control 2 function @@ -374,9 +371,9 @@ { if(movAg1>Threshold1 && movAg0<Threshold0) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0) { + v_y = 0.0; v_x = 0.05; //Movement in +x direction - v_y = 0.0; - + ledr = 0; //Led is red ledb = 1; ledg = 1;