PID_controler voor het latere script
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp
- Committer:
- sivuu
- Date:
- 2016-11-03
- Revision:
- 3:56f31420abac
- Parent:
- 2:ca645c585a03
- Child:
- 4:61d9325d7f2e
File content as of revision 3:56f31420abac:
#include "mbed.h" #include "QEI.h" //bieb voor encoderfuncties in c++ #include "MODSERIAL.h" //bieb voor modserial #include "BiQuad.h" #include "HIDScope.h" #include "math.h" DigitalIn sw3(SW3); DigitalIn sw2(SW2); DigitalIn encoder1A(D13); DigitalIn encoder1B(D12); DigitalIn encoder2A(D11); DigitalIn encoder2B(D10); DigitalIn button_cw(D9); DigitalIn button_ccw(PTC12); MODSERIAL pc(USBTX, USBRX); DigitalOut richting_motor1(D4); PwmOut pwm_motor1(D5); 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 const int CW = 2.5; //definitie rechtsom 'lage waarde' const int CCW =-1; //definitie linksom 'hoge waarde' const float gearboxratio=131.25; // gearboxratio van encoder naar motor const float rev_rond=64.0; // aantal revoluties per omgang van de encoder volatile double current_position_motor1 = 0; // current position is 0 op het begin volatile double previous_position_motor1 = 0; // previous position is 0 op het begin volatile double current_position_motor2 = 0; volatile double previous_position_motor2 = 0; volatile bool tickerflag; //bool zorgt er voor dat de tickerflag alleen 1 of 0 kan zijn (true or false) volatile double snelheid_motor1; // snelheid van motor1 wordt later berekend door waardes uit de encoder is in rad/s volatile double snelheid_motor2; // snelheid van motor2 wordt later berekend door waardes uit de encoder is in rad/s double ticker_TS=0.001; // zorgt voor een tijdstap van de ticker van 0.1 seconde volatile double timepassed=0; //de waarde van hoeveel tijd er verstreken is Ticker t; // maakt ticker t aan 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. volatile double value2_resetbutton = 0; 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 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 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(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_ref2 > 21.36){ d_ref2 = 21.36; //d_ref_const_cw = 1; } else{ d_ref2 = d_ref2; } if (button_ccw == 0){ d_ref2 = d_ref2 - w_ref2 * Ts; } if (d_ref2 < -21.36){ d_ref2 = -21.36; } else{ d_ref2 = d_ref2; } } // haakje sluit voor if loop met n%!=0) } void m1_controller(){ error1 = d_ref-rev_counts_motor1_rad; 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 //} int main() { pc.baud(115200); 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; 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){ //if (tickerflag ==1){ // richting_motor1 = 1; // pwm_motor1 = voltage_motor1; //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; 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); pwm_motor2 = fabs(voltage_motor2); pc.printf("%f", pwm_motor2.read()); pc.printf(" %f", d_ref2); pc.printf(" %f \r\n", rev_counts_motor2_rad); } }