This is the final code
Dependencies: HIDScope MODSERIAL QEI mbed
Fork of WearealltogetherEMGboi by
Diff: main.cpp
- Revision:
- 27:16327d1337cf
- Parent:
- 26:c640851fa1e7
- Child:
- 28:d265c64d2bca
--- a/main.cpp Fri Oct 28 08:29:18 2016 +0000 +++ b/main.cpp Fri Oct 28 10:37:21 2016 +0000 @@ -56,6 +56,7 @@ double threshold_Right= 0; Ticker sample_timer; Ticker sample_timer2; +Ticker printinfo; HIDScope scope( 2 ); DigitalOut led(LED1); const double a1 = -1.6475; @@ -78,23 +79,23 @@ double lowpassFilterRight = 0; //setpoints -const double setpoint_Translation = -200; -const double setpoint_Back = 0; -const double setpoint_Rotation = 135; +const double Setpoint_Translation = -200; +const double Setpoint_Back = 0; +const double Setpoint_Rotation = pi; double M3_ControlSpeed = 0; double M2_ControlSpeed = 0; -double setpointError_Translation; -double setpointError_Rotation; +double SetpointError_Translation = 0; +double SetpointError_Rotation = 0; //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; +const double Translation_Kp = 6.9240, Translation_Ki = 3, Translation_Kd = 0.4080; double Translation_error = 0; double Translation_e_prev = 0; //Spatel PID -const double Rotation_Kp = , Rotation_Ki = , Rotation_Kd = ; +const double Rotation_Kp = 0.0499, Rotation_Ki = 0.4299 , Rotation_Kd = 0.0429; double Rotation_error = 0; double Rotation_e_prev = 0; @@ -120,24 +121,7 @@ /** 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)); @@ -231,35 +215,80 @@ float degrees3 = (pulses3/Puls_degree); float radians3 = (degrees3/360)*2*pi; - return degrees3; + return radians3; +} +void motorRotation(double setpoint) +{ + double theta_rotation = GetRotationM3(); + SetpointError_Rotation = setpoint - theta_rotation; + + //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.25) { + M3_ControlSpeed = 0; + } + M3_Speed = M3_ControlSpeed; + +} +void motorTranslation(double setpoint) +{ + double theta_translation = GetPositionM2(); + SetpointError_Translation = setpoint - theta_translation; + + //set direction + if (SetpointError_Translation < 0) { + M2_Rotate = 0; + } else { + M2_Rotate = 1; + + } + M2_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev)); + if (M2_ControlSpeed < 0.25) { + M2_ControlSpeed = 0; + } + M2_Speed = M2_ControlSpeed; + } void GoBack() { - while (GetPositionM2() < 0) { - M3_Speed = 0; - M2_Speed = 1; - M2_Rotate = 1; - pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2()); - led_r = 0; - } - M2_Speed = 0; + motorRotation(Setpoint_Back); + led_r = 1; + led_b = 0; + /* while (GetPositionM2() < 0) { + M3_Speed = 0; + M2_Speed = 1; + M2_Rotate = 1; + pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2()); + led_r = 0; + } + M2_Speed = 0; - while (GetRotationM3() > 0) { - M3_Rotate = 1; - M3_Speed = 0.2; - led_r = 1; - led_b = 0; - pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2()); + while (GetRotationM3() > 0) { + M3_Rotate = 1; + M3_Speed = 0.2; + led_r = 1; + led_b = 0; + pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2()); - } - M3_Speed = 0; + } + M3_Speed = 0; - turn = 0; + turn = 0; */ } void Burgerflip() { + led_r = 0; + led_b = 1; + motorTranslation(Setpoint_Translation); + /* pc.printf("get position %f, get rotation %f \n \r", GetPositionM2(), GetRotationM3()); if (GetPositionM2()< afstand) { M3_Speed = 0.2; @@ -273,6 +302,11 @@ if (GetRotationM3() > setpoint_Rotation) { GoBack(); } + */ +} +void print() +{ + pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2()); } int main() { @@ -287,6 +321,7 @@ //sample_timer.attach(&sample, 0.001953125); sample_timer2.attach(&filterSampleLeft, Ts); //512 Hz sample_timer.attach(&filterSampleRight, Ts); + //printinfo.attach(&print, Ts); pc.baud(115200); pc.printf("please push the button to calibrate \n \r"); while (1) { @@ -298,6 +333,7 @@ } if (calibrate == true) { //pc.printf("calibration complete, left = %f, right = %f \n \r", threshold_Left, threshold_Right); + pc.printf("rotation is %f error = %f en translation = %f en de error %f \n \r", GetRotationM3(), SetpointError_Rotation, GetPositionM2(), SetpointError_Translation); GetDirections(); if (draairechts == true) { M1_Speed = 0.2;