![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Voor het testen van de motorsnelheid
Dependencies: Encoder MODSERIAL mbed
Revision 0:0cd34994e6d6, committed 2014-10-31
- Comitter:
- JKleinRot
- Date:
- Fri Oct 31 19:04:28 2014 +0000
- Commit message:
- Voor het testen van de motorsnelheid
Changed in this revision
diff -r 000000000000 -r 0cd34994e6d6 Encoder.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.lib Fri Oct 31 19:04:28 2014 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/vsluiter/code/Encoder/#18b000b443af
diff -r 000000000000 -r 0cd34994e6d6 MODSERIAL.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Fri Oct 31 19:04:28 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/MODSERIAL/#180e968a751e
diff -r 000000000000 -r 0cd34994e6d6 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Oct 31 19:04:28 2014 +0000 @@ -0,0 +1,97 @@ +#include "mbed.h" +#include "encoder.h" +#include "MODSERIAL.h" + +#define SAMPLETIME_REGELAAR 0.005 +#define KP_arm2 0.05 //Factor proprotionele regelaar arm 2 +#define KI_arm2 0 //Factor integraal regelaar arm 2 +#define KD_arm2 0 //Factor afgeleide regelaar arm 2 + #define A_TTTT 10049 + float t = 0; + +PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2 +DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2 +Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit) + +Ticker ticker_motor_arm2_pid; +Ticker ticker_regelaar; +MODSERIAL pc(USBTX, USBRX); + +float pwm_to_motor_arm2; //PWM output naar motor arm 2 +float error_arm2_kalibratie; //Error in pulsen arm 2 +float vorige_error_arm2 = 0; //Derivative actie van regelaar arm 2 +float integraal_arm2 = 0; //Integraal actie van regelaar arm 2 +float afgeleide_arm2; //Afgeleide actie van regelaar arm 2 +float referentie_arm2 = 0; + + +volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma +void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true +{ + regelaar_ticker_flag = true; +} + +void keep_in_range(float * in, float min, float max) //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt +{ + if (*in < min) { //Als de waarde kleiner is als het minimum wordt de waarde het minimum + *in = min; + } + if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum + *in = max; + } else { //In alle andere gevallen is de waarde de waarde zelf + *in = *in; + } +} + +void motor_arm2_pid() +{ + error_arm2_kalibratie = (referentie_arm2 - puls_motor_arm2.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor + integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar + afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar + pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2;//Output naar motor na PID regelaar + vorige_error_arm2 = error_arm2_kalibratie; + keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) + + if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1 + dir_motor_arm2.write(1); + } else { //Anders richting nul + dir_motor_arm2.write(0); + } + pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM +} + +int main() +{ + pc.printf("hoi"); + ticker_motor_arm2_pid.attach(motor_arm2_pid, SAMPLETIME_REGELAAR); + ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR); + + pc.baud(38400); + + while(1) { + + + while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel + regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan + + referentie_arm2 = 0.5 * 10049 * t * t; + t = t + 0.0165; + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); + pc.printf("t is %f\n\r", t); + + if(referentie_arm2 >= 1902) { + while(pwm_to_motor_arm2 != 0) { + referentie_arm2 = referentie_arm2 - 90/200; + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + } + if(pwm_to_motor_arm2 == 0) { + pc.printf("staat stil"); + } + } + } +} \ No newline at end of file
diff -r 000000000000 -r 0cd34994e6d6 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Oct 31 19:04:28 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89 \ No newline at end of file