Peter Bartels / Mbed 2 deprecated PIDregelaar_begrenzer

In deze code vinden jullie een PIDregelaar en begrenzer!

Dependencies:   Encoder HIDScope mbed

main.cpp@0:16ee9fc421f8, 2014-10-29 (annotated)

Committer:
phgbartels
Date:
Wed Oct 29 11:13:23 2014 +0000
Revision:
0:16ee9fc421f8
PIDregelaar_begrenzer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
phgbartels 0:16ee9fc421f8 1 #include "mbed.h"
phgbartels 0:16ee9fc421f8 2 #include "encoder.h"
phgbartels 0:16ee9fc421f8 3 #include "HIDScope.h"
phgbartels 0:16ee9fc421f8 4 #include "PwmOut.h"
phgbartels 0:16ee9fc421f8 5 /*definieren pinnen Motor 1*/
phgbartels 0:16ee9fc421f8 6 #define M1_PWM PTA5
phgbartels 0:16ee9fc421f8 7 #define M1_DIR PTA4
phgbartels 0:16ee9fc421f8 8 #define M2_PWM PTC8
phgbartels 0:16ee9fc421f8 9 #define M2_DIR PTC9
phgbartels 0:16ee9fc421f8 10 /*Definieren minimale waarden PWM per motor*/
phgbartels 0:16ee9fc421f8 11 #define PWM1_min_50 0.529 /*met koppelstuk!*/
phgbartels 0:16ee9fc421f8 12 #define PWM2_min_30 0.529 /*aanpassen! Klopt nog niet met koppelstuk*/
phgbartels 0:16ee9fc421f8 13 /*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/
phgbartels 0:16ee9fc421f8 14 #define TSAMP 0.005
phgbartels 0:16ee9fc421f8 15 #define K_P (100)
phgbartels 0:16ee9fc421f8 16 #define K_I (50 * TSAMP)
phgbartels 0:16ee9fc421f8 17 #define K_D (1)
phgbartels 0:16ee9fc421f8 18 //#define K_D (0.0005 /TSAMP)
phgbartels 0:16ee9fc421f8 19 #define I_LIMIT 100.
phgbartels 0:16ee9fc421f8 20 #define PI 3.1415926535897
phgbartels 0:16ee9fc421f8 21 #define lengte_arm 0.5
phgbartels 0:16ee9fc421f8 22
phgbartels 0:16ee9fc421f8 23 Encoder motor1(PTD3,PTD1, true);
phgbartels 0:16ee9fc421f8 24 PwmOut pwm_motor1(M1_PWM);
phgbartels 0:16ee9fc421f8 25 DigitalOut motordir1(M1_DIR);
phgbartels 0:16ee9fc421f8 26 void clamp(float * in, float min, float max);
phgbartels 0:16ee9fc421f8 27 float pid(float setpoint, float measurement);
phgbartels 0:16ee9fc421f8 28 volatile bool looptimerflag;
phgbartels 0:16ee9fc421f8 29 float PWM1_percentage = 0;
phgbartels 0:16ee9fc421f8 30 int PWM2_percentage = 100;
phgbartels 0:16ee9fc421f8 31 int aantal_rotaties_1 = 10;
phgbartels 0:16ee9fc421f8 32 int aantalcr_1 = 1600;
phgbartels 0:16ee9fc421f8 33 int aantalcr_2 = 960;
phgbartels 0:16ee9fc421f8 34 int beginpos_motor1;
phgbartels 0:16ee9fc421f8 35 int beginpos_motor1_new;
phgbartels 0:16ee9fc421f8 36 int beginpos_motor2;
phgbartels 0:16ee9fc421f8 37 int beginpos_motor2_new;
phgbartels 0:16ee9fc421f8 38 uint16_t gewenste_snelheid = 2;
phgbartels 0:16ee9fc421f8 39 uint16_t gewenste_snelheid_rad = 4;
phgbartels 0:16ee9fc421f8 40 int previous_pos_motor1 = 0;
phgbartels 0:16ee9fc421f8 41 int current_pos_motor1;
phgbartels 0:16ee9fc421f8 42 float pos_motor1_rad;
phgbartels 0:16ee9fc421f8 43 int delta_pos_motor1_puls;
phgbartels 0:16ee9fc421f8 44 float delta_pos_motor1_rad;
phgbartels 0:16ee9fc421f8 45 float snelheid_motor1_rad;
phgbartels 0:16ee9fc421f8 46 float snelheid_arm_ms;
phgbartels 0:16ee9fc421f8 47 float PWM1;
phgbartels 0:16ee9fc421f8 48 float Speed_motor1;
phgbartels 0:16ee9fc421f8 49 float Speed_motor1rad;
phgbartels 0:16ee9fc421f8 50
phgbartels 0:16ee9fc421f8 51 void setlooptimerflag(void) //Ticker maakt een constructie die zoveel keer per seconde een functie uitvoerd, met sampingling freq TSAMP
phgbartels 0:16ee9fc421f8 52 {
phgbartels 0:16ee9fc421f8 53 looptimerflag = true;
phgbartels 0:16ee9fc421f8 54 }
phgbartels 0:16ee9fc421f8 55
phgbartels 0:16ee9fc421f8 56 int main()
phgbartels 0:16ee9fc421f8 57 {
phgbartels 0:16ee9fc421f8 58 motor1.setPosition(0);
phgbartels 0:16ee9fc421f8 59 pwm_motor1.period_us(100);
phgbartels 0:16ee9fc421f8 60 float setpoint;
phgbartels 0:16ee9fc421f8 61 float prev_setpoint = 0;
phgbartels 0:16ee9fc421f8 62 Ticker looptimer;
phgbartels 0:16ee9fc421f8 63 looptimer.attach(setlooptimerflag,TSAMP);
phgbartels 0:16ee9fc421f8 64
phgbartels 0:16ee9fc421f8 65 while(true)
phgbartels 0:16ee9fc421f8 66 {
phgbartels 0:16ee9fc421f8 67 while(!looptimerflag) //Hierdoor zeg je eigenlijk tegen je systeem; voer dit script uit na elke TSAMP s.
phgbartels 0:16ee9fc421f8 68 {
phgbartels 0:16ee9fc421f8 69 /*do nothing*/ //Zolang dit waar is, doe je niets.
phgbartels 0:16ee9fc421f8 70 }
phgbartels 0:16ee9fc421f8 71 looptimerflag = false; //clear flag
phgbartels 0:16ee9fc421f8 72
phgbartels 0:16ee9fc421f8 73 current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
phgbartels 0:16ee9fc421f8 74 pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
phgbartels 0:16ee9fc421f8 75
phgbartels 0:16ee9fc421f8 76 //begrenzen van de hoek(in dit geval 90 graden)
phgbartels 0:16ee9fc421f8 77 if (current_pos_motor1 >= 400)
phgbartels 0:16ee9fc421f8 78 gewenste_snelheid_rad = 0;
phgbartels 0:16ee9fc421f8 79 else
phgbartels 0:16ee9fc421f8 80
phgbartels 0:16ee9fc421f8 81 setpoint = prev_setpoint + TSAMP * gewenste_snelheid_rad;
phgbartels 0:16ee9fc421f8 82 PWM1_percentage = pid(setpoint, pos_motor1_rad);
phgbartels 0:16ee9fc421f8 83
phgbartels 0:16ee9fc421f8 84 // zorgen dat PWM1_percentage niet buiten de range van 100% valt
phgbartels 0:16ee9fc421f8 85 if (PWM1_percentage < -100)
phgbartels 0:16ee9fc421f8 86 PWM1_percentage = -100;
phgbartels 0:16ee9fc421f8 87 else if (PWM1_percentage >100)
phgbartels 0:16ee9fc421f8 88 PWM1_percentage =100;
phgbartels 0:16ee9fc421f8 89 else
phgbartels 0:16ee9fc421f8 90
phgbartels 0:16ee9fc421f8 91 // bepalen van de richting
phgbartels 0:16ee9fc421f8 92 if(PWM1_percentage < 0)
phgbartels 0:16ee9fc421f8 93 motordir1 = 1;
phgbartels 0:16ee9fc421f8 94 else
phgbartels 0:16ee9fc421f8 95 motordir1 = 0;
phgbartels 0:16ee9fc421f8 96 // ennnn uitsturen van PWM
phgbartels 0:16ee9fc421f8 97 pwm_motor1.write(abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
phgbartels 0:16ee9fc421f8 98 scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
phgbartels 0:16ee9fc421f8 99 prev_setpoint = setpoint;
phgbartels 0:16ee9fc421f8 100 }
phgbartels 0:16ee9fc421f8 101 }
phgbartels 0:16ee9fc421f8 102
phgbartels 0:16ee9fc421f8 103 void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variebele instaat). Dus je slaat niet de variabele op
phgbartels 0:16ee9fc421f8 104 // maar de locatie van de variabele.
phgbartels 0:16ee9fc421f8 105 {
phgbartels 0:16ee9fc421f8 106 *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c
phgbartels 0:16ee9fc421f8 107 // *in = het getal dat staat op locatie van in --> waarde van new_pwm
phgbartels 0:16ee9fc421f8 108 }
phgbartels 0:16ee9fc421f8 109
phgbartels 0:16ee9fc421f8 110
phgbartels 0:16ee9fc421f8 111 float pid(float setpoint, float measurement)
phgbartels 0:16ee9fc421f8 112 {
phgbartels 0:16ee9fc421f8 113 float error;
phgbartels 0:16ee9fc421f8 114 static float prev_error = 0;
phgbartels 0:16ee9fc421f8 115 float out_p = 0;
phgbartels 0:16ee9fc421f8 116 static float out_i = 0;
phgbartels 0:16ee9fc421f8 117 float out_d = 0;
phgbartels 0:16ee9fc421f8 118 error = (setpoint-measurement);
phgbartels 0:16ee9fc421f8 119 out_p = error*K_P;
phgbartels 0:16ee9fc421f8 120 out_i += error*K_I;
phgbartels 0:16ee9fc421f8 121 out_d = (error-prev_error)*K_D;
phgbartels 0:16ee9fc421f8 122 clamp(&out_i,-I_LIMIT,I_LIMIT);
phgbartels 0:16ee9fc421f8 123 prev_error = error;
phgbartels 0:16ee9fc421f8 124 return out_p + out_i + out_d;
phgbartels 0:16ee9fc421f8 125 }