PID_controler voor het latere script
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 3:56f31420abac
- Parent:
- 2:ca645c585a03
- Child:
- 4:61d9325d7f2e
diff -r ca645c585a03 -r 56f31420abac main.cpp --- a/main.cpp Wed Nov 02 16:58:55 2016 +0000 +++ b/main.cpp Thu Nov 03 14:31:56 2016 +0000 @@ -5,7 +5,7 @@ #include "HIDScope.h" #include "math.h" -InterruptIn sw3(SW3); +DigitalIn sw3(SW3); DigitalIn sw2(SW2); DigitalIn encoder1A(D13); DigitalIn encoder1B(D12); @@ -19,6 +19,7 @@ DigitalOut richting_motor2(D7); PwmOut pwm_motor2(D6); BiQuad PID_controller; +BiQuad PID_controller2; // maakt een biquad aan voor PID controler 2 //HIDScope Scope(1); //constanten voor de encoder @@ -42,69 +43,127 @@ volatile float d_ref = 0; +volatile float d_ref2 = 0; volatile float d_ref_new; const float w_ref = 3; +const float w_ref2 = 4.5; const double Ts = 0.001; const double Kp = 0.3823; const double Ki = 0.127875; const double Kd = 0.2519; const double N = 100; +const double Ts2 = 0.001; +const double Kp2 = 0.3823; +const double Ki2 = 0.127875; +const double Kd2 = 0.2519; +const double N2 = 100; Ticker tick; -Ticker controllerticker; +Ticker controllerticker; +Ticker tick2; // ticker voor reverence voor motor 2 +Ticker controllerticker2; //ticker voor controler voor motor 2 float rev_counts_motor1_rad; +float rev_counts_motor2_rad; volatile float voltage_motor1=0.5; +volatile float voltage_motor2=0.5; // pwm motor 2 aansturing volatile double error1; -volatile double ctrlOutput; +volatile double error2; // error voor tweede motor +volatile double ctrlOutput1; +volatile double ctrlOutput2; // control output voor motor 2 volatile double d_ref_const_cw = 0; volatile double d_ref_const_ccw = 0; +volatile int n = 0; +void SwitchN() { // maakt simpele functie die 1 bij n optelt voor de switch naar motor 2 + n++; + } + void reference(){ - if (button_cw == 0 ){ - d_ref = d_ref + w_ref * Ts; + if(n%2== 0){ + if (button_cw == 0){ + d_ref = d_ref + w_ref * Ts; + } + if (d_ref > 21.36){ + d_ref = 21.36; + //d_ref_const_cw = 1; + } + else{ + d_ref = d_ref; + } + + if (button_ccw == 0){ + d_ref = d_ref - w_ref * Ts; + } + if (d_ref < -21.36){ + d_ref = -21.36; + + } + else{ + d_ref = d_ref; + } + } // haakje sluit voor if loop voor n%==0 +} + +void reference2(){ // maakt reference aan voor motor2 +if(n%2 != 0){ + if (button_cw == 0){ + d_ref2 = d_ref2 + w_ref2 * Ts; } - if (d_ref > 21.36 ){ - d_ref = 21.36; + if (d_ref2 > 21.36){ + d_ref2 = 21.36; //d_ref_const_cw = 1; } else{ - d_ref = d_ref; + d_ref2 = d_ref2; } if (button_ccw == 0){ - d_ref = d_ref - w_ref * Ts; + d_ref2 = d_ref2 - w_ref2 * Ts; } - if (d_ref < -21.36){ - d_ref = -21.36; + if (d_ref2 < -21.36){ + d_ref2 = -21.36; } else{ - d_ref = d_ref; + d_ref2 = d_ref2; } - + } // haakje sluit voor if loop met n%!=0) } void m1_controller(){ error1 = d_ref-rev_counts_motor1_rad; - ctrlOutput = PID_controller.step(error1); + ctrlOutput1 = PID_controller.step(error1); } +void m2_controller(){ // void voor tweede controller + error2 = d_ref2-rev_counts_motor2_rad; + ctrlOutput2 = PID_controller.step(error2); + } -void tickerfunctie() // maakt een ticker functie aan -{ -tickerflag = 1; // het enige wat deze functie doet is zorgen dat tickerflag 1 is -} +//void tickerfunctie() // maakt een ticker functie aan +//{ +//tickerflag = 1; // het enige wat deze functie doet is zorgen dat tickerflag 1 is +//} int main() { pc.baud(115200); -QEI Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING); +QEI Encoder2(D10,D11, NC, rev_rond,QEI::X4_ENCODING); +QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING); //encoder motor 2 int counts_encoder1; //variabele counts aanmaken +int counts_encoder2; // variabele counts voor motor2 float rev_counts_motor1; -t.attach(&tickerfunctie,ticker_TS); +float rev_counts_motor2; // variabele coutns voor motor2 +//t.attach(&tickerfunctie,ticker_TS); +sw3.attach(&SwitchN,Ts); tick.attach(&reference,Ts); +tick2.attach(&reference2,Ts); // ticker voor reference2 attach controllerticker.attach(&m1_controller,Ts); +controllerticker2.attach(&m2_controller,Ts); // ticker voor controller2 attach + + PID_controller.PIDF(Kp,Ki,Kd,N,Ts); +PID_controller2.PIDF(Kp2,Ki2,Kd2,N2,Ts2); while (true) { // if (button_cw == 0){ @@ -115,48 +174,31 @@ //pc.printf("pwm_motor is %f\r\n", pwm_motor1); counts_encoder1 = Encoder1.getPulses(); // zorgt er voor dat het aantal rondjes geteld worden + counts_encoder2 = Encoder2.getPulses(); rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond); rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; - voltage_motor1=ctrlOutput; + rev_counts_motor2=(float)counts_encoder2/(gearboxratio*rev_rond); + rev_counts_motor2_rad=rev_counts_motor2*6.28318530718; + voltage_motor1=ctrlOutput1; + voltage_motor2=ctrlOutput2; if (voltage_motor1<0){ richting_motor1 = 0; } else { richting_motor1 = 1; } + if (voltage_motor2<0){ + richting_motor2 = 0; + } + else { + richting_motor2 = 1; + } pwm_motor1 = fabs(voltage_motor1); - - pc.printf("%f", pwm_motor1.read()); - pc.printf(" %f", d_ref); - pc.printf(" %f \r\n", rev_counts_motor1_rad); - //Scope.set(0, rev_counts_motor1_rad); - //Scope.send(); - //tickerflag = 0; - // d_ref_const_ccw = 0; - //} - // } - // else if (button_ccw == 0){ - - /* richting_motor1 = 0; - pwm_motor1 = voltage_motor1; - //pc.printf("pwm_motor is %f\r\n", pwm_motor1); + pwm_motor2 = fabs(voltage_motor2); - counts_encoder1 = Encoder1.getPulses(); // zorgt er voor dat het aantal rondjes geteld worden - rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond); - rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; - voltage_motor1=ctrlOutput; - pc.printf("%f", pwm_motor1); - pc.printf(" %f", d_ref); - pc.printf(" %f \r\n", rev_counts_motor1_rad); - //Scope.set(0, rev_counts_motor1_rad); - //Scope.send(); - //tickerflag = 0; - d_ref_const_cw = 0; */ - // } - - // else{ - // pwm_motor1=0; - - //} + pc.printf("%f", pwm_motor2.read()); + pc.printf(" %f", d_ref2); + pc.printf(" %f \r\n", rev_counts_motor2_rad); + } } \ No newline at end of file