PID_controler voor het latere script
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp@2:ca645c585a03, 2016-11-02 (annotated)
- Committer:
- sivuu
- Date:
- Wed Nov 02 16:58:55 2016 +0000
- Revision:
- 2:ca645c585a03
- Parent:
- 1:faa90344cdd8
- Child:
- 3:56f31420abac
PID control, works! With limits both ways, one motor
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 | 0:57aa29917d4c | 8 | InterruptIn 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 | 2:ca645c585a03 | 22 | //HIDScope Scope(1); |
sivuu | 0:57aa29917d4c | 23 | |
sivuu | 0:57aa29917d4c | 24 | //constanten voor de encoder |
sivuu | 0:57aa29917d4c | 25 | const int CW = 2.5; //definitie rechtsom 'lage waarde' |
sivuu | 0:57aa29917d4c | 26 | const int CCW =-1; //definitie linksom 'hoge waarde' |
sivuu | 0:57aa29917d4c | 27 | const float gearboxratio=131.25; // gearboxratio van encoder naar motor |
sivuu | 0:57aa29917d4c | 28 | const float rev_rond=64.0; // aantal revoluties per omgang van de encoder |
sivuu | 0:57aa29917d4c | 29 | |
sivuu | 0:57aa29917d4c | 30 | volatile double current_position_motor1 = 0; // current position is 0 op het begin |
sivuu | 0:57aa29917d4c | 31 | volatile double previous_position_motor1 = 0; // previous position is 0 op het begin |
sivuu | 0:57aa29917d4c | 32 | volatile double current_position_motor2 = 0; |
sivuu | 0:57aa29917d4c | 33 | volatile double previous_position_motor2 = 0; |
sivuu | 0:57aa29917d4c | 34 | volatile bool tickerflag; //bool zorgt er voor dat de tickerflag alleen 1 of 0 kan zijn (true or false) |
sivuu | 0:57aa29917d4c | 35 | volatile double snelheid_motor1; // snelheid van motor1 wordt later berekend door waardes uit de encoder is in rad/s |
sivuu | 0:57aa29917d4c | 36 | volatile double snelheid_motor2; // snelheid van motor2 wordt later berekend door waardes uit de encoder is in rad/s |
sivuu | 2:ca645c585a03 | 37 | double ticker_TS=0.001; // zorgt voor een tijdstap van de ticker van 0.1 seconde |
sivuu | 0:57aa29917d4c | 38 | volatile double timepassed=0; //de waarde van hoeveel tijd er verstreken is |
sivuu | 0:57aa29917d4c | 39 | Ticker t; // maakt ticker t aan |
sivuu | 0:57aa29917d4c | 40 | 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 | 41 | volatile double value2_resetbutton = 0; |
sivuu | 0:57aa29917d4c | 42 | |
sivuu | 0:57aa29917d4c | 43 | |
sivuu | 0:57aa29917d4c | 44 | volatile float d_ref = 0; |
sivuu | 0:57aa29917d4c | 45 | volatile float d_ref_new; |
sivuu | 0:57aa29917d4c | 46 | const float w_ref = 3; |
sivuu | 2:ca645c585a03 | 47 | const double Ts = 0.001; |
sivuu | 2:ca645c585a03 | 48 | const double Kp = 0.3823; |
sivuu | 2:ca645c585a03 | 49 | const double Ki = 0.127875; |
sivuu | 2:ca645c585a03 | 50 | const double Kd = 0.2519; |
sivuu | 0:57aa29917d4c | 51 | const double N = 100; |
sivuu | 0:57aa29917d4c | 52 | Ticker tick; |
sivuu | 0:57aa29917d4c | 53 | Ticker controllerticker; |
sivuu | 0:57aa29917d4c | 54 | float rev_counts_motor1_rad; |
sivuu | 2:ca645c585a03 | 55 | volatile float voltage_motor1=0.5; |
sivuu | 0:57aa29917d4c | 56 | volatile double error1; |
sivuu | 0:57aa29917d4c | 57 | volatile double ctrlOutput; |
sivuu | 2:ca645c585a03 | 58 | volatile double d_ref_const_cw = 0; |
sivuu | 2:ca645c585a03 | 59 | volatile double d_ref_const_ccw = 0; |
sivuu | 2:ca645c585a03 | 60 | |
sivuu | 0:57aa29917d4c | 61 | |
sivuu | 0:57aa29917d4c | 62 | void reference(){ |
sivuu | 2:ca645c585a03 | 63 | if (button_cw == 0 ){ |
sivuu | 2:ca645c585a03 | 64 | d_ref = d_ref + w_ref * Ts; |
sivuu | 2:ca645c585a03 | 65 | } |
sivuu | 2:ca645c585a03 | 66 | if (d_ref > 21.36 ){ |
sivuu | 2:ca645c585a03 | 67 | d_ref = 21.36; |
sivuu | 2:ca645c585a03 | 68 | //d_ref_const_cw = 1; |
sivuu | 2:ca645c585a03 | 69 | } |
sivuu | 2:ca645c585a03 | 70 | else{ |
sivuu | 2:ca645c585a03 | 71 | d_ref = d_ref; |
sivuu | 0:57aa29917d4c | 72 | } |
sivuu | 0:57aa29917d4c | 73 | |
sivuu | 2:ca645c585a03 | 74 | if (button_ccw == 0){ |
sivuu | 2:ca645c585a03 | 75 | d_ref = d_ref - w_ref * Ts; |
sivuu | 2:ca645c585a03 | 76 | } |
sivuu | 2:ca645c585a03 | 77 | if (d_ref < -21.36){ |
sivuu | 2:ca645c585a03 | 78 | d_ref = -21.36; |
sivuu | 2:ca645c585a03 | 79 | |
sivuu | 2:ca645c585a03 | 80 | } |
sivuu | 2:ca645c585a03 | 81 | else{ |
sivuu | 2:ca645c585a03 | 82 | d_ref = d_ref; |
sivuu | 2:ca645c585a03 | 83 | } |
sivuu | 2:ca645c585a03 | 84 | |
sivuu | 2:ca645c585a03 | 85 | } |
sivuu | 2:ca645c585a03 | 86 | |
sivuu | 0:57aa29917d4c | 87 | void m1_controller(){ |
sivuu | 0:57aa29917d4c | 88 | error1 = d_ref-rev_counts_motor1_rad; |
sivuu | 0:57aa29917d4c | 89 | ctrlOutput = PID_controller.step(error1); |
sivuu | 2:ca645c585a03 | 90 | } |
sivuu | 1:faa90344cdd8 | 91 | |
sivuu | 2:ca645c585a03 | 92 | void tickerfunctie() // maakt een ticker functie aan |
sivuu | 2:ca645c585a03 | 93 | { |
sivuu | 2:ca645c585a03 | 94 | tickerflag = 1; // het enige wat deze functie doet is zorgen dat tickerflag 1 is |
sivuu | 2:ca645c585a03 | 95 | } |
sivuu | 0:57aa29917d4c | 96 | |
sivuu | 0:57aa29917d4c | 97 | |
sivuu | 0:57aa29917d4c | 98 | int main() |
sivuu | 0:57aa29917d4c | 99 | { |
sivuu | 0:57aa29917d4c | 100 | pc.baud(115200); |
sivuu | 0:57aa29917d4c | 101 | QEI Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING); |
sivuu | 2:ca645c585a03 | 102 | int counts_encoder1; //variabele counts aanmaken |
sivuu | 0:57aa29917d4c | 103 | float rev_counts_motor1; |
sivuu | 2:ca645c585a03 | 104 | t.attach(&tickerfunctie,ticker_TS); |
sivuu | 2:ca645c585a03 | 105 | tick.attach(&reference,Ts); |
sivuu | 2:ca645c585a03 | 106 | controllerticker.attach(&m1_controller,Ts); |
sivuu | 2:ca645c585a03 | 107 | PID_controller.PIDF(Kp,Ki,Kd,N,Ts); |
sivuu | 0:57aa29917d4c | 108 | |
sivuu | 0:57aa29917d4c | 109 | while (true) { |
sivuu | 2:ca645c585a03 | 110 | // if (button_cw == 0){ |
sivuu | 2:ca645c585a03 | 111 | //if (tickerflag ==1){ |
sivuu | 0:57aa29917d4c | 112 | |
sivuu | 2:ca645c585a03 | 113 | // richting_motor1 = 1; |
sivuu | 2:ca645c585a03 | 114 | // pwm_motor1 = voltage_motor1; |
sivuu | 2:ca645c585a03 | 115 | //pc.printf("pwm_motor is %f\r\n", pwm_motor1); |
sivuu | 2:ca645c585a03 | 116 | |
sivuu | 2:ca645c585a03 | 117 | counts_encoder1 = Encoder1.getPulses(); // zorgt er voor dat het aantal rondjes geteld worden |
sivuu | 2:ca645c585a03 | 118 | rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond); |
sivuu | 2:ca645c585a03 | 119 | rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; |
sivuu | 2:ca645c585a03 | 120 | voltage_motor1=ctrlOutput; |
sivuu | 2:ca645c585a03 | 121 | if (voltage_motor1<0){ |
sivuu | 2:ca645c585a03 | 122 | richting_motor1 = 0; |
sivuu | 2:ca645c585a03 | 123 | } |
sivuu | 2:ca645c585a03 | 124 | else { |
sivuu | 2:ca645c585a03 | 125 | richting_motor1 = 1; |
sivuu | 2:ca645c585a03 | 126 | } |
sivuu | 2:ca645c585a03 | 127 | pwm_motor1 = fabs(voltage_motor1); |
sivuu | 2:ca645c585a03 | 128 | |
sivuu | 2:ca645c585a03 | 129 | pc.printf("%f", pwm_motor1.read()); |
sivuu | 2:ca645c585a03 | 130 | pc.printf(" %f", d_ref); |
sivuu | 2:ca645c585a03 | 131 | pc.printf(" %f \r\n", rev_counts_motor1_rad); |
sivuu | 2:ca645c585a03 | 132 | //Scope.set(0, rev_counts_motor1_rad); |
sivuu | 2:ca645c585a03 | 133 | //Scope.send(); |
sivuu | 2:ca645c585a03 | 134 | //tickerflag = 0; |
sivuu | 2:ca645c585a03 | 135 | // d_ref_const_ccw = 0; |
sivuu | 2:ca645c585a03 | 136 | //} |
sivuu | 2:ca645c585a03 | 137 | // } |
sivuu | 2:ca645c585a03 | 138 | // else if (button_ccw == 0){ |
sivuu | 0:57aa29917d4c | 139 | |
sivuu | 2:ca645c585a03 | 140 | /* richting_motor1 = 0; |
sivuu | 2:ca645c585a03 | 141 | pwm_motor1 = voltage_motor1; |
sivuu | 2:ca645c585a03 | 142 | //pc.printf("pwm_motor is %f\r\n", pwm_motor1); |
sivuu | 2:ca645c585a03 | 143 | |
sivuu | 2:ca645c585a03 | 144 | counts_encoder1 = Encoder1.getPulses(); // zorgt er voor dat het aantal rondjes geteld worden |
sivuu | 2:ca645c585a03 | 145 | rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond); |
sivuu | 2:ca645c585a03 | 146 | rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; |
sivuu | 2:ca645c585a03 | 147 | voltage_motor1=ctrlOutput; |
sivuu | 2:ca645c585a03 | 148 | pc.printf("%f", pwm_motor1); |
sivuu | 2:ca645c585a03 | 149 | pc.printf(" %f", d_ref); |
sivuu | 2:ca645c585a03 | 150 | pc.printf(" %f \r\n", rev_counts_motor1_rad); |
sivuu | 2:ca645c585a03 | 151 | //Scope.set(0, rev_counts_motor1_rad); |
sivuu | 2:ca645c585a03 | 152 | //Scope.send(); |
sivuu | 2:ca645c585a03 | 153 | //tickerflag = 0; |
sivuu | 2:ca645c585a03 | 154 | d_ref_const_cw = 0; */ |
sivuu | 2:ca645c585a03 | 155 | // } |
sivuu | 2:ca645c585a03 | 156 | |
sivuu | 2:ca645c585a03 | 157 | // else{ |
sivuu | 2:ca645c585a03 | 158 | // pwm_motor1=0; |
sivuu | 2:ca645c585a03 | 159 | |
sivuu | 2:ca645c585a03 | 160 | //} |
sivuu | 0:57aa29917d4c | 161 | } |
sivuu | 0:57aa29917d4c | 162 | } |