Jorn-Jan van de Beld
/
kin_PD_project
begin van episch avontuur
Diff: main.cpp
- Revision:
- 4:95bf97b44237
- Parent:
- 3:329acce2487c
- Child:
- 5:21e846902e3e
diff -r 329acce2487c -r 95bf97b44237 main.cpp --- a/main.cpp Tue Oct 31 13:26:52 2017 +0000 +++ b/main.cpp Tue Oct 31 15:41:40 2017 +0000 @@ -163,6 +163,31 @@ } + +void controller(double q1ref, double q2ref) //HIER WAS JE BEZIG@!#!@#!@#@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +{ +pulses1 = q1_enc.getPulses(); +error1_1 = ref1 - pulses1; +error1_I = error1_I2 + ts*((error1_1 - error2_1)/2); + +PI = Kp*error1 + Ki*(error_I); + +error2 = error1; +error_I2 = error_I; + + //Motor control + if (PI<0 ) + { + motor2DirectionPin = 0; + motor2MagnitudePin = fabs(PI); + + } + else if (PI>0) + { + motor2DirectionPin = 1; + motor2MagnitudePin = fabs(PI); + } +} void aansturing() {