emg zooi er op
Dependencies: HIDScope MODSERIAL QEI mbed
Fork of Wearealltogheter_goed by
Diff: main.cpp
- Revision:
- 24:bdd74b91abbb
- Parent:
- 23:fdde3e4b9e69
- Child:
- 25:6dcf79346c40
--- a/main.cpp Tue Oct 25 10:47:41 2016 +0000 +++ b/main.cpp Thu Oct 27 08:12:25 2016 +0000 @@ -2,10 +2,22 @@ #include "HIDScope.h" #include "MODSERIAL.h" #include "QEI.h" +#include "PID.h" +#include "BiQuad.h" DigitalOut led_g(LED_GREEN); DigitalOut led_b(LED_BLUE); DigitalOut led_r(LED_RED); +/* +#define RATEM2 0.001f // transmission during translation +#define RATEM3 1 // transmission during rotation +#define Kc 0.30 +#define Ti 0.00 +#define Td 0.00 + +PID M3_Rotation_Controller(KC, Ti, Td, RATEM3); +PID M2_Translation_Controller(KC, Ti, Td, RATEM2); +*/ DigitalOut M1_Rotate(D2); // voltage only base rotation PwmOut M1_Speed(D3); // voltage only base rotation @@ -37,7 +49,7 @@ bool draailinks; bool turn = 0; float waiter = 0.1; -float afstand = 200; +float afstand = -200; float translation = 0; float degrees3 = 0; @@ -78,6 +90,23 @@ double highpassFilterRight = 0; double lowpassFilterRight = 0; +/*/ copied from slides +const double Ts = 0.01; +const double Kp = 1.0, Ki = 0.5, Kd = 0.1; +const double N = 25; // N = 1/Tf + +BiQuad pidf; +// AnalogIn reference( A0 ); +Ticker controllerTicker; + +void controller() +{ + double ctrlOutputTranslation = pidf.step(GetTranslationM2()); + double ctrlOutputRotation = pidf.step( GetRotationM3()); + //double ctrlOutput = pidf.step( reference.read() ); +} + end copy*/ + double biquad1(double u, double&v1, double&v2, const double a1, const double a2, const double b0, const double b1, const double b2) { @@ -87,16 +116,7 @@ v1 = v; return y; } -/*double biquad2(double u, double&v1, double&v2, const double c1, const double c2, const double d0, - const double d1, const double d2) -{ - double v = u - c1*v1 - c2*v2; - double y = d0*v + d1*v1 + d2*v2; - v2 = v1; - v1 = v; - return y; -} -*/ + /** Sample function * this function samples the emg and sends it to HIDScope **/ @@ -117,9 +137,10 @@ scope.send(); //pc.printf("%f \n \r ", lowpassFilter); } + void sample() { - /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ + // Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' scope.set(0, emg0.read() ); scope.set(1, emg1.read() ); /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) @@ -131,10 +152,11 @@ x_prev = x; // Prepare for next round scope.send(); - /* To indicate that the function is working, the LED is toggled */ + // To indicate that the function is working, the LED is toggled led = !led; pc.printf("%f, %f \n \r ", x, b); } + void GetDirections() { pc.baud(115200); @@ -217,14 +239,13 @@ M3_Speed = 0; turn = 0; - +} -} void Burgerflip() { if (GetPositionM2() > afstand) { M3_Speed = 0.2; - M3_Rotate = 0; + M3_Rotate = 1; M2_Speed = 0; } else if (GetPositionM2() < afstand) { M2_Speed = 1; @@ -237,10 +258,16 @@ } int main() { + //Leds led_g = 1; led_b = 1; led_r = 1; + /* + //PID + pidf.PIDF( Kp, Ki, Kd, N, Ts ); + controllerTicker.attach( &controller, Ts ); + */ /**Attach the 'sample' function to the timer 'sample_timer'. * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz */ @@ -254,7 +281,7 @@ calibrate = true; threshold_Left = lowpassFilterLeft*0.7; threshold_Right = lowpassFilterRight*0.7; - + } if (calibrate == true) { pc.printf("calibration complete, left = %f, right = %f \n \r", threshold_Left, threshold_Right);