Marijke Zondag
final version
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
Project_script_union_final
by 
Jorine Oosting
Diff: main.cpp
- Revision:
- 28:5e54cd4525de
- Parent:
- 27:fa493551be99
- Child:
- 29:6cd4f5ac57c4
--- a/main.cpp Wed Oct 31 12:38:00 2018 +0000
+++ b/main.cpp Thu Nov 01 17:29:06 2018 +0000
@@ -3,6 +3,7 @@
#include "BiQuad.h"
#include "HIDScope.h"
#include <math.h>
+#include "QEI.h"
//ATTENTION: set mBed to version 151
// set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
@@ -14,19 +15,14 @@
AnalogIn emg1_in (A1); //Second raw EMG signal input
AnalogIn emg2_in (A2); //Third raw EMG signal input
-InterruptIn encoderA1 (D9);
-InterruptIn encoderB1 (D8);
-InterruptIn encoderA2 (D12);
-InterruptIn encoderB2 (D13);
-
InterruptIn button1 (D10);
InterruptIn button2 (D11);
-DigitalOut directionpin1 (D7);
-DigitalOut directionpin2 (D4);
+DigitalOut directionpin1 (D4);
+DigitalOut directionpin2 (D7);
-PwmOut pwmpin1 (D6);
-PwmOut pwmpin2 (D5);
+PwmOut pwmpin1 (D5);
+PwmOut pwmpin2 (D6);
DigitalOut ledr (LED_RED);
DigitalOut ledb (LED_BLUE);
@@ -34,11 +30,15 @@
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
//Tickers
-Ticker ticker;
+Ticker func_tick;
+Ticker movag_tick;
+Ticker emg_tick;
//Global variables
const float T = 0.002f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders???
@@ -79,224 +79,66 @@
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
+//Global variables
+const float T = 0.002f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders???
+const float T2 = 0.002f;
+
// 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:
-double q1ref = 0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
-double q2ref = 0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
+double q1ref = 0.0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
+double q2ref = 0.0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals
double v_y; // Desired velocity end effector in y direction --> Determined by EMG signals
double Lq1; // Translatieafstand als gevolg van motor rotation joint 1
double Cq2; // Joint angle of the system (corrected for gear ratio 1:5)
-double q1_dot; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
-double q2_dot; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken
+double q1_dot=0.0; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
+double q2_dot=0.0; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken
-double q1_ii; // Reference signal for motorangle q1ref
-double q2_ii; // Reference signal for motorangle q2ref
+double q1_ii=0.0; // Reference signal for motorangle q1ref
+double q2_ii=0.0; // Reference signal for motorangle q2ref
//Variables PID controller
double PI = 3.14159;
-double Kp1 = 17.5; //Motor 1
+double Kp1 = 20.0; //Motor 1 eerst 17.5 , nu 1
double Ki1 = 1.02;
-double Kd1 = 23.2;
-double encoder1 = 0;
+double Kd1 = 1.0;
double encoder_radians1=0;
+double err_integral1 = 0;
+double err_prev1, err_prev2;
+double err1, err2;
-double Kp2 = 17.5; //Motor 2
+//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 Ki2 = 1.02;
-double Kd2 = 23.2;
-double encoder2 = 0;
+double Kd2 = 1.0;
double encoder_radians2=0;
+double err_integral2 = 0;
+double u1, u2;
-double start_control = 0;
+
+int start_control = 0;
+double emg_cal = 1;
//--------------Functions----------------------------------------------------------------------------------------------------------------------------//
-//------------------ Encoder motor 1 --------------------------------//
-
-void encoderA1_rise()
-{
- if(encoderB1==false)
- {
- encoder1++;
- }
- else
- {
- encoder1--;
- }
-}
-
-void encoderA1_fall()
-{
- if(encoderB1==true)
- {
- encoder1++;
- }
- else
- {
- encoder1--;
- }
-}
-
-void encoderB1_rise()
-{
- if(encoderA1==true)
- {
- encoder1++;
- }
- else
- {
- encoder1--;
- }
-}
-
-void encoderB1_fall()
-{
- if(encoderA1==false)
- {
- encoder1++;
- }
- else
- {
- encoder1--;
- }
-}
-
-void encoder_count1()
-{
- encoderA1.rise(&encoderA1_rise);
- encoderA1.fall(&encoderA1_fall);
- encoderB1.rise(&encoderB1_rise);
- encoderB1.fall(&encoderB1_fall);
-}
-
-//------------------ Encoder motor 2 --------------------------------//
-
-void encoderA2_rise()
-{
- if(encoderB2==false)
- {
- encoder2++;
- }
- else
- {
- encoder2--;
- }
-}
-
-void encoderA2_fall()
-{
- if(encoderB2==true)
- {
- encoder2++;
- }
- else
- {
- encoder2--;
- }
-}
-
-void encoderB2_rise()
-{
- if(encoderA2==true)
- {
- encoder2++;
- }
- else
- {
- encoder2--;
- }
-}
-
-void encoderB2_fall()
-{
- if(encoderA2==false)
- {
- encoder2++;
- }
- else
- {
- encoder2--;
- }
-}
-
-void encoder_count2()
-{
- encoderA2.rise(&encoderA2_rise);
- encoderA2.fall(&encoderA2_fall);
- encoderB2.rise(&encoderB2_rise);
- encoderB2.fall(&encoderB2_fall);
-}
-
//------------------ 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.
@@ -391,6 +233,65 @@
}
}
}
+
+void EMGFilter0()
+{
+ emg0_raw = emg0_in.read(); //give name to raw EMG0 data
+ 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
+ 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
+ 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), klopt nog niet!!
+{
+ 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;
+ //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout.
+}
+
+void emg_filtered() //Call all filter functions
+{
+ EMGFilter0();
+ EMGFilter1();
+ EMGFilter2();
+}
+
/*
void HIDScope_sample()
{
@@ -408,39 +309,142 @@
}
*/
+
+//---------PID controller 1 + 2 + motor control 1 & 2-----------------------------------------------------------//
+void PID_control1()
+{
+ //pc.printf("ik doe het, PDI \n\r");
+
+ // 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 = LowPassFilterDer1.step(err_derivative1);
+ double u_d1 = Kd1 * filtered_err_derivative1;
+ err_prev1 = err1;
+
+
+ // Sum all parts and return it
+ u1 = u_k1 + u_i1 + u_d1;
+}
+
+void PID_control2()
+{
+ //pc.printf("ik doe het, PDI \n\r");
+
+ // 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 = LowPassFilterDer2.step(err_derivative2);
+ double u_d2 = Kd2 * filtered_err_derivative2;
+ err_prev2 = err2;
+
+
+ // 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
+{
+ //pc.printf("ik doe het, engine control 1\n\r");
+ encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0;
+ //pc.printf("encoder1 %f \n\r", (float)encoder1.getPulses());
+ //pc.printf("encoder_radians1 %f \n\r",(float) encoder_radians1);
+ err1 = q1ref - encoder_radians1;
+ //pc.printf("err1 = %f\n\r",err1);
+ PID_control1(); //PID controller function call
+
+ //pc.printf("u1 = %f\n\r",u1);
+
+ //if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600
+ //{
+ pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+ directionpin1.write(u1<0);
+ //}
+ //else
+ // {
+ // pwmpin1 = 0;
+ // }
+}
+
+void engine_control2() //Engine 2 is rotational engine, connected with right side wires
+{
+ encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0;
+ //pc.printf("encoder2 %f \n\r",(float)encoder2.getPulses());
+ //pc.printf("encoder_radians2 %f \n\r",(float)encoder_radians2);
+ err2 = q2ref - encoder_radians2;
+ //pc.printf("err2 = %f\n\r",err2);
+ PID_control2(); //PID controller function call
+ //pc.printf("u2 = %f\n\r",u2);
+
+ //if(encoder2.getPulses()<-5250 && encoder2.getPulses()>5250) //limits rotation, in counts
+ // {
+ pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+ directionpin2.write(u2>0);
+ // }
+ //else
+ // {
+ // pwmpin2 = 0;
+ // }
+}
+
//------------------ Inversed Kinematics --------------------------------//
void inverse_kinematics()
{
+
+ //pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y);
+
Lq1 = q1ref*r_trans;
Cq2 = q2ref/5.0;
- q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2));
- q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2));
+ q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2)); //RKI systeem
+ q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2)); //
- q1_ii = q1ref + q1_dot*T;
- q2_ii = q2ref + q2_dot*T;
+ q1_ii = q1ref + (q1_dot/r_trans)*T; //Omgezet naar motorhoeken
+ q2_ii = q2ref + (q2_dot*5.0)*T;
q1ref = q1_ii;
q2ref = q2_ii;
- start_control = 1;
+
+ //pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref);
+
+
+ //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.
+ 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
{
- v_x = 1.0; //beweging in +x direction
+ v_x = 0.5; //beweging in +x direction
+ v_y = 0.0;
+
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
{
- v_y = 1.0; //beweging in +y direction
+ v_y = 0.5; //beweging in +y direction
+ v_x = 0.0;
+
ledr = 1; //green
ledb = 1;
ledg = 0;
@@ -450,14 +454,17 @@
{
v_x = -v_x;
v_y = -v_y;
+
ledr = 1; //Blue
ledb = 0;
ledg = 1;
}
+
else //If not higher than the threshold, motors will not turn at all
{
v_x = 0;
v_y = 0;
+
ledr = 0; //white
ledb = 0;
ledg = 0;
@@ -465,113 +472,11 @@
inverse_kinematics(); //Call inverse kinematics function
+ break;
}
}
-//---------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 --------------------------//
@@ -586,18 +491,21 @@
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.
+ movag_tick.attach(&MovAg,T);
+ func_tick.attach(&v_des_calculate_qref,T); //v_des determined every T
+ engine_control1_tick.attach(&engine_control1,T);
+ engine_control2_tick.attach(&engine_control2,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
+
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);