PID_controler voor het latere script
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 0:57aa29917d4c
- Child:
- 1:faa90344cdd8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Nov 01 11:50:51 2016 +0000 @@ -0,0 +1,106 @@ +#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; + +} + } +} \ No newline at end of file