Voor het testen van de motorsnelheid
Dependencies: Encoder MODSERIAL mbed
main.cpp@0:0cd34994e6d6, 2014-10-31 (annotated)
- Committer:
- JKleinRot
- Date:
- Fri Oct 31 19:04:28 2014 +0000
- Revision:
- 0:0cd34994e6d6
Voor het testen van de motorsnelheid
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JKleinRot | 0:0cd34994e6d6 | 1 | #include "mbed.h" |
JKleinRot | 0:0cd34994e6d6 | 2 | #include "encoder.h" |
JKleinRot | 0:0cd34994e6d6 | 3 | #include "MODSERIAL.h" |
JKleinRot | 0:0cd34994e6d6 | 4 | |
JKleinRot | 0:0cd34994e6d6 | 5 | #define SAMPLETIME_REGELAAR 0.005 |
JKleinRot | 0:0cd34994e6d6 | 6 | #define KP_arm2 0.05 //Factor proprotionele regelaar arm 2 |
JKleinRot | 0:0cd34994e6d6 | 7 | #define KI_arm2 0 //Factor integraal regelaar arm 2 |
JKleinRot | 0:0cd34994e6d6 | 8 | #define KD_arm2 0 //Factor afgeleide regelaar arm 2 |
JKleinRot | 0:0cd34994e6d6 | 9 | #define A_TTTT 10049 |
JKleinRot | 0:0cd34994e6d6 | 10 | float t = 0; |
JKleinRot | 0:0cd34994e6d6 | 11 | |
JKleinRot | 0:0cd34994e6d6 | 12 | PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2 |
JKleinRot | 0:0cd34994e6d6 | 13 | DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2 |
JKleinRot | 0:0cd34994e6d6 | 14 | Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit) |
JKleinRot | 0:0cd34994e6d6 | 15 | |
JKleinRot | 0:0cd34994e6d6 | 16 | Ticker ticker_motor_arm2_pid; |
JKleinRot | 0:0cd34994e6d6 | 17 | Ticker ticker_regelaar; |
JKleinRot | 0:0cd34994e6d6 | 18 | MODSERIAL pc(USBTX, USBRX); |
JKleinRot | 0:0cd34994e6d6 | 19 | |
JKleinRot | 0:0cd34994e6d6 | 20 | float pwm_to_motor_arm2; //PWM output naar motor arm 2 |
JKleinRot | 0:0cd34994e6d6 | 21 | float error_arm2_kalibratie; //Error in pulsen arm 2 |
JKleinRot | 0:0cd34994e6d6 | 22 | float vorige_error_arm2 = 0; //Derivative actie van regelaar arm 2 |
JKleinRot | 0:0cd34994e6d6 | 23 | float integraal_arm2 = 0; //Integraal actie van regelaar arm 2 |
JKleinRot | 0:0cd34994e6d6 | 24 | float afgeleide_arm2; //Afgeleide actie van regelaar arm 2 |
JKleinRot | 0:0cd34994e6d6 | 25 | float referentie_arm2 = 0; |
JKleinRot | 0:0cd34994e6d6 | 26 | |
JKleinRot | 0:0cd34994e6d6 | 27 | |
JKleinRot | 0:0cd34994e6d6 | 28 | volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma |
JKleinRot | 0:0cd34994e6d6 | 29 | void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true |
JKleinRot | 0:0cd34994e6d6 | 30 | { |
JKleinRot | 0:0cd34994e6d6 | 31 | regelaar_ticker_flag = true; |
JKleinRot | 0:0cd34994e6d6 | 32 | } |
JKleinRot | 0:0cd34994e6d6 | 33 | |
JKleinRot | 0:0cd34994e6d6 | 34 | void keep_in_range(float * in, float min, float max) //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt |
JKleinRot | 0:0cd34994e6d6 | 35 | { |
JKleinRot | 0:0cd34994e6d6 | 36 | if (*in < min) { //Als de waarde kleiner is als het minimum wordt de waarde het minimum |
JKleinRot | 0:0cd34994e6d6 | 37 | *in = min; |
JKleinRot | 0:0cd34994e6d6 | 38 | } |
JKleinRot | 0:0cd34994e6d6 | 39 | if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum |
JKleinRot | 0:0cd34994e6d6 | 40 | *in = max; |
JKleinRot | 0:0cd34994e6d6 | 41 | } else { //In alle andere gevallen is de waarde de waarde zelf |
JKleinRot | 0:0cd34994e6d6 | 42 | *in = *in; |
JKleinRot | 0:0cd34994e6d6 | 43 | } |
JKleinRot | 0:0cd34994e6d6 | 44 | } |
JKleinRot | 0:0cd34994e6d6 | 45 | |
JKleinRot | 0:0cd34994e6d6 | 46 | void motor_arm2_pid() |
JKleinRot | 0:0cd34994e6d6 | 47 | { |
JKleinRot | 0:0cd34994e6d6 | 48 | 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 |
JKleinRot | 0:0cd34994e6d6 | 49 | integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar |
JKleinRot | 0:0cd34994e6d6 | 50 | afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar |
JKleinRot | 0:0cd34994e6d6 | 51 | pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2;//Output naar motor na PID regelaar |
JKleinRot | 0:0cd34994e6d6 | 52 | vorige_error_arm2 = error_arm2_kalibratie; |
JKleinRot | 0:0cd34994e6d6 | 53 | keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) |
JKleinRot | 0:0cd34994e6d6 | 54 | |
JKleinRot | 0:0cd34994e6d6 | 55 | if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1 |
JKleinRot | 0:0cd34994e6d6 | 56 | dir_motor_arm2.write(1); |
JKleinRot | 0:0cd34994e6d6 | 57 | } else { //Anders richting nul |
JKleinRot | 0:0cd34994e6d6 | 58 | dir_motor_arm2.write(0); |
JKleinRot | 0:0cd34994e6d6 | 59 | } |
JKleinRot | 0:0cd34994e6d6 | 60 | pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM |
JKleinRot | 0:0cd34994e6d6 | 61 | } |
JKleinRot | 0:0cd34994e6d6 | 62 | |
JKleinRot | 0:0cd34994e6d6 | 63 | int main() |
JKleinRot | 0:0cd34994e6d6 | 64 | { |
JKleinRot | 0:0cd34994e6d6 | 65 | pc.printf("hoi"); |
JKleinRot | 0:0cd34994e6d6 | 66 | ticker_motor_arm2_pid.attach(motor_arm2_pid, SAMPLETIME_REGELAAR); |
JKleinRot | 0:0cd34994e6d6 | 67 | ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR); |
JKleinRot | 0:0cd34994e6d6 | 68 | |
JKleinRot | 0:0cd34994e6d6 | 69 | pc.baud(38400); |
JKleinRot | 0:0cd34994e6d6 | 70 | |
JKleinRot | 0:0cd34994e6d6 | 71 | while(1) { |
JKleinRot | 0:0cd34994e6d6 | 72 | |
JKleinRot | 0:0cd34994e6d6 | 73 | |
JKleinRot | 0:0cd34994e6d6 | 74 | while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel |
JKleinRot | 0:0cd34994e6d6 | 75 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 0:0cd34994e6d6 | 76 | |
JKleinRot | 0:0cd34994e6d6 | 77 | referentie_arm2 = 0.5 * 10049 * t * t; |
JKleinRot | 0:0cd34994e6d6 | 78 | t = t + 0.0165; |
JKleinRot | 0:0cd34994e6d6 | 79 | |
JKleinRot | 0:0cd34994e6d6 | 80 | pc.printf("referentie arm 2 %f ", referentie_arm2); |
JKleinRot | 0:0cd34994e6d6 | 81 | pc.printf("get position %d ", puls_motor_arm2.getPosition()); |
JKleinRot | 0:0cd34994e6d6 | 82 | pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); |
JKleinRot | 0:0cd34994e6d6 | 83 | pc.printf("t is %f\n\r", t); |
JKleinRot | 0:0cd34994e6d6 | 84 | |
JKleinRot | 0:0cd34994e6d6 | 85 | if(referentie_arm2 >= 1902) { |
JKleinRot | 0:0cd34994e6d6 | 86 | while(pwm_to_motor_arm2 != 0) { |
JKleinRot | 0:0cd34994e6d6 | 87 | referentie_arm2 = referentie_arm2 - 90/200; |
JKleinRot | 0:0cd34994e6d6 | 88 | pc.printf("referentie arm 2 %f ", referentie_arm2); |
JKleinRot | 0:0cd34994e6d6 | 89 | pc.printf("get position %d ", puls_motor_arm2.getPosition()); |
JKleinRot | 0:0cd34994e6d6 | 90 | pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); |
JKleinRot | 0:0cd34994e6d6 | 91 | } |
JKleinRot | 0:0cd34994e6d6 | 92 | if(pwm_to_motor_arm2 == 0) { |
JKleinRot | 0:0cd34994e6d6 | 93 | pc.printf("staat stil"); |
JKleinRot | 0:0cd34994e6d6 | 94 | } |
JKleinRot | 0:0cd34994e6d6 | 95 | } |
JKleinRot | 0:0cd34994e6d6 | 96 | } |
JKleinRot | 0:0cd34994e6d6 | 97 | } |