Marijke Zondag
final version
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
Project_script_union_final
by 
Jorine Oosting
Diff: main.cpp
- Revision:
- 36:650a9245bc44
- Parent:
- 35:63c890ac71ff
- Child:
- 37:c7ca9bc29d20
--- 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;