![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Motor programma
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of frdm_Motor_V2_3 by
main.cpp
- Committer:
- Rvs94
- Date:
- 2015-09-29
- Revision:
- 14:baebaef79aa6
- Parent:
- 13:a6770307a5d2
- Child:
- 15:7fbee317af2d
File content as of revision 14:baebaef79aa6:
#include "mbed.h" #include "MODSERIAL.h" #include "HIDScope.h" #include "QEI.h" #include "biquadFilter.h" Serial pc(USBTX, USBRX); // tx, rx DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield) PwmOut motor2speed(D5); AnalogIn potmeter2(A5); QEI Encoder(D3, D2, NC, 128); HIDScope scope(3); Ticker ScopeTime; Ticker myControllerTicker; double Aantal_Degs; double Aantal_pulses; double reference; double position; double m2_ref; void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt { scope.set(0, motor2direction.read()); scope.set(1, motor2speed.read()); scope.set(2, Aantal_Degs); Aantal_Degs = Encoder.getPulses()*360/(0.5*128*131); scope.send(); } // Sample time (motor2-step) const double m2_Ts = 0.01; // Controller gain const double m2_Kp = 0.07,m2_Ki = 0.005, m2_Kd = 0.001; double m2_err_int = 0, m2_prev_err = 0; //Derivative filter coeffs const double BiGain = 0.016955; const double m2_f_a1 = -0.96608908283*BiGain, m2_f_a2 = 0.0*BiGain, m2_f_b0 = 1.0*BiGain, m2_f_b1 = 1.0*BiGain, m2_f_b2 = 0.0*BiGain; // Filter variables double m2_f_v1 = 0, m2_f_v2 = 0; // Biquad filter double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 ) { double v = u - a1*v1 - a2*v2; double y = b0*v + b1*v1 + b2*v2; v2 = v1; v1 = v; return y; } // Reusable PID controller double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2) { // Derivative double e_der = (e-e_prev)/Ts; e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2); e_prev = e; // Integral e_int = e_int + Ts*e; // PID return Kp * e + Ki*e_int + Kd*e_der; } // Motor control void motor2_Controller() { reference = m2_ref; position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs double P2 = PID( reference - position, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2); motor2speed = abs(P2); if(P2 > 0) { motor2direction = 0; } else { motor2direction = 1; } } int main() { pc.baud(115200); pc.printf("Tot aan loop werkt\n"); ScopeTime.attach_us(&ScopeSend, 10e4); myControllerTicker.attach( &motor2_Controller, 0.01f ); // 100 Hz while(true) { char c = pc.getc(); if(c == 'r') { m2_ref = m2_ref + 5; } if(c == 'f') { m2_ref = m2_ref - 5; } pc.printf("position = %f aantal degs = %f \n",reference,position); } }