Motor control, feedback, PI controller, BiQuad filter
Dependencies: FastPWM HIDScope MODSERIAL biquadFilter mbed QEI
Diff: main.cpp
- Revision:
- 10:b002572e37fd
- Parent:
- 8:ceb9abb5a4a8
- Child:
- 11:4e3ef6150a2e
--- a/main.cpp Mon Oct 15 12:13:42 2018 +0000 +++ b/main.cpp Mon Oct 15 13:53:15 2018 +0000 @@ -7,24 +7,27 @@ AnalogIn potMeter1(A4); InterruptIn button2(D3); FastPWM motor1_pwm(D6); //FastPWM input, PES lecture 2 +QEI Encoder (D12, D13, NC, 64, QEI::X4_ENCODING); Ticker MotorSpeedCounts; -QEI Encoder (D12, D13, NC, 64, QEI::X4_ENCODING); +Ticker measurevelocity; + +//Global variables +volatile double RotationalPosition = 0.0; +volatile double measuredVelocity = 0.0; +const double tickertime = 0.001; void Motor() { // Aflezen Potentiometers voor PWM - float potMeterIn = potMeter1.read(); // Aflezen PotMeter 1 - motor1_pwm = potMeterIn; + motor1_pwm = potMeter1.read(); // Aflezen PotMeter 1 // Encoder counts printen - pc.printf("%i\r\n", Encoder.getPulses()); + //pc.printf("%i\r\n", Encoder.getPulses()); } void changeDirectionButton() { - motor1_direction = 1 - motor1_direction; - - //float motor1_velocity = pot1.read() *6.2; - //pc.printf("Velocity is %f \r\n", motor1_velocity); + motor1_direction = 1 - motor1_direction; // + pc.printf("Motor direction value %i \r\n", motor1_direction); } double GetReferenceVelocity() @@ -32,40 +35,50 @@ // Returns reference velocity in rad/s. // Positive value means clockwise rotation. // 60 rpm = 60*2*pi/60 = 6.28 ~ 6.2 rad/s - const float maxVelocity=6.2; // in rad/s of course! + const double maxVelocity=6.2; // in rad/s of course! double referenceVelocity; // in rad/s - if (button2) - { + switch (motor1_direction) + { + case 0: // check if motor1_direction is 0 // Clockwise rotation - referenceVelocity = potMeterIn * maxVelocity; - } - else - { + referenceVelocity = potMeter1 * maxVelocity; + pc.printf("Reference velocity is %f. \r\n", referenceVelocity); + break; + case 1: // check if motor1_direction is 1 // Counterclockwise rotation - referenceVelocity = -1*potMeterIn * maxVelocity; + referenceVelocity = -1*potMeter1 * maxVelocity; + pc.printf("Reference velocity is %f. \r\n", referenceVelocity); + break; + default: + pc.printf("Something has gone wrong. \r \n"); } return referenceVelocity; } -double GetMeasuredVelocity() +void GetMeasuredVelocity() { // Get actual velocity from the motor plant // Use encoder (counts) - - int counts = Encoder.getPulses(); - double RotationalPosition = ( counts / 4 * 64) * 6.28; // Rotational position in radians - - double measuredVelocity; + double OldRotationalPosition = RotationalPosition; + double counts = Encoder.getPulses(); + RotationalPosition = ( counts / 4 * 64) * 6.28; // Rotational position in radians // hier komt de berekening van measured velocity - return measuredVelocity; + double measuredVelocity = (RotationalPosition - OldRotationalPosition) / tickertime; + //measuredVelocity defined as global variable, doesn't have to be returned anymore, is stored in memory all the time; } int main() { pc.baud(115200); + motor1_direction = 0; motor1_pwm.period_us(60.0); // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to done once), FastPWM variabele MotorSpeedCounts.attach(Motor, 0.5); // Ticker die de functie Motor aanroept elke halve seconde, meer tijd is tragere respons op potmeter button2.rise(changeDirectionButton); + measurevelocity.attach(GetMeasuredVelocity, tickertime); + + double referenceSnelheid = GetReferenceVelocity(); + pc.printf("Reference velocity is %f \r\n Measured velocity is %f. \r\n", referenceSnelheid, measuredVelocity); + while(true){} // Endless loop } \ No newline at end of file