Jorine Oosting
RKI aangepast 10:02
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
Project_script_union
by 
Marijke Zondag
Diff: main.cpp
- Revision:
- 30:f04a35f2a06d
- Parent:
- 29:6cd4f5ac57c4
- Child:
- 31:0418ce58af56
--- a/main.cpp Thu Nov 01 17:37:19 2018 +0000
+++ b/main.cpp Thu Nov 01 18:02:30 2018 +0000
@@ -38,10 +38,13 @@
//Tickers
Ticker func_tick;
Ticker movag_tick;
-Ticker emg_tick;
+Ticker emg_tick;
+Ticker print_tick;
+
//Global variables
-const float T = 0.002f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders???
+const float T = 0.002f; //Ticker period EMG, engine control
+const float T2 = 0.2f; //Ticker print function
//EMG filter
double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2
@@ -108,26 +111,22 @@
//Variables PID controller
double PI = 3.14159;
-double Kp1 = 20.0; //Motor 1 eerst 17.5 , nu 1
+double Kp1 = 20.0; //Motor 1
double Ki1 = 1.02;
double Kd1 = 1.0;
double encoder_radians1=0;
double err_integral1 = 0;
double err_prev1, err_prev2;
double err1, err2;
+BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); //sample frequency 500 Hz, cutoff 20Hz low pass
-//BiQuad LowPassFilterDer1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-//BiQuad LowPassFilterDer2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); //sample frequency 500 Hz, cutoff 20Hz low pass
-BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );
-
-
-double Kp2 = 20.0; //Motor 2 eerst 17.5, nu 1
+double Kp2 = 20.0; //Motor 2
double Ki2 = 1.02;
double Kd2 = 1.0;
double encoder_radians2=0;
double err_integral2 = 0;
double u1, u2;
+BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );
int start_control = 0;
@@ -429,53 +428,68 @@
void v_des_calculate_qref()
{
- while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
+ while(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(movAg1>Threshold1 && movAg0<Threshold0) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0)
{
- v_x = 0.5; //beweging in +x direction
+ v_x = 0.5; //movement in +x direction
v_y = 0.0;
- ledr = 0; //red
+ 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(movAg2>Threshold2 && movAg0<Threshold0) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is off (movAg0)
{
- v_y = 0.5; //beweging in +y direction
+ v_y = 0.5; //Movement in +y direction
v_x = 0.0;
- ledr = 1; //green
+ 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(movAg0>Threshold0 && movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is on (movAg0)
{
- v_x = -v_x;
- v_y = -v_y;
+ v_y = 0.0; //Movement in -x direction
+ v_x = -0.5;
- ledr = 1; //Blue
+ ledr = 0; //Purple
ledb = 0;
- ledg = 1;
- }
+ ledg = 1;
+ }
- else //If not higher than the threshold, motors will not turn at all
+ else if(movAg0>Threshold0 && movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is on (movAg0)
+ {
+ v_y = -0.5; //Movement in -y direction
+ v_x = 0.0;
+
+ ledr = 1; //Blue
+ ledb = 0;
+ ledg = 1;
+ }
+ else //If not higher than any threshold, motors will not turn at all
{
v_x = 0;
v_y = 0;
- ledr = 0; //white
+ ledr = 0; //White
ledb = 0;
ledg = 0;
}
- inverse_kinematics(); //Call inverse kinematics function
+ inverse_kinematics(); //Call inverse kinematics function
break;
}
}
+void printFunction()
+{
+ 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);
+}
+
//------------------ Start main function --------------------------//
@@ -484,29 +498,26 @@
int main()
{
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
+ 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 );
- emg_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
+ emg_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
movag_tick.attach(&MovAg,T);
- func_tick.attach(&v_des_calculate_qref,T); //v_des determined every T
+ func_tick.attach(&v_des_calculate_qref,T); //v_des determined every T
+ print_tick.attach(&printFunction,T2);
+ // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
- // 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
+ 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
while(true)
{
- 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);
+ ;
}
}