Motor programma
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of frdm_Motor_V2_3 by
Diff: main.cpp
- Revision:
- 26:cd1db85359aa
- Parent:
- 25:ae908de29943
- Child:
- 27:3392f03bfada
--- a/main.cpp Sat Oct 03 18:04:16 2015 +0000 +++ b/main.cpp Mon Oct 05 16:58:58 2015 +0000 @@ -32,6 +32,8 @@ Ticker ScopeTime; Ticker myControllerTicker2; Ticker myControllerTicker1; + Ticker myEMG1; + Ticker myEMG2; // Constants double reference2, reference1; @@ -40,18 +42,19 @@ int count = 0; double Grens2 = 90, Grens1 = 90; double Stapgrootte = 5; + double Threshold; //Sample time (motor-step) const double m2_Ts = 0.01, m1_Ts = 0.01; //Controller gain Motor 2 & 1 - const double m2_Kp = 5,m2_Ki = 0.05, m2_Kd = 2; - const double m1_Kp = 5,m1_Ki = 0.05, m1_Kd = 2; + const double m2_Kp = 5,m2_Ki = 0.01, m2_Kd = 20; + const double m1_Kp = 5,m1_Ki = 0.01, m1_Kd = 20; double m2_err_int = 0, m2_prev_err = 0; double m1_err_int = 0, m1_prev_err = 0; //Derivative filter coeffs Motor 2 & 1 - const double BiGain2 = 0.016955, BiGain1 = 0.016955; + const double BiGain2 = 0.012, BiGain1 = 0.016955; const double m2_f_a1 = -0.96608908283*BiGain2, m2_f_a2 = 0.0*BiGain2, m2_f_b0 = 1.0*BiGain2, m2_f_b1 = 1.0*BiGain2, m2_f_b2 = 0.0*BiGain2; const double m1_f_a1 = -0.96608908283*BiGain1, m1_f_a2 = 0.0*BiGain1, m1_f_b0 = 1.0*BiGain1, m1_f_b1 = 1.0*BiGain1, m1_f_b2 = 0.0*BiGain1; @@ -97,6 +100,13 @@ // PID return Kp * e + Ki*e_int + Kd*e_der; } + +//--------------------------------------------------------------------------------------------------------------------------// +//EMG functions +//--------------------------------------------------------------------------------------------------------------------------// + +// Hier komen functies die de EMG signalen uitlezen en filteren + //--------------------------------------------------------------------------------------------------------------------------// // Motor control functions //--------------------------------------------------------------------------------------------------------------------------// @@ -164,6 +174,7 @@ ScopeTime.attach(&ScopeSend, 0.01f); // 100 Hz, Scope myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2 myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1 + // 2 Tickers die de waarden van de EMG-signalen uitlezen //--------------------------------------------------------------------------------------------------------------------------// // Control Program @@ -172,7 +183,7 @@ { char c = pc.getc(); // 1 Program UP - if(c == 'e') + if(c == 'e') // If ((EMG1 => Threshold) && (EMG2 => Threshold)) { count = count + 1; if(count > 2) @@ -182,7 +193,7 @@ } // 1 Program DOWN - if(c == 'd') + if(c == 'd') // Hoe gaat dit aangestuurd worden? { count = count - 1; if(count < 0) @@ -196,7 +207,7 @@ LedR = LedB = 1; LedG = 0; - if(c == 'r') + if(c == 'r') // if ((EMG1 => Threshold) && (EMG2 =< Threshold)) { m2_ref = m2_ref + Stapgrootte; m1_ref = m1_ref - Stapgrootte; @@ -206,7 +217,7 @@ m1_ref = -1*Grens1; } } - if(c == 'f') + if(c == 'f') // if ((EMG2 => Threshold) && (EMG1 =< Threshold)) { m2_ref = m2_ref - Stapgrootte; m1_ref = m1_ref + Stapgrootte; @@ -222,7 +233,7 @@ { LedG = LedB = 1; LedR = 0; - if(c == 't') + if(c == 't') // if ((EMG1 => Threshold) && (EMG2 =< Threshold)) { m1_ref = m1_ref + Stapgrootte; if (m1_ref > Grens1) @@ -230,7 +241,7 @@ m1_ref = Grens1; } } - if(c == 'g') + if(c == 'g') // if ((EMG1 => Threshold) && (EMG2 =< Threshold)) { m1_ref = m1_ref - Stapgrootte; if (m1_ref < -1*Grens1) @@ -248,7 +259,8 @@ //VUUUUR!! (To Do) wait(1); m2_ref = 0; - m1_ref = 0; + m1_ref = 0; + count = 0; } }