PID_controler voor het latere script
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp
- Committer:
- sivuu
- Date:
- 2016-11-01
- Revision:
- 0:57aa29917d4c
- Child:
- 1:faa90344cdd8
File content as of revision 0:57aa29917d4c:
#include "mbed.h" #include "QEI.h" //bieb voor encoderfuncties in c++ #include "MODSERIAL.h" //bieb voor modserial #include "BiQuad.h" InterruptIn 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; //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.025; // 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_ref_new; const float w_ref = 3; const double Fs = 0.001; const double Kp = 5; const double Ki = 5; const double Kd = 5; const double N = 100; Ticker tick; Ticker controllerticker; float rev_counts_motor1_rad; volatile float voltage_motor1=0.18; volatile double error1; volatile double ctrlOutput; void reference(){ d_ref = d_ref + w_ref * Fs; } void m1_controller(){ error1 = d_ref-rev_counts_motor1_rad; ctrlOutput = PID_controller.step(error1); // if (ctrlOutput > 1){ // ctrlOutput =1; // } // if (ctrlOutput < 0.01){ // ctrlOutput = 0.01; // } // else{ // ctrlOutput = ctrlOutput; // } } int main() { pc.baud(115200); QEI Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING); float counts_encoder1; //variabele counts aanmaken float rev_counts_motor1; while (true) { if (button_cw == 0){ richting_motor1 = 1; pwm_motor1 = voltage_motor1; counts_encoder1 = Encoder1.getPulses(); // zorgt er voor dat het aantal rondjes geteld worden rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; tick.attach(&reference,Fs); PID_controller.PIDF(Kp,Ki,Kd,N,Fs); controllerticker.attach(&m1_controller,Fs); voltage_motor1=ctrlOutput; pc.printf("%f", voltage_motor1); pc.printf(" %f", d_ref); pc.printf(" %f \r\n", rev_counts_motor1_rad); } else{ pwm_motor1=0; } } }