Juliette Slob / Mbed 2 deprecated PES_tutorial_5_copy

Using HIDScope for P(I)D controller

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

Fork of PES_tutorial_5 by BMT Module 9 Group 4

main.cpp@10:076eb8beea30, 2018-10-15 (annotated)

Committer:
lweersink
Date:
Mon Oct 15 12:38:37 2018 +0000
Revision:
10:076eb8beea30
Parent:
8:ceb9abb5a4a8 
Reference velocity

Who changed what in which revision?

UserRevisionLine numberNew contents of line
1856413 0:2e33035d4e86 1 #include "mbed.h"
1856413 0:2e33035d4e86 2 #include "FastPWM.h" // FastPWM library
1856413 2:34c14fb36b5d 3 #include "MODSERIAL.h"
lweersink 4:49c5fd62a192 4 #include "QEI.h"
1856413 2:34c14fb36b5d 5 MODSERIAL pc(USBTX, USBRX);
1856413 0:2e33035d4e86 6 DigitalOut motor1_direction(D7);
1856413 8:ceb9abb5a4a8 7 AnalogIn potMeter1(A4);
1856413 8:ceb9abb5a4a8 8 InterruptIn button2(D3);
1856413 1:c19fc63d555f 9 FastPWM motor1_pwm(D6); //FastPWM input, PES lecture 2
1856413 8:ceb9abb5a4a8 10 Ticker MotorSpeedCounts;
1856413 6:bd73804c8cec 11 QEI Encoder (D12, D13, NC, 64, QEI::X4_ENCODING);
nicollevanrijswijk 5:a1fb2d2fb2d0 12
1856413 0:2e33035d4e86 13 void Motor()
1856413 0:2e33035d4e86 14 {
1856413 1:c19fc63d555f 15 // Aflezen Potentiometers voor PWM
1856413 8:ceb9abb5a4a8 16 float potMeterIn = potMeter1.read(); // Aflezen PotMeter 1
1856413 8:ceb9abb5a4a8 17 motor1_pwm = potMeterIn;
1856413 8:ceb9abb5a4a8 18 // Encoder counts printen
1856413 6:bd73804c8cec 19 pc.printf("%i\r\n", Encoder.getPulses());
1856413 0:2e33035d4e86 20 }
1856413 1:c19fc63d555f 21
1856413 8:ceb9abb5a4a8 22 void changeDirectionButton()
1856413 1:c19fc63d555f 23 {
1856413 3:ea819bcf667f 24 motor1_direction = 1 - motor1_direction;
1856413 8:ceb9abb5a4a8 25
1856413 8:ceb9abb5a4a8 26 //float motor1_velocity = pot1.read() *6.2;
1856413 8:ceb9abb5a4a8 27 //pc.printf("Velocity is %f \r\n", motor1_velocity);
1856413 1:c19fc63d555f 28 }
1856413 0:2e33035d4e86 29
1856413 8:ceb9abb5a4a8 30 double GetReferenceVelocity()
1856413 8:ceb9abb5a4a8 31 {
1856413 8:ceb9abb5a4a8 32 // Returns reference velocity in rad/s.
1856413 8:ceb9abb5a4a8 33 // Positive value means clockwise rotation.
1856413 8:ceb9abb5a4a8 34 // 60 rpm = 60*2*pi/60 = 6.28 ~ 6.2 rad/s
lweersink 10:076eb8beea30 35 const double maxVelocity= 6.2; // in rad/s of course!
1856413 8:ceb9abb5a4a8 36 double referenceVelocity; // in rad/s
lweersink 10:076eb8beea30 37 switch(motor1_direction)
lweersink 10:076eb8beea30 38 {
lweersink 10:076eb8beea30 39 case 0: // Clockwise rotation
lweersink 10:076eb8beea30 40 referenceVelocity = -1*potMeterIn * maxVelocity;
lweersink 10:076eb8beea30 41 pc.printf("Velocity is %d\r\n", referenceVelocity);
lweersink 10:076eb8beea30 42 break;
lweersink 10:076eb8beea30 43 case 1: // Counterclockwise rotation
1856413 8:ceb9abb5a4a8 44 referenceVelocity = -1*potMeterIn * maxVelocity;
lweersink 10:076eb8beea30 45 pc.printf("Velocity is %d\r\n", referenceVelocity);
lweersink 10:076eb8beea30 46 break;
lweersink 10:076eb8beea30 47 default:
lweersink 10:076eb8beea30 48 pc.printf("It goes wrong, the velocity is not -1 or 1");
lweersink 10:076eb8beea30 49 }
1856413 8:ceb9abb5a4a8 50 return referenceVelocity;
1856413 8:ceb9abb5a4a8 51 }
1856413 8:ceb9abb5a4a8 52
1856413 8:ceb9abb5a4a8 53 double GetMeasuredVelocity()
1856413 8:ceb9abb5a4a8 54 {
1856413 8:ceb9abb5a4a8 55 // Get actual velocity from the motor plant
1856413 8:ceb9abb5a4a8 56 // Use encoder (counts)
1856413 8:ceb9abb5a4a8 57
1856413 8:ceb9abb5a4a8 58 int counts = Encoder.getPulses();
1856413 8:ceb9abb5a4a8 59 double RotationalPosition = ( counts / 4 * 64) * 6.28; // Rotational position in radians
1856413 8:ceb9abb5a4a8 60
1856413 8:ceb9abb5a4a8 61 double measuredVelocity;
1856413 8:ceb9abb5a4a8 62 // hier komt de berekening van measured velocity
1856413 8:ceb9abb5a4a8 63 return measuredVelocity;
1856413 8:ceb9abb5a4a8 64 }
1856413 8:ceb9abb5a4a8 65
1856413 8:ceb9abb5a4a8 66
1856413 0:2e33035d4e86 67 int main()
1856413 0:2e33035d4e86 68 {
1856413 6:bd73804c8cec 69 pc.baud(115200);
1856413 0:2e33035d4e86 70 motor1_pwm.period_us(60.0); // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to done once), FastPWM variabele
1856413 8:ceb9abb5a4a8 71 MotorSpeedCounts.attach(Motor, 0.5); // Ticker die de functie Motor aanroept elke halve seconde, meer tijd is tragere respons op potmeter
1856413 8:ceb9abb5a4a8 72 button2.rise(changeDirectionButton);
1856413 0:2e33035d4e86 73 while(true){} // Endless loop
1856413 0:2e33035d4e86 74 }