goede versie met arm omgedraaid
Dependencies: HIDScope MODSERIAL QEI mbed
Fork of Wearealltogheternietgoed by
Revision 28:d265c64d2bca, committed 2016-11-01
- Comitter:
- Frostworks
- Date:
- Tue Nov 01 09:18:30 2016 +0000
- Parent:
- 27:16327d1337cf
- Commit message:
- wouter boyyy
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 16327d1337cf -r d265c64d2bca main.cpp --- a/main.cpp Fri Oct 28 10:37:21 2016 +0000 +++ b/main.cpp Tue Nov 01 09:18:30 2016 +0000 @@ -87,15 +87,18 @@ double SetpointError_Translation = 0; double SetpointError_Rotation = 0; +//booleans for control +bool booltranslate = false; +bool boolrotate = false; //copied from slides //Arm PID const double Ts = 0.001953125; //Ts=1/fs (sample frequency) -const double Translation_Kp = 6.9240, Translation_Ki = 3, Translation_Kd = 0.4080; +const double Translation_Kp = 6.9, Translation_Ki = 0.8, Translation_Kd = 0.4; double Translation_error = 0; double Translation_e_prev = 0; //Spatel PID -const double Rotation_Kp = 0.0499, Rotation_Ki = 0.4299 , Rotation_Kd = 0.0429; +const double Rotation_Kp = 0.0499, Rotation_Ki = 0.0429 , Rotation_Kd = 0.0429; double Rotation_error = 0; double Rotation_e_prev = 0; @@ -230,11 +233,14 @@ } M3_ControlSpeed = Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev)); - if (M3_ControlSpeed < 0.25) { + if (fabs(SetpointError_Rotation) < fabs(Setpoint_Rotation*0.05)) { M3_ControlSpeed = 0; } - M3_Speed = M3_ControlSpeed; - + if (theta_rotation > (Setpoint_Rotation*0.9)) + boolrotate = true; + if ((theta_rotation < (Setpoint_Rotation*0.07) ) && (M3_Speed == 0)) + boolrotate = false; + M3_Speed = M3_ControlSpeed; } void motorTranslation(double setpoint) { @@ -246,20 +252,31 @@ 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) { + if (fabs(SetpointError_Translation) < fabs(Setpoint_Translation*0.05)) { M2_ControlSpeed = 0; + } + if ((theta_translation < Setpoint_Translation*0.95) && (M2_ControlSpeed == 0)) + booltranslate = true; + if ((theta_translation > Setpoint_Translation*0.05) && (M2_ControlSpeed == 0)) + booltranslate = false; M2_Speed = M2_ControlSpeed; - + } void GoBack() { - motorRotation(Setpoint_Back); + motorTranslation(Setpoint_Back); + if (booltranslate == false) { + motorRotation(Setpoint_Back); + } + if (boolrotate == false) { + turn = 0; + } led_r = 1; led_b = 0; + /* while (GetPositionM2() < 0) { M3_Speed = 0; M2_Speed = 1; @@ -288,6 +305,9 @@ led_r = 0; led_b = 1; motorTranslation(Setpoint_Translation); + if (booltranslate == true) { + motorRotation(Setpoint_Rotation); + } /* pc.printf("get position %f, get rotation %f \n \r", GetPositionM2(), GetRotationM3()); if (GetPositionM2()< afstand) { @@ -304,6 +324,13 @@ } */ } +void BurgerflipActie() +{ + Burgerflip(); + if (boolrotate == true) { + GoBack(); + } +} void print() { pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2()); @@ -346,7 +373,7 @@ M2_Rotate = 1; M3_Speed = 0.5; M3_Rotate = 1;*/ - Burgerflip(); + BurgerflipActie(); } else if (turn == 0) { M2_Speed = 0; M3_Speed = 0;