This is the final code
Dependencies: HIDScope MODSERIAL QEI mbed
Fork of WearealltogetherEMGboi by
Diff: main.cpp
- Revision:
- 26:c640851fa1e7
- Parent:
- 25:6dcf79346c40
- Child:
- 27:16327d1337cf
--- a/main.cpp Thu Oct 27 09:10:32 2016 +0000 +++ b/main.cpp Fri Oct 28 08:29:18 2016 +0000 @@ -2,22 +2,10 @@ #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 @@ -49,7 +37,6 @@ bool draailinks; bool turn = 0; float waiter = 0.1; -float afstand = -200; float translation = 0; float degrees3 = 0; @@ -90,22 +77,35 @@ 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 +//setpoints +const double setpoint_Translation = -200; +const double setpoint_Back = 0; +const double setpoint_Rotation = 135; +double M3_ControlSpeed = 0; +double M2_ControlSpeed = 0; +double setpointError_Translation; +double setpointError_Rotation; -BiQuad pidf; -// AnalogIn reference( A0 ); -Ticker controllerTicker; +//copied from slides +//Arm PID +const double Ts = 0.001953125; //Ts=1/fs (sample frequency) +const double Translation_Kp = 6.9240, Translation_Ki = 0.8160, Translation_Kd = 0.4080; +double Translation_error = 0; +double Translation_e_prev = 0; -void controller() +//Spatel PID +const double Rotation_Kp = , Rotation_Ki = , Rotation_Kd = ; +double Rotation_error = 0; +double Rotation_e_prev = 0; + +double pid_control(double error, const double kp, const double ki, const double kd, double &e_int, double &e_prev) { - double ctrlOutputTranslation = pidf.step(GetTranslationM2()); - double ctrlOutputRotation = pidf.step( GetRotationM3()); - //double ctrlOutput = pidf.step( reference.read() ); + double e_der = (error - e_prev) / Ts; + e_prev = error; + e_int = e_int + (Ts * error); + + return kp*error + ki + e_int + kd + e_der; } - 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) @@ -120,7 +120,24 @@ /** Sample function * this function samples the emg and sends it to HIDScope **/ +void motorRotation(double setpoint) +{ + theta = GetRotationM3(); + setpointError_Rotation = setpoint - theta; + //set direction + if (setpointError_Rotation >0) { + M3_Rotate = 0; + } else { + M3_Rotate = 1; + } + M3_ControlSpeed = (Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev)); + if (M3_ControlSpeed < 0.02) { + M3_ControlSpeed = 0; + } + M3_Speed = M3_ControlSpeed; + +} void filterSampleLeft() { highpassFilterLeft = fabs(biquad1(emg0.read(), v1_high, v2_high, a1, a2, b0, b1, b2)); @@ -253,7 +270,7 @@ M2_Rotate = 0; } - if (GetRotationM3() > 150) { + if (GetRotationM3() > setpoint_Rotation) { GoBack(); } } @@ -264,17 +281,12 @@ 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 */ //sample_timer.attach(&sample, 0.001953125); - sample_timer2.attach(&filterSampleLeft, 0.001953125); //512 Hz - sample_timer.attach(&filterSampleRight, 0.001953125); + sample_timer2.attach(&filterSampleLeft, Ts); //512 Hz + sample_timer.attach(&filterSampleRight, Ts); pc.baud(115200); pc.printf("please push the button to calibrate \n \r"); while (1) {