Motor control, feedback, PI controller, BiQuad filter
Dependencies: FastPWM HIDScope MODSERIAL biquadFilter mbed QEI
Diff: main.cpp
- Revision:
- 24:f9dccbc1fc7e
- Parent:
- 22:2a560f0f1671
- Child:
- 25:039e2b6429ad
diff -r 2a560f0f1671 -r f9dccbc1fc7e main.cpp --- a/main.cpp Wed Oct 31 19:42:55 2018 +0000 +++ b/main.cpp Wed Oct 31 21:11:28 2018 +0000 @@ -27,6 +27,7 @@ volatile double referencePosition2 = 0.0; volatile double motorValue1 = 0.01; volatile double motorValue2 = 0.01; +volatile double errorM1 = 0.0; 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; @@ -37,14 +38,14 @@ 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? + referencePosition1 = 8.0*3.14*potMeter1In - 4.0*3.14 ; // Reference value y, scaled to -4 to 4 revolutions return referencePosition1; } double GetReferencePosition2() { double potMeter2In = potMeter2.read(); - referencePosition2 = 4.0*3.14*potMeter2In - 2.0*3.14 ; + referencePosition2 = 8.0*3.14*potMeter2In - 4.0*3.14 ; return referencePosition2; } @@ -108,10 +109,12 @@ // bits for motor 1. Positive value makes motor rotating // clockwise. motorValues outside range are truncated to // within range + // 0 = clockwise motor direction + // 1 = counterclockwise motor direction if (motorValue1 >=0) { + motor1DirectionPin=0; + } else { motor1DirectionPin=1; - } else { - motor1DirectionPin=0; } if (fabs(motorValue1)>1) { motor1MagnitudePin = 1; @@ -126,6 +129,8 @@ // bits for motor 1. Positive value makes motor rotating // clockwise. motorValues outside range are truncated to // within range + // 0 = counterclockwise motor direction + // 1 = clockwise motor direction if (motorValue2 >=0) { motor2DirectionPin=1; } else { @@ -146,6 +151,7 @@ // a simple Feedback controller. Call this from a Ticker. referencePosition1 = GetReferencePosition1(); measuredPosition1 = GetMeasuredPosition1(); + errorM1 = referencePosition1 - measuredPosition1; motorValue1 = FeedbackControl1(referencePosition1 - measuredPosition1); SetMotor1(motorValue1); } @@ -164,10 +170,9 @@ { 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("Measured position 1 = %f \t Measured position 2 = %f \r\n", measuredPosition1, measuredPosition2); + pc.printf("Error M1 = %f \r\n", errorM1); + pc.printf("Motorvalue M1 = %f \r\n", motorValue1); //pc.printf("Proportional gain %f \r\n", Kp); //pc.printf("Integral gain %f \r\n", Ki); //pc.printf("Derivative gain %f \r\n", Kd);