Using HIDScope for P(I)D controller
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Fork of PES_tutorial_5 by
main.cpp
- Committer:
- 1856413
- Date:
- 2018-10-15
- Revision:
- 9:b002572e37fd
- Parent:
- 8:ceb9abb5a4a8
- Child:
- 11:4e3ef6150a2e
File content as of revision 9:b002572e37fd:
#include "mbed.h" #include "FastPWM.h" // FastPWM library #include "MODSERIAL.h" #include "QEI.h" MODSERIAL pc(USBTX, USBRX); DigitalOut motor1_direction(D7); 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; 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 motor1_pwm = potMeter1.read(); // Aflezen PotMeter 1 // Encoder counts printen //pc.printf("%i\r\n", Encoder.getPulses()); } void changeDirectionButton() { motor1_direction = 1 - motor1_direction; // pc.printf("Motor direction value %i \r\n", motor1_direction); } double GetReferenceVelocity() { // Returns reference velocity in rad/s. // Positive value means clockwise rotation. // 60 rpm = 60*2*pi/60 = 6.28 ~ 6.2 rad/s const double maxVelocity=6.2; // in rad/s of course! double referenceVelocity; // in rad/s switch (motor1_direction) { case 0: // check if motor1_direction is 0 // Clockwise rotation 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*potMeter1 * maxVelocity; pc.printf("Reference velocity is %f. \r\n", referenceVelocity); break; default: pc.printf("Something has gone wrong. \r \n"); } return referenceVelocity; } void GetMeasuredVelocity() { // Get actual velocity from the motor plant // Use encoder (counts) double OldRotationalPosition = RotationalPosition; double counts = Encoder.getPulses(); RotationalPosition = ( counts / 4 * 64) * 6.28; // Rotational position in radians // 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; } 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 }