PID_controler voor het latere script
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp@3:56f31420abac, 2016-11-03 (annotated)
- Committer:
- sivuu
- Date:
- Thu Nov 03 14:31:56 2016 +0000
- Revision:
- 3:56f31420abac
- Parent:
- 2:ca645c585a03
- Child:
- 4:61d9325d7f2e
hopelijk met 2 PID controllers;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sivuu | 0:57aa29917d4c | 1 | #include "mbed.h" |
sivuu | 0:57aa29917d4c | 2 | #include "QEI.h" //bieb voor encoderfuncties in c++ |
sivuu | 0:57aa29917d4c | 3 | #include "MODSERIAL.h" //bieb voor modserial |
sivuu | 0:57aa29917d4c | 4 | #include "BiQuad.h" |
sivuu | 1:faa90344cdd8 | 5 | #include "HIDScope.h" |
sivuu | 1:faa90344cdd8 | 6 | #include "math.h" |
sivuu | 0:57aa29917d4c | 7 | |
sivuu | 3:56f31420abac | 8 | DigitalIn sw3(SW3); |
sivuu | 0:57aa29917d4c | 9 | DigitalIn sw2(SW2); |
sivuu | 0:57aa29917d4c | 10 | DigitalIn encoder1A(D13); |
sivuu | 0:57aa29917d4c | 11 | DigitalIn encoder1B(D12); |
sivuu | 0:57aa29917d4c | 12 | DigitalIn encoder2A(D11); |
sivuu | 0:57aa29917d4c | 13 | DigitalIn encoder2B(D10); |
sivuu | 0:57aa29917d4c | 14 | DigitalIn button_cw(D9); |
sivuu | 0:57aa29917d4c | 15 | DigitalIn button_ccw(PTC12); |
sivuu | 0:57aa29917d4c | 16 | MODSERIAL pc(USBTX, USBRX); |
sivuu | 0:57aa29917d4c | 17 | DigitalOut richting_motor1(D4); |
sivuu | 0:57aa29917d4c | 18 | PwmOut pwm_motor1(D5); |
sivuu | 0:57aa29917d4c | 19 | DigitalOut richting_motor2(D7); |
sivuu | 0:57aa29917d4c | 20 | PwmOut pwm_motor2(D6); |
sivuu | 0:57aa29917d4c | 21 | BiQuad PID_controller; |
sivuu | 3:56f31420abac | 22 | BiQuad PID_controller2; // maakt een biquad aan voor PID controler 2 |
sivuu | 2:ca645c585a03 | 23 | //HIDScope Scope(1); |
sivuu | 0:57aa29917d4c | 24 | |
sivuu | 0:57aa29917d4c | 25 | //constanten voor de encoder |
sivuu | 0:57aa29917d4c | 26 | const int CW = 2.5; //definitie rechtsom 'lage waarde' |
sivuu | 0:57aa29917d4c | 27 | const int CCW =-1; //definitie linksom 'hoge waarde' |
sivuu | 0:57aa29917d4c | 28 | const float gearboxratio=131.25; // gearboxratio van encoder naar motor |
sivuu | 0:57aa29917d4c | 29 | const float rev_rond=64.0; // aantal revoluties per omgang van de encoder |
sivuu | 0:57aa29917d4c | 30 | |
sivuu | 0:57aa29917d4c | 31 | volatile double current_position_motor1 = 0; // current position is 0 op het begin |
sivuu | 0:57aa29917d4c | 32 | volatile double previous_position_motor1 = 0; // previous position is 0 op het begin |
sivuu | 0:57aa29917d4c | 33 | volatile double current_position_motor2 = 0; |
sivuu | 0:57aa29917d4c | 34 | volatile double previous_position_motor2 = 0; |
sivuu | 0:57aa29917d4c | 35 | volatile bool tickerflag; //bool zorgt er voor dat de tickerflag alleen 1 of 0 kan zijn (true or false) |
sivuu | 0:57aa29917d4c | 36 | volatile double snelheid_motor1; // snelheid van motor1 wordt later berekend door waardes uit de encoder is in rad/s |
sivuu | 0:57aa29917d4c | 37 | volatile double snelheid_motor2; // snelheid van motor2 wordt later berekend door waardes uit de encoder is in rad/s |
sivuu | 2:ca645c585a03 | 38 | double ticker_TS=0.001; // zorgt voor een tijdstap van de ticker van 0.1 seconde |
sivuu | 0:57aa29917d4c | 39 | volatile double timepassed=0; //de waarde van hoeveel tijd er verstreken is |
sivuu | 0:57aa29917d4c | 40 | Ticker t; // maakt ticker t aan |
sivuu | 0:57aa29917d4c | 41 | volatile double value1_resetbutton = 0; // deze value wordt gebruikt zodat als er bij de reset button na het bereiken van de waarde nul. De motor stopt met draaien. |
sivuu | 0:57aa29917d4c | 42 | volatile double value2_resetbutton = 0; |
sivuu | 0:57aa29917d4c | 43 | |
sivuu | 0:57aa29917d4c | 44 | |
sivuu | 0:57aa29917d4c | 45 | volatile float d_ref = 0; |
sivuu | 3:56f31420abac | 46 | volatile float d_ref2 = 0; |
sivuu | 0:57aa29917d4c | 47 | volatile float d_ref_new; |
sivuu | 0:57aa29917d4c | 48 | const float w_ref = 3; |
sivuu | 3:56f31420abac | 49 | const float w_ref2 = 4.5; |
sivuu | 2:ca645c585a03 | 50 | const double Ts = 0.001; |
sivuu | 2:ca645c585a03 | 51 | const double Kp = 0.3823; |
sivuu | 2:ca645c585a03 | 52 | const double Ki = 0.127875; |
sivuu | 2:ca645c585a03 | 53 | const double Kd = 0.2519; |
sivuu | 0:57aa29917d4c | 54 | const double N = 100; |
sivuu | 3:56f31420abac | 55 | const double Ts2 = 0.001; |
sivuu | 3:56f31420abac | 56 | const double Kp2 = 0.3823; |
sivuu | 3:56f31420abac | 57 | const double Ki2 = 0.127875; |
sivuu | 3:56f31420abac | 58 | const double Kd2 = 0.2519; |
sivuu | 3:56f31420abac | 59 | const double N2 = 100; |
sivuu | 0:57aa29917d4c | 60 | Ticker tick; |
sivuu | 3:56f31420abac | 61 | Ticker controllerticker; |
sivuu | 3:56f31420abac | 62 | Ticker tick2; // ticker voor reverence voor motor 2 |
sivuu | 3:56f31420abac | 63 | Ticker controllerticker2; //ticker voor controler voor motor 2 |
sivuu | 0:57aa29917d4c | 64 | float rev_counts_motor1_rad; |
sivuu | 3:56f31420abac | 65 | float rev_counts_motor2_rad; |
sivuu | 2:ca645c585a03 | 66 | volatile float voltage_motor1=0.5; |
sivuu | 3:56f31420abac | 67 | volatile float voltage_motor2=0.5; // pwm motor 2 aansturing |
sivuu | 0:57aa29917d4c | 68 | volatile double error1; |
sivuu | 3:56f31420abac | 69 | volatile double error2; // error voor tweede motor |
sivuu | 3:56f31420abac | 70 | volatile double ctrlOutput1; |
sivuu | 3:56f31420abac | 71 | volatile double ctrlOutput2; // control output voor motor 2 |
sivuu | 2:ca645c585a03 | 72 | volatile double d_ref_const_cw = 0; |
sivuu | 2:ca645c585a03 | 73 | volatile double d_ref_const_ccw = 0; |
sivuu | 3:56f31420abac | 74 | volatile int n = 0; |
sivuu | 2:ca645c585a03 | 75 | |
sivuu | 3:56f31420abac | 76 | void SwitchN() { // maakt simpele functie die 1 bij n optelt voor de switch naar motor 2 |
sivuu | 3:56f31420abac | 77 | n++; |
sivuu | 3:56f31420abac | 78 | } |
sivuu | 3:56f31420abac | 79 | |
sivuu | 0:57aa29917d4c | 80 | |
sivuu | 0:57aa29917d4c | 81 | void reference(){ |
sivuu | 3:56f31420abac | 82 | if(n%2== 0){ |
sivuu | 3:56f31420abac | 83 | if (button_cw == 0){ |
sivuu | 3:56f31420abac | 84 | d_ref = d_ref + w_ref * Ts; |
sivuu | 3:56f31420abac | 85 | } |
sivuu | 3:56f31420abac | 86 | if (d_ref > 21.36){ |
sivuu | 3:56f31420abac | 87 | d_ref = 21.36; |
sivuu | 3:56f31420abac | 88 | //d_ref_const_cw = 1; |
sivuu | 3:56f31420abac | 89 | } |
sivuu | 3:56f31420abac | 90 | else{ |
sivuu | 3:56f31420abac | 91 | d_ref = d_ref; |
sivuu | 3:56f31420abac | 92 | } |
sivuu | 3:56f31420abac | 93 | |
sivuu | 3:56f31420abac | 94 | if (button_ccw == 0){ |
sivuu | 3:56f31420abac | 95 | d_ref = d_ref - w_ref * Ts; |
sivuu | 3:56f31420abac | 96 | } |
sivuu | 3:56f31420abac | 97 | if (d_ref < -21.36){ |
sivuu | 3:56f31420abac | 98 | d_ref = -21.36; |
sivuu | 3:56f31420abac | 99 | |
sivuu | 3:56f31420abac | 100 | } |
sivuu | 3:56f31420abac | 101 | else{ |
sivuu | 3:56f31420abac | 102 | d_ref = d_ref; |
sivuu | 3:56f31420abac | 103 | } |
sivuu | 3:56f31420abac | 104 | } // haakje sluit voor if loop voor n%==0 |
sivuu | 3:56f31420abac | 105 | } |
sivuu | 3:56f31420abac | 106 | |
sivuu | 3:56f31420abac | 107 | void reference2(){ // maakt reference aan voor motor2 |
sivuu | 3:56f31420abac | 108 | if(n%2 != 0){ |
sivuu | 3:56f31420abac | 109 | if (button_cw == 0){ |
sivuu | 3:56f31420abac | 110 | d_ref2 = d_ref2 + w_ref2 * Ts; |
sivuu | 2:ca645c585a03 | 111 | } |
sivuu | 3:56f31420abac | 112 | if (d_ref2 > 21.36){ |
sivuu | 3:56f31420abac | 113 | d_ref2 = 21.36; |
sivuu | 2:ca645c585a03 | 114 | //d_ref_const_cw = 1; |
sivuu | 2:ca645c585a03 | 115 | } |
sivuu | 2:ca645c585a03 | 116 | else{ |
sivuu | 3:56f31420abac | 117 | d_ref2 = d_ref2; |
sivuu | 0:57aa29917d4c | 118 | } |
sivuu | 0:57aa29917d4c | 119 | |
sivuu | 2:ca645c585a03 | 120 | if (button_ccw == 0){ |
sivuu | 3:56f31420abac | 121 | d_ref2 = d_ref2 - w_ref2 * Ts; |
sivuu | 2:ca645c585a03 | 122 | } |
sivuu | 3:56f31420abac | 123 | if (d_ref2 < -21.36){ |
sivuu | 3:56f31420abac | 124 | d_ref2 = -21.36; |
sivuu | 2:ca645c585a03 | 125 | |
sivuu | 2:ca645c585a03 | 126 | } |
sivuu | 2:ca645c585a03 | 127 | else{ |
sivuu | 3:56f31420abac | 128 | d_ref2 = d_ref2; |
sivuu | 2:ca645c585a03 | 129 | } |
sivuu | 3:56f31420abac | 130 | } // haakje sluit voor if loop met n%!=0) |
sivuu | 2:ca645c585a03 | 131 | } |
sivuu | 2:ca645c585a03 | 132 | |
sivuu | 0:57aa29917d4c | 133 | void m1_controller(){ |
sivuu | 0:57aa29917d4c | 134 | error1 = d_ref-rev_counts_motor1_rad; |
sivuu | 3:56f31420abac | 135 | ctrlOutput1 = PID_controller.step(error1); |
sivuu | 2:ca645c585a03 | 136 | } |
sivuu | 3:56f31420abac | 137 | void m2_controller(){ // void voor tweede controller |
sivuu | 3:56f31420abac | 138 | error2 = d_ref2-rev_counts_motor2_rad; |
sivuu | 3:56f31420abac | 139 | ctrlOutput2 = PID_controller.step(error2); |
sivuu | 3:56f31420abac | 140 | } |
sivuu | 1:faa90344cdd8 | 141 | |
sivuu | 3:56f31420abac | 142 | //void tickerfunctie() // maakt een ticker functie aan |
sivuu | 3:56f31420abac | 143 | //{ |
sivuu | 3:56f31420abac | 144 | //tickerflag = 1; // het enige wat deze functie doet is zorgen dat tickerflag 1 is |
sivuu | 3:56f31420abac | 145 | //} |
sivuu | 0:57aa29917d4c | 146 | |
sivuu | 0:57aa29917d4c | 147 | |
sivuu | 0:57aa29917d4c | 148 | int main() |
sivuu | 0:57aa29917d4c | 149 | { |
sivuu | 0:57aa29917d4c | 150 | pc.baud(115200); |
sivuu | 3:56f31420abac | 151 | QEI Encoder2(D10,D11, NC, rev_rond,QEI::X4_ENCODING); |
sivuu | 3:56f31420abac | 152 | QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING); //encoder motor 2 |
sivuu | 2:ca645c585a03 | 153 | int counts_encoder1; //variabele counts aanmaken |
sivuu | 3:56f31420abac | 154 | int counts_encoder2; // variabele counts voor motor2 |
sivuu | 0:57aa29917d4c | 155 | float rev_counts_motor1; |
sivuu | 3:56f31420abac | 156 | float rev_counts_motor2; // variabele coutns voor motor2 |
sivuu | 3:56f31420abac | 157 | //t.attach(&tickerfunctie,ticker_TS); |
sivuu | 3:56f31420abac | 158 | sw3.attach(&SwitchN,Ts); |
sivuu | 2:ca645c585a03 | 159 | tick.attach(&reference,Ts); |
sivuu | 3:56f31420abac | 160 | tick2.attach(&reference2,Ts); // ticker voor reference2 attach |
sivuu | 2:ca645c585a03 | 161 | controllerticker.attach(&m1_controller,Ts); |
sivuu | 3:56f31420abac | 162 | controllerticker2.attach(&m2_controller,Ts); // ticker voor controller2 attach |
sivuu | 3:56f31420abac | 163 | |
sivuu | 3:56f31420abac | 164 | |
sivuu | 2:ca645c585a03 | 165 | PID_controller.PIDF(Kp,Ki,Kd,N,Ts); |
sivuu | 3:56f31420abac | 166 | PID_controller2.PIDF(Kp2,Ki2,Kd2,N2,Ts2); |
sivuu | 0:57aa29917d4c | 167 | |
sivuu | 0:57aa29917d4c | 168 | while (true) { |
sivuu | 2:ca645c585a03 | 169 | // if (button_cw == 0){ |
sivuu | 2:ca645c585a03 | 170 | //if (tickerflag ==1){ |
sivuu | 0:57aa29917d4c | 171 | |
sivuu | 2:ca645c585a03 | 172 | // richting_motor1 = 1; |
sivuu | 2:ca645c585a03 | 173 | // pwm_motor1 = voltage_motor1; |
sivuu | 2:ca645c585a03 | 174 | //pc.printf("pwm_motor is %f\r\n", pwm_motor1); |
sivuu | 2:ca645c585a03 | 175 | |
sivuu | 2:ca645c585a03 | 176 | counts_encoder1 = Encoder1.getPulses(); // zorgt er voor dat het aantal rondjes geteld worden |
sivuu | 3:56f31420abac | 177 | counts_encoder2 = Encoder2.getPulses(); |
sivuu | 2:ca645c585a03 | 178 | rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond); |
sivuu | 2:ca645c585a03 | 179 | rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; |
sivuu | 3:56f31420abac | 180 | rev_counts_motor2=(float)counts_encoder2/(gearboxratio*rev_rond); |
sivuu | 3:56f31420abac | 181 | rev_counts_motor2_rad=rev_counts_motor2*6.28318530718; |
sivuu | 3:56f31420abac | 182 | voltage_motor1=ctrlOutput1; |
sivuu | 3:56f31420abac | 183 | voltage_motor2=ctrlOutput2; |
sivuu | 2:ca645c585a03 | 184 | if (voltage_motor1<0){ |
sivuu | 2:ca645c585a03 | 185 | richting_motor1 = 0; |
sivuu | 2:ca645c585a03 | 186 | } |
sivuu | 2:ca645c585a03 | 187 | else { |
sivuu | 2:ca645c585a03 | 188 | richting_motor1 = 1; |
sivuu | 2:ca645c585a03 | 189 | } |
sivuu | 3:56f31420abac | 190 | if (voltage_motor2<0){ |
sivuu | 3:56f31420abac | 191 | richting_motor2 = 0; |
sivuu | 3:56f31420abac | 192 | } |
sivuu | 3:56f31420abac | 193 | else { |
sivuu | 3:56f31420abac | 194 | richting_motor2 = 1; |
sivuu | 3:56f31420abac | 195 | } |
sivuu | 2:ca645c585a03 | 196 | pwm_motor1 = fabs(voltage_motor1); |
sivuu | 3:56f31420abac | 197 | pwm_motor2 = fabs(voltage_motor2); |
sivuu | 2:ca645c585a03 | 198 | |
sivuu | 3:56f31420abac | 199 | pc.printf("%f", pwm_motor2.read()); |
sivuu | 3:56f31420abac | 200 | pc.printf(" %f", d_ref2); |
sivuu | 3:56f31420abac | 201 | pc.printf(" %f \r\n", rev_counts_motor2_rad); |
sivuu | 3:56f31420abac | 202 | |
sivuu | 0:57aa29917d4c | 203 | } |
sivuu | 0:57aa29917d4c | 204 | } |