Robot
Dependencies: HIDScope MODSERIAL Motordriver QEI Servo mbed
Fork of The_Claw_with_EMG_Control_PID by
Revision 17:18e9df406502, committed 2016-11-07
- Comitter:
- meikefrok
- Date:
- Mon Nov 07 09:38:21 2016 +0000
- Parent:
- 16:3c9a3ff09765
- Commit message:
- Met PID en calibrate
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Nov 03 16:50:49 2016 +0000 +++ b/main.cpp Mon Nov 07 09:38:21 2016 +0000 @@ -265,11 +265,78 @@ return y; } +int t = 0; +double value_right = 0; +double value_left = 0; + +double calibrate_right(){ + for(t=0; t<200; t++){ + if(value_right <= rlf_y){ + value_right = rlf_y; + }else{ + value_right = value_right +0.0 ; + } + } + return value_right; +} + +double calibrate_left(){ + for(t=0; t<200; t++){ + if(value_left <= llf_y){ + value_left = llf_y; + }else{ + value_left = value_left +0.0 ; + } + } + return value_left; +} + + /* function that calculates the filtered EMG signal from the raw EMG signal. So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal. After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like. The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/ +//==========PID================ +//setpoints +const double Setpoint_Translation = -200; +const double Setpoint_Back = 0; +const double Setpoint_Rotation = 0; +double M3_ControlSpeed = 0; +double M2_ControlSpeed = 0; +double SetpointError_Translation = 0; +double SetpointError_Rotation = 0; + +//booleans for control +bool booltranslate = false; +bool boolrotate = false; + + + double setpoint; + double Arm_ControlSpeed = 0; + double Cart_ControlSpeed = 0; + +//Cart PID +const double Ts = 0.001953125; //Ts=1/fs (sample frequency) +const double Translation_Kp = 6.9, Translation_Ki = 0.8, Translation_Kd = 0.4; +double Translation_error = 0; +double Translation_e_prev = 0; + +//Arm PID +const double Rotation_Kp = 0.0499, Rotation_Ki = 0.0429 , Rotation_Kd = 0.0429; +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 e_der = (error - e_prev) / Ts; + e_prev = error; + e_int = e_int + (Ts * error); + + return kp*error + ki + e_int + kd + e_der; +} + + //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> //======== Functions and main ============================================================== @@ -282,12 +349,14 @@ lno2_y = biquad_lno2(lno_y, lno2_v1, lno2_v2, lno2_a1, lno2_a2, lno2_b0, lno2_b1, lno2_b2); lhf_y = biquad_lhf(lno2_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2); lrect_y = fabs(lhf_y); - llf_y = biquad_llf(lrect_y, llf_v1, llf_v2, llf_a1, llf_a2, llf_b0, llf_b1, llf_b2)/0.2; + llf_y = biquad_llf(lrect_y, llf_v1, llf_v2, llf_a1, llf_a2, llf_b0, llf_b1, llf_b2)/value_left; + rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2); rno2_y = biquad_rno2(rno_y, rno2_v1, rno2_v2, rno2_a1, rno2_a2, rno2_b0, rno2_b1, rno2_b2); rhf_y = biquad_rhf(rno2_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2); rrect_y = fabs(rhf_y); - rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/0.2; + rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/value_right; + scope.set(1, llf_y); scope.set(0, rlf_y); scope.send(); @@ -375,7 +444,23 @@ } } - + SetpointError_Translation = setpoint - position_cart; + + //set direction + if (SetpointError_Translation < 0) { + cart_speed = 0; + } else {} + + + Cart_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev)); + if (fabs(SetpointError_Translation) < fabs(Setpoint_Translation*0.05)) { + cart_speed = 0; + + } + else{ + cart_speed = Cart_ControlSpeed; + } + // controle LED led_g = not LedOn; @@ -400,7 +485,7 @@ if (rlf_y > threshold_value_claw) { - if(position_cart > -170 && position_arm >= 95){ //If the cart is not at the end, the arm can't move any further than 45 degrees + if(position_cart > -170 && position_arm >= 90){ //If the cart is not at the end, the arm can't move any further than 45 degrees Arm.stop(1)==1; }else if(position_cart > -200 && position_arm >= 60 && position_claw == 27){ @@ -409,9 +494,6 @@ }else if(position_cart<= -170 && position_arm>=160){ //If the cart is at the right end, the arm can't move any further than 70 degrees Arm.stop(1)==1; - }else if(position_cart <= -170 && position_arm >= 95){ - Arm.speed(0.075)==0.075; - }else{ Arm.speed(arm_speed)==arm_speed; } @@ -421,7 +503,7 @@ } }else if (llf_y > threshold_value_claw) { - if(position_cart < 170 && position_arm <= -95){ //If the cart is not at the end, the arm can't move any further than 45 degrees + if(position_cart < 170 && position_arm <= -90){ //If the cart is not at the end, the arm can't move any further than 45 degrees Arm.stop(1)==1; }else if(position_cart < 170 && position_arm <= -60 && position_claw == -18){ @@ -453,6 +535,25 @@ } } } + + { + + SetpointError_Rotation = setpoint - position_arm; + + //set direction + if (SetpointError_Rotation > 0) { + arm_speed = 0; + } else {} + + Arm_ControlSpeed = Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev)); + if (fabs(SetpointError_Rotation) < fabs(Setpoint_Rotation*0.05)) { + Arm_ControlSpeed = 0; + }else{ + arm_speed = Arm_ControlSpeed; + } +} + + // controle LED led_r = not LedOn; led_b = not LedOn;