Janine Klein Rot / Mbed 2 deprecated Motorsnelheid

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?

UserRevisionLine numberNew 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 }