Motor control, feedback, PI controller, BiQuad filter
Dependencies: FastPWM HIDScope MODSERIAL biquadFilter mbed QEI
main.cpp
- Committer:
- s1682253
- Date:
- 2018-10-31
- Revision:
- 21:9ba0ed42ee42
- Parent:
- 20:16373bb9af42
- Child:
- 22:2a560f0f1671
File content as of revision 21:9ba0ed42ee42:
#include "mbed.h" #include "FastPWM.h" #include "MODSERIAL.h" #include "QEI.h" #include <math.h> MODSERIAL pc(USBTX, USBRX); DigitalOut motor1DirectionPin(D7); DigitalOut motor2DirectionPin(D4); FastPWM motor1MagnitudePin(D6); FastPWM motor2MagnitudePin(D5); AnalogIn potMeter1(A4); AnalogIn potMeter2(A5); InterruptIn button2(D3); QEI Encoder1 (D12, D13, NC, 64, QEI::X4_ENCODING); QEI Encoder2 (D10, D11, NC, 64, QEI::X4_ENCODING); //Tickers Ticker MeasureControl1; Ticker MeasureControl2; Ticker print; //Global variables volatile double measuredPosition1 = 0.0; volatile double measuredPosition2 = 0.0; volatile double referencePosition1 = 0.0; volatile double referencePosition2 = 0.0; volatile double motorValue1 = 0.01; volatile double motorValue2 = 0.01; volatile double Kp = 0.34; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot volatile double Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld volatile double Kd = 0.0; volatile double Ts = 0.01; //------------------------------------------------------------------------------ double GetReferencePosition1() { double potMeter1In = potMeter1.read(); referencePosition1 = 4.0*3.14*potMeter1In - 2.0*3.14 ; // Reference value y, scaled to -2 to 2 revolutions (or 0 to 100 pi) WAAROM? return referencePosition1; } double GetReferencePosition2() { double potMeter2In = potMeter2.read(); referencePosition2 = 4.0*3.14*potMeter2In - 2.0*3.14 ; return referencePosition2; } double GetMeasuredPosition1() { int counts1 = Encoder1.getPulses(); double counts1d = counts1*1.0f; measuredPosition1 = ( counts1d / (8400)) * 6.28; // Rotational position in radians return measuredPosition1; } double GetMeasuredPosition2() { int counts2 = Encoder2.getPulses(); double counts2d = counts2*1.0f; measuredPosition2 = ( counts2d / (8400)) * 6.28; return measuredPosition2; } double FeedbackControl1(double Error1) { static double Error_integral1 = 0; static double Error_prev1 = Error1; //static BiQuad LowPassFilter(..., ..., ..., ..., ...) // Proportional part: //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan) double u_k1 = Kp * Error1; // Integral part: Error_integral1 = Error_integral1 + Error1 * Ts; double u_i1 = Ki * Error_integral1; // Derivative part double Error_derivative1 = (Error1 - Error_prev1)/Ts; double u_d1 = Kd * Error_derivative1; Error_prev1 = Error1; // Sum all parts and return it return u_k1 + u_i1 + u_d1; //motorValue } double FeedbackControl2(double Error2) { static double Error_integral2 = 0; static double Error_prev2 = Error2; //static BiQuad LowPassFilter(..., ..., ..., ..., ...) // Proportional part: //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan) double u_k2 = Kp * Error2; // Integral part: Error_integral2 = Error_integral2 + Error2 * Ts; double u_i2 = Ki * Error_integral2; // Derivative part double Error_derivative2 = (Error2 - Error_prev2)/Ts; double u_d2 = Kd * Error_derivative2; Error_prev2 = Error2; // Sum all parts and return it return u_k2 + u_i2 + u_d2; //motorValue } void SetMotor1(double motorValue1) { // Given -1<=motorValue<=1, this sets the PWM and direction // bits for motor 1. Positive value makes motor rotating // clockwise. motorValues outside range are truncated to // within range if (motorValue1 >=0) { motor1DirectionPin=1; } else { motor1DirectionPin=0; } if (fabs(motorValue1)>1) { motor1MagnitudePin = 1; } else { motor1MagnitudePin = fabs(motorValue1); } } void SetMotor2(double motorValue2) { // Given -1<=motorValue<=1, this sets the PWM and direction // bits for motor 1. Positive value makes motor rotating // clockwise. motorValues outside range are truncated to // within range if (motorValue2 >=0) { motor2DirectionPin=1; } else { motor2DirectionPin=0; } if (fabs(motorValue2)>1) { motor2MagnitudePin = 1; } else { motor2MagnitudePin = fabs(motorValue2); } } //----------------------------------------------------------------------------- // Tickers void MeasureAndControl1(void) { // This function determines the desired velocity, measures the // actual velocity, and controls the motor with // a simple Feedback controller. Call this from a Ticker. referencePosition1 = GetReferencePosition1(); measuredPosition1 = GetMeasuredPosition1(); motorValue1 = FeedbackControl1(referencePosition1 - measuredPosition1); SetMotor1(motorValue1); } void MeasureAndControl2(void) { // This function determines the desired velocity, measures the // actual velocity, and controls the motor with // a simple Feedback controller. Call this from a Ticker. referencePosition2 = GetReferencePosition2(); measuredPosition2 = GetMeasuredPosition2(); motorValue2 = FeedbackControl2(referencePosition2 - measuredPosition2); SetMotor2(motorValue2); } void printen() { pc.baud (115200); pc.printf("Referenceposition POT1 = %f \t Referenceposition POT2 = %f \r\n", referencePosition1, referencePosition2); pc.printf("Measured position 1 %f \r\n", measuredPosition1); pc.printf("Measured position 2 %f \r\n", measuredPosition2); pc.printf("Motorvalue M1 = %f \t Motorvalue M2 = %f \r\n", motorValue1, motorValue2); //pc.printf("Proportional gain %f \r\n", Kp); //pc.printf("Integral gain %f \r\n", Ki); //pc.printf("Derivative gain %f \r\n", Kd); } //----------------------------------------------------------------------------- int main() { //Initialize once pc.baud(115200); motor1MagnitudePin.period_us(60.0); // 60 microseconds PWM period: 16.7 kHz. motor2MagnitudePin.period_us(60.0); // 60 microseconds PWM period: 16.7 kHz. MeasureControl1.attach(MeasureAndControl1, 0.01); MeasureControl2.attach(MeasureAndControl2, 0.01); print.attach(printen, 3); //Other initializations while(true) { } }