![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
PIDT werkend,2 motoren niet perfect
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Revision 18:e1354199fd48, committed 2016-10-28
- Comitter:
- IngmarLoohuis
- Date:
- Fri Oct 28 08:35:16 2016 +0000
- Parent:
- 17:457dd9a70c7c
- Commit message:
- Nothing
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Oct 26 13:08:37 2016 +0000 +++ b/main.cpp Fri Oct 28 08:35:16 2016 +0000 @@ -57,7 +57,11 @@ //plant variable volatile double motor1; volatile double motor2; - +//max/min angles +const double max_rad_m1 = pi; +const double min_rad_m1 = -0.5*pi; +const double max_rad_m2 = pi; +const double min_rad_m2 = -0.5*pi; //*****************Angles Arms*********************** @@ -121,7 +125,7 @@ // Next task, measure the error and apply the output to the plant void motor1_Controller(double radians_m1) { - double reference_m1 = -1.0*pi; + double reference_m1 = 0.5*pi; volatile double error_m1 = reference_m1 - radians_m1; motor1 = PID1( error_m1,m1_Kp,m1_Ki,m1_Kd,m1_Ts, m1_N, m1_v1, m1_v2 ); } @@ -129,7 +133,7 @@ // Next task, measure the error and apply the output to the plant void motor2_Controller(double radians_m2) { - double reference_m2 = -0.5*pi; + double reference_m2 = 2*pi; volatile double error_m2 = reference_m2 - radians_m2; motor2 = PID1( error_m2,m2_Kp,m2_Ki,m2_Kd,m2_Ts, m2_N, m2_v1, m2_v2 ); @@ -138,11 +142,11 @@ -void control_m1(double motor1) +void control_m1(double motor1,double radians_m1) { if(abs(motor1)>1.0) { - motor1MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN + motor1MagnitudePin=0.2;//MOET NOG TUSSENWAAREN KRIJGEN } else { @@ -155,9 +159,17 @@ else { motor1DirectionPin=1.0; } +if (radians_m1>max_rad_m1) + { + motor1MagnitudePin = 0; + } +else if (radians_m1<min_rad_m1) + { + motor1MagnitudePin = 0; + } } -void control_m2(double motor2) +void control_m2(double motor2,double radians_m2) { if(abs(motor2)>1) { @@ -174,6 +186,14 @@ else { motor2DirectionPin=0.0; } +if (radians_m2>max_rad_m2) + { + motor2MagnitudePin = 0; + } +else if (radians_m2<min_rad_m2) + { + motor2MagnitudePin = 0; + } } @@ -205,7 +225,7 @@ if(fn3_go) { fn3_go = false; - control_m1(motor1); + control_m1(motor1,radians_m1); } if(fn4_go) { @@ -220,7 +240,7 @@ if(fn6_go) { fn6_go = false; - control_m2(motor2); + control_m2(motor2,radians_m2); } }