derivative design
Dependencies: FastPWM HIDScope MODSERIAL QEI mbed
Diff: Der_design.cpp
- Revision:
- 0:588962464765
diff -r 000000000000 -r 588962464765 Der_design.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Der_design.cpp Fri Oct 26 10:41:14 2018 +0000 @@ -0,0 +1,186 @@ +#include "mbed.h" +#include "FastPWM.h" +#include "MODSERIAL.h" +#include "QEI.h" +#include "math.h" +#include "HIDScope.h" + +#define PI 3.14159265 + +// Declare variables +DigitalOut gpo(D0); +DigitalOut led(LED_RED); +AnalogIn pot1(A0); +AnalogIn pot2(A1); +QEI encoder1(D10, D11, NC, 32); +QEI encoder2(D12, D13, NC, 32); +FastPWM motor1_pwm(D6); +DigitalOut motor1_richting(D7); + +// Define the HIDScope and Ticker objects +HIDScope scope(1); +Ticker scopeTimer; + +MODSERIAL pc(USBTX, USBRX); + +int count = 0; + +// Tickers +Ticker MotorTicker; + + +// Functions + +void read_pots(volatile double &pot1_value, volatile double &pot2_value) // read pot 1 en pot 2 +{ + pot1_value = pot1.read(); + pot2_value = pot2.read(); +} + +double det_ref(double ref_value) //zet referentiewaarde om in positie +{ + return ref_value * 2 * 3.1416; +} + +double meas_pos () //Encoder functie +{ + return encoder1.getPulses()/32.0/131.25*2.0*3.1416; +} + +double comp_error (double meas_pos, double ref_pos) +{ + return ref_pos - meas_pos; +} + +double comp_integral_error (double error) // compute the integral error +{ + double T = 1.0 / 400.0; + volatile double error_integral = error_integral + error * T; + + return error_integral; +} + +double comp_derivative_error (double error) +{ + double T = 1.0 / 400.0; + volatile double error_prev; + double error_der = (error - error_prev)/T; + error_prev = error; + + return error_der; +} + +// PID controller +double PID_controller (double pot2_val, double error, double integral_error, double error_der) +{ + + // Proportional control + double u_k = 30.0 * error; //20.0 * pot2_val * error; + + + // Integral control + double u_i = 20.0 * pot2_val * integral_error; + + // Derivative + + double u_d = 30.0 * error_der; + + + //u definieren + double u = u_k; //+ u_d; //+ u_i; + + // Signaal binnen de perken houden. + if (u > 1.0) + { + u = 1.0; + } + + else if (u < -1.0) + { + u = -1.0; + } + + + + + return u; +} + + +//Move motor +void move_mot(double u) +{ + + if (u <= 0) + { + motor1_richting = 0; + motor1_pwm.write(-u); //write Duty cycle + } + + if (u >= 0) + { + motor1_richting = 1; + motor1_pwm.write(u); //write Duty cycle + } +} + +//sinefunction +double sinefunct() +{ + static int t; + double pos_sine = sin((double)t*0.0025*0.5*PI); + t++; + return (pos_sine + 1.0) / 2.0; +} + +// Alles samenvoegen +void Motor_control() +{ + + volatile double pot_value; + + volatile double pot2_val; + + read_pots(pot_value, pot2_val); + + volatile double yref = sinefunct(); // reference position + + volatile double y = meas_pos(); // measured position + + volatile double error = comp_error (y, yref); + + volatile double integral_error = comp_integral_error(error); + + volatile double error_der = comp_derivative_error(error); + + scope.set(0, error_der); + + //scope.set(0, meas_pos()); + + volatile double u = PID_controller(pot2_val, error, integral_error, error_der); // output PID controller + + move_mot(u); //functie die motor laat bewegen. + + count++; + //pc.printf("count = %i", count); + + if (count == 400) // print 1x per seconde waardes. + { + pc.printf("u = %lf, measured position y = %lf, reference position yref = %lf, gain = %lf, error = %lf, integral error = %lf, derivative error = %lf \n\r", u, y, yref, pot2_val*20.0, error, integral_error, error_der); + count = 0; + } +} + + +int main() +{ + pc.baud(115200); + + int frequency_pwm = 16700; //16.7 kHz PWM + motor1_pwm.period(1.0/frequency_pwm); // T = 1/f + MotorTicker.attach(Motor_control, 0.0025); + scopeTimer.attach_us(&scope, &HIDScope::send, 2e4); + + while (true) + {} +} \ No newline at end of file