Marijke Zondag
werkt nog niet
Dependencies: HIDScope MODSERIAL biquadFilter mbed QEI
Fork of
Project_script_union
by 
Marijke Zondag
Diff: main.cpp
- Revision:
- 28:61d1372349c8
- Parent:
- 27:fa493551be99
- Child:
- 29:df10cb76ef26
diff -r fa493551be99 -r 61d1372349c8 main.cpp
--- a/main.cpp Wed Oct 31 12:38:00 2018 +0000
+++ b/main.cpp Wed Oct 31 19:14:56 2018 +0000
@@ -10,9 +10,8 @@
// 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 potmeter1 (A0); //First raw EMG signal input
+//AnalogIn potmeter2 (A1); //Second raw EMG signal input
InterruptIn encoderA1 (D9);
InterruptIn encoderB1 (D8);
@@ -38,53 +37,20 @@
//HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
//Tickers
-Ticker ticker;
+Ticker func_tick;
+Ticker engine_control1_tick;
+Ticker engine_control2_tick;
//Global variables
const float T = 0.002f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders???
-
-//EMG filter
-double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2
-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. (random number)
-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 movAg0, movAg1, movAg2; //outcome of MovAg (moet dit een array zijn??)
-
-//Calibration variables
-int x = -1; //Start switch, colour LED is blue.
-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, eerst 2000
-double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //arrays to put the dataset of the calibration in
-double Mean0,Mean1,Mean2; //average of maximum tightening
-double Threshold0, Threshold1, Threshold2;
-
-//Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo)
-BiQuadChain emg0filter;
-BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
-BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
-BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
-BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients
-
-BiQuadChain emg1filter;
-BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
-BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
-BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
-BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
-
-BiQuadChain emg2filter;
-BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
-BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
-BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
-BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
+const float T2 = 0.01f;
// Inverse Kinematica variables
const double L1 = 0.208; // Hoogte van tafel tot joint 1
-const double L2 = 0.288; // Hoogte van tafel tot joint 2
+//const double L2 = 0.288; // Hoogte van tafel tot joint 2
const double L3 = 0.212; // Lengte van de arm
const double L4 = 0.089; // Afstand van achterkant base tot joint 1
-const double L5 = 0.030; // Afstand van joint 1 naar joint 2
+//const double L5 = 0.030; // Afstand van joint 1 naar joint 2
const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation
// Variërende variabelen inverse kinematics:
@@ -104,19 +70,23 @@
//Variables PID controller
double PI = 3.14159;
-double Kp1 = 17.5; //Motor 1
+double Kp1 = 5.0; //Motor 1 eerst 17.5 , nu 5
double Ki1 = 1.02;
double Kd1 = 23.2;
double encoder1 = 0;
double encoder_radians1=0;
-double Kp2 = 17.5; //Motor 2
+double Kp2 = 5.0; //Motor 2 eerst 17.5, nu 5
double Ki2 = 1.02;
double Kd2 = 23.2;
double encoder2 = 0;
double encoder_radians2=0;
-double start_control = 0;
+int start_control = 0;
+
+//double potmeter1s = (potmeter1*2)-1.0f;
+//double potmeter2s = (potmeter2*2)-1.0f;
+double emg_cal = 1;
//--------------Functions----------------------------------------------------------------------------------------------------------------------------//
@@ -240,178 +210,128 @@
//------------------ Filter EMG + Calibration EMG --------------------------------//
-void EMGFilter0()
-{
- emg0_raw = emg0_in.read(); //give name to raw EMG0 data calve
- emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data
- emg0_filt = abs(emg0_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken.
-}
-
-void EMGFilter1()
-{
- emg1_raw = emg1_in.read(); //give name to raw EMG1 data bicep 1
- emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data
- emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
-}
-
-void EMGFilter2()
-{
- emg2_raw = emg2_in.read(); //Give name to raw EMG1 data bicep 2
- emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data
- emg2_filt = abs(emg2_filt_x); //Rectifier. LET OP: volgorde filter: band-notch-rectifier.
-}
-
-void MovAg() //Calculate moving average (MovAg)
-{
- for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals
- {
- StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average.
- StoreArray1[i] = StoreArray1[i-1];
- StoreArray2[i] = StoreArray2[i-1];
- }
-
- StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array
- StoreArray1[0] = emg1_filt;
- StoreArray2[0] = emg2_filt;
-
- sum1 = 0.0;
- sum2 = 0.0;
- sum3 = 0.0;
-
- for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays
- {
- sum1 += StoreArray0[a];
- sum2 += StoreArray1[a];
- sum3 += StoreArray2[a];
- }
-
- movAg0 = sum1/windowsize; //calculates an average in the array
- movAg1 = sum2/windowsize;
- movAg2 = sum3/windowsize;
-}
-
-void emg_filtered() //Call all filter functions
-{
- EMGFilter0();
- EMGFilter1();
- EMGFilter2();
- MovAg();
-}
-void switch_to_calibrate()
-{
- x++; //Every time function gets called, x increases. Every button press --> new calibration state.
- //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1.
-
- if(x==0) //If x = 0, led is red
- {
- ledr = 0;
- ledb = 1;
- ledg = 1;
- }
- else if (x==1) //If x = 1, led is blue
- {
- ledr = 1;
- ledb = 0;
- ledg = 1;
- }
- else if (x==2) //If x = 2, led is green
- {
- ledr = 1;
- ledb = 1;
- ledg = 0;
- }
- else //If x = 3 or 4, led is white
- {
- ledr = 0;
- ledb = 0;
- ledg = 0;
- }
-
- if(x==4) //Reset back to x = -1
- {
- x = -1;
- emg_cal=0; //reset, motors off
- }
-}
-
-
-void calibrate(void)
-{
- switch(x)
- {
- case 0: //If calibration state 0:
- {
- sum = 0.0;
- for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0
- {
- StoreCal0[j] = emg0_filt;
- sum+=StoreCal0[j];
- wait(0.001f); //Does there need to be a wait?
- }
- Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples)
- Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean
- break; //Stop. Threshold is calculated, we will use this further in the code
- }
- case 1: //If calibration state 1:
- {
- sum = 0.0;
- for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1
- {
- StoreCal1[j] = emg1_filt;
- sum+=StoreCal1[j];
- wait(0.001f);
- }
- Mean1 = sum/sizeCal;
- Threshold1 = Mean1/2;
- break;
- }
- case 2: //If calibration state 2:
- {
- sum = 0.0;
- for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2
- {
- StoreCal2[j] = emg2_filt;
- sum+=StoreCal2[j];
- wait(0.001f);
- }
- Mean2 = sum/sizeCal;
- Threshold2 = Mean2/2;
- break;
- }
- case 3: //EMG is calibrated, robot can be set to Home position.
- {
- emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!)
-
- wait(0.001f);
- break;
- }
- default: //Ensures nothing happens if x is not 0,1 or 2.
- {
- break;
- }
- }
-}
-/*
-void HIDScope_sample()
-{
- scope.set(0,emg0_raw);
- scope.set(1,emg0_filt);
- scope.set(1,movAg0); //als moving average werkt
- scope.set(2,emg1_raw);
- scope.set(3,emg1_filt);
- scope.set(3,movAg1); //als moving average werkt
- scope.set(4,emg2_raw);
- scope.set(5,emg2_filt);
- scope.set(5,movAg2); //als moving average werkt
-
- scope.send(); //Send data to HIDScope server
-}
-*/
//------------------ Inversed Kinematics --------------------------------//
+
+//---------PID controller motor 1 + start motor 1 -----------------------------------------------------------//
+double PID_controller1(double err1)
+{
+ pc.printf("ik doe het, PDI 1\n\r");
+
+ static double err_integral1 = 0;
+ static double err_prev1 = err1; // initialization with this value only done once!
+
+ static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+
+ // Proportional part:
+ double u_k1 = Kp1 * err1;
+
+ /* Integral part
+ err_integral1 = err_integral1 + err1 * T;
+ double u_i1 = Ki1 * err_integral1;
+
+ // Derivative part
+ double err_derivative1 = (err1 - err_prev1)/T;
+ double filtered_err_derivative1 = LowPassFilter1.step(err_derivative1);
+ double u_d1 = Kd1 * filtered_err_derivative1;
+ err_prev1 = err1;
+ */
+
+ // Sum all parts and return it
+ return u_k1+0; //+ u_i1 + u_d1;
+}
+
+void start_your_engines1(double u1)
+{
+ pc.printf("ik doe het, engine start 1\n\r");
+
+ if(encoder1<5250 && encoder1>-5250) //limits rotation, in counts
+ {
+ pwmpin1 = 1; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+ directionpin1.write(0);
+ }
+ else
+ {
+ pwmpin1 = 0;
+ }
+}
+
+void engine_control1() //Engine 1 is rotational engine, connected with left side pins
+{
+ //while(start_control == 1)
+ //{
+ pc.printf("ik doe het, engine control 1\n\r");
+ encoder_radians1 = encoder1*(2*PI)/8400;
+ double err1 = q1ref - encoder_radians1;
+ double u1 = PID_controller1(err1); //PID controller function call
+ start_your_engines1(u1);
+
+ // break;
+ //}
+}
+
+
+
+//---------PID controller motor 2 + start motor 2 -----------------------------------------------------------//
+double PID_controller2(double err2)
+{
+ pc.printf("ik doe het, PDI 2\n\r");
+
+ static double err_integral2 = 0;
+ static double err_prev2 = err2; // initialization with this value only done once!
+
+ static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+
+ // Proportional part:
+ double u_k2 = Kp2 * err2;
+
+ /* Integral part
+ err_integral2 = err_integral2 + err2 * T;
+ double u_i2 = Ki2 * err_integral2;
+
+ // Derivative part
+ double err_derivative2 = (err2 - err_prev2)/T;
+ double filtered_err_derivative2 = LowPassFilter2.step(err_derivative2);
+ double u_d2 = Kd2 * filtered_err_derivative2;
+ err_prev2 = err2;
+ */
+
+ // Sum all parts and return it
+ return u_k2+0; //+ u_i2 + u_d2;
+}
+
+void start_your_engines2(double u2)
+{
+ pc.printf("ik doe het, engine start 2\n\r");
+
+ if(encoder2<12600 && encoder2>-1) //limits translation in counts
+ {
+ pwmpin2 = 1; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+ directionpin2.write(0);
+ }
+ else
+ {
+ pwmpin2 = 0;
+ }
+
+}
+
+void engine_control2() //Engine 2 is translational engine, connected with right side wires
+{
+ pc.printf("ik doe het, engine control 2\n\r");
+
+ encoder_radians2 = encoder2*(2*PI)/8400;
+ double err2 = q2ref - encoder_radians2;
+ double u2 = PID_controller2(err2); //PID controller function call
+ start_your_engines2(u2); //Call start_your_engines function
+}
+
+
void inverse_kinematics()
{
+
+ pc.printf("ik doe het, inverse kinematics\n\r");
Lq1 = q1ref*r_trans;
Cq2 = q2ref/5.0;
@@ -424,36 +344,37 @@
q1ref = q1_ii;
q2ref = q2_ii;
- start_control = 1;
+ //start_control = 1;
+ engine_control1();
+ engine_control2();
}
void v_des_calculate_qref()
{
- if(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
- {
- if(movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn
+ if(button1==0) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn
{
- v_x = 1.0; //beweging in +x direction
+ v_x = 0.5f; //beweging in +x direction
ledr = 0; //red
ledb = 1;
ledg = 1;
}
- else if(movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn
+ else if(button2==0) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn
{
- v_y = 1.0; //beweging in +y direction
+ v_y = 0.5f; //beweging in +y direction
ledr = 1; //green
ledb = 1;
ledg = 0;
}
-
- else if(movAg0>Threshold0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
+ /*
+ else if(button1==0 && button2==0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
{
- v_x = -v_x;
- v_y = -v_y;
+ v_x = -0.5f;
+ v_y = -0.5f;
ledr = 1; //Blue
ledb = 0;
ledg = 1;
}
+ */
else //If not higher than the threshold, motors will not turn at all
{
v_x = 0;
@@ -461,116 +382,12 @@
ledr = 0; //white
ledb = 0;
ledg = 0;
+ //pwmpin1 = 0;
+ //pwmpin2 = 0;
}
inverse_kinematics(); //Call inverse kinematics function
-
- }
-}
-
-//---------PID controller motor 1 + start motor 1 -----------------------------------------------------------//
-double PID_controller1(double err1)
-{
- static double err_integral1 = 0;
- static double err_prev1 = err1; // initialization with this value only done once!
-
- static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-
- // Proportional part:
- double u_k1 = Kp1 * err1;
-
- // Integral part
- err_integral1 = err_integral1 + err1 * T;
- double u_i1 = Ki1 * err_integral1;
-
- // Derivative part
- double err_derivative1 = (err1 - err_prev1)/T;
- double filtered_err_derivative1 = LowPassFilter1.step(err_derivative1);
- double u_d1 = Kd1 * filtered_err_derivative1;
- err_prev1 = err1;
-
- // Sum all parts and return it
- return u_k1 + u_i1 + u_d1;
-}
-
-void start_your_engines1(double u1)
-{
- if(encoder1<5250 && encoder1>-5250) //limits rotation, in counts
- {
- pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
- directionpin1.write(u1 < 0.0f);
- }
- else
- {
- pwmpin1 = 0;
- }
-}
-
-void engine_control1() //Engine 1 is rotational engine, connected with left side pins
-{
- while(start_control == 1)
- {
- encoder_radians1 = encoder1*(2*PI)/8400;
- double err1 = q1ref - encoder_radians1;
- double u1 = PID_controller1(err1); //PID controller function call
- start_your_engines1(u1); //Call start_your_engines function
-
- break;
- }
-}
-
-
-
-//---------PID controller motor 1 + start motor 1 -----------------------------------------------------------//
-double PID_controller2(double err2)
-{
- static double err_integral2 = 0;
- static double err_prev2 = err2; // initialization with this value only done once!
-
- static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-
- // Proportional part:
- double u_k2 = Kp2 * err2;
-
- // Integral part
- err_integral2 = err_integral2 + err2 * T;
- double u_i2 = Ki2 * err_integral2;
-
- // Derivative part
- double err_derivative2 = (err2 - err_prev2)/T;
- double filtered_err_derivative2 = LowPassFilter2.step(err_derivative2);
- double u_d2 = Kd2 * filtered_err_derivative2;
- err_prev2 = err2;
-
- // Sum all parts and return it
- return u_k2 + u_i2 + u_d2;
-}
-
-void start_your_engines2(double u2)
-{
- if(encoder2<12600 && encoder2>-1) //limits translation in counts
- {
- pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
- directionpin2.write(u2 < 0.0f);
- }
- else
- {
- pwmpin2 = 0;
- }
-
-}
-
-void engine_control2() //Engine 2 is translational engine, connected with right side wires
-{
- while(start_control == 1)
- {
- encoder_radians2 = encoder2*(2*PI)/8400;
- double err2 = q2ref - encoder_radians2;
- double u2 = PID_controller2(err2); //PID controller function call
- start_your_engines2(u2); //Call start_your_engines function
-
- break;
- }
+
}
//------------------ Start main function --------------------------//
@@ -581,26 +398,14 @@
pc.baud(115200);
pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off.
pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
-
- emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( ¬ch1 ); //attach biquad elements to chain
- emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( ¬ch2 );
- emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( ¬ch3 );
+
+ func_tick.attach(&v_des_calculate_qref,T2); //v_des determined every T
+ //engine_control1_tick.attach(&engine_control1,T2);
+ //engine_control2_tick.attach(&engine_control2,T2);
+
+ // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
+
while(true)
- {
- ticker.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
- ticker.attach(&v_des_calculate_qref,T); //v_des determined every T
-
- // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
-
- button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
- wait(0.2f); //Wait to avoid bouncing of button
- button2.rise(calibrate); //Calibrate threshold for 3 muscles
- wait(0.2f); //Wait to avoid bouncing of button
-
- pc.printf("x is %i\n\r",x);
- pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2);
- pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
- //wait(2.0f);
- }
+ {;}
}