Motor control, feedback, PI controller, BiQuad filter
Dependencies: FastPWM HIDScope MODSERIAL biquadFilter mbed QEI
Diff: main.cpp
- Revision:
- 11:4e3ef6150a2e
- Parent:
- 10:b002572e37fd
- Child:
- 12:1ecd11dc2c00
--- a/main.cpp Mon Oct 15 13:53:15 2018 +0000 +++ b/main.cpp Mon Oct 15 14:36:33 2018 +0000 @@ -16,21 +16,43 @@ volatile double measuredVelocity = 0.0; const double tickertime = 0.001; -void Motor() +/*void Motor() { // Aflezen Potentiometers voor PWM motor1_pwm = potMeter1.read(); // Aflezen PotMeter 1 + // Encoder counts printen //pc.printf("%i\r\n", Encoder.getPulses()); +}*/ + +double ReferencePosition() +{ + double y_ref = 314.0*potMeter1.read(); // Reference value y, scaled from 0 to 100 pi + pc.printf('Value potmeter is %f\r\n', y_ref); + return y_ref; } - + +double EncoderPosition() +{ + double OldRotationalPosition = RotationalPosition; + double counts = Encoder.getPulses(); + RotationalPosition = ( counts / 4.0 * 64.0) * 6.28; // Rotational position in radians + return RotationalPosition; +} + +void PositionGain() +{ + double Kp = 10.0*potMeter2.read(); // Scale 0 to 10 +} + + void changeDirectionButton() { motor1_direction = 1 - motor1_direction; // pc.printf("Motor direction value %i \r\n", motor1_direction); } -double GetReferenceVelocity() +/* double GetReferenceVelocity() { // Returns reference velocity in rad/s. // Positive value means clockwise rotation. @@ -53,9 +75,9 @@ pc.printf("Something has gone wrong. \r \n"); } return referenceVelocity; -} +} */ -void GetMeasuredVelocity() +/*void GetMeasuredVelocity() { // Get actual velocity from the motor plant // Use encoder (counts) @@ -65,20 +87,28 @@ // hier komt de berekening van measured velocity double measuredVelocity = (RotationalPosition - OldRotationalPosition) / tickertime; //measuredVelocity defined as global variable, doesn't have to be returned anymore, is stored in memory all the time; +}*/ + +double Error() +{ + double Error = ReferencePosition()-EncoderPosition(); + return Error; } - int main() { pc.baud(115200); - motor1_direction = 0; + bool 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 + //MotorSpeedCounts.attach(ReferencePosition, 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); + //measurevelocity.attach(GetMeasuredVelocity, tickertime); - double referenceSnelheid = GetReferenceVelocity(); - pc.printf("Reference velocity is %f \r\n Measured velocity is %f. \r\n", referenceSnelheid, measuredVelocity); + //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 + while(Error ==! 0) // when reference postion is reached, stop with the while loop + { + MotorSpeedCounts.attach(PositionGain,0.5); + } +}