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@11:4e3ef6150a2e, 2018-10-15 (annotated)

Committer:
nicollevanrijswijk
Date:
Mon Oct 15 14:36:33 2018 +0000
Revision:
11:4e3ef6150a2e
Parent:
9:b002572e37fd
Child:
12:1ecd11dc2c00
Adding Proportional controller (assignment 1.3)

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 9:b002572e37fd 10 QEI Encoder (D12, D13, NC, 64, QEI::X4_ENCODING);
1856413 8:ceb9abb5a4a8 11 Ticker MotorSpeedCounts;
1856413 9:b002572e37fd 12 Ticker measurevelocity;
1856413 9:b002572e37fd 13
1856413 9:b002572e37fd 14 //Global variables
1856413 9:b002572e37fd 15 volatile double RotationalPosition = 0.0;
1856413 9:b002572e37fd 16 volatile double measuredVelocity = 0.0;
1856413 9:b002572e37fd 17 const double tickertime = 0.001;
nicollevanrijswijk 5:a1fb2d2fb2d0 18
nicollevanrijswijk 11:4e3ef6150a2e 19 /*void Motor()
1856413 0:2e33035d4e86 20 {
1856413 1:c19fc63d555f 21 // Aflezen Potentiometers voor PWM
1856413 9:b002572e37fd 22 motor1_pwm = potMeter1.read(); // Aflezen PotMeter 1
nicollevanrijswijk 11:4e3ef6150a2e 23
1856413 8:ceb9abb5a4a8 24 // Encoder counts printen
1856413 9:b002572e37fd 25 //pc.printf("%i\r\n", Encoder.getPulses());
nicollevanrijswijk 11:4e3ef6150a2e 26 }*/
nicollevanrijswijk 11:4e3ef6150a2e 27
nicollevanrijswijk 11:4e3ef6150a2e 28 double ReferencePosition()
nicollevanrijswijk 11:4e3ef6150a2e 29 {
nicollevanrijswijk 11:4e3ef6150a2e 30 double y_ref = 314.0*potMeter1.read(); // Reference value y, scaled from 0 to 100 pi
nicollevanrijswijk 11:4e3ef6150a2e 31 pc.printf('Value potmeter is %f\r\n', y_ref);
nicollevanrijswijk 11:4e3ef6150a2e 32 return y_ref;
1856413 0:2e33035d4e86 33 }
nicollevanrijswijk 11:4e3ef6150a2e 34
nicollevanrijswijk 11:4e3ef6150a2e 35 double EncoderPosition()
nicollevanrijswijk 11:4e3ef6150a2e 36 {
nicollevanrijswijk 11:4e3ef6150a2e 37 double OldRotationalPosition = RotationalPosition;
nicollevanrijswijk 11:4e3ef6150a2e 38 double counts = Encoder.getPulses();
nicollevanrijswijk 11:4e3ef6150a2e 39 RotationalPosition = ( counts / 4.0 * 64.0) * 6.28; // Rotational position in radians
nicollevanrijswijk 11:4e3ef6150a2e 40 return RotationalPosition;
nicollevanrijswijk 11:4e3ef6150a2e 41 }
nicollevanrijswijk 11:4e3ef6150a2e 42
nicollevanrijswijk 11:4e3ef6150a2e 43 void PositionGain()
nicollevanrijswijk 11:4e3ef6150a2e 44 {
nicollevanrijswijk 11:4e3ef6150a2e 45 double Kp = 10.0*potMeter2.read(); // Scale 0 to 10
nicollevanrijswijk 11:4e3ef6150a2e 46 }
nicollevanrijswijk 11:4e3ef6150a2e 47
nicollevanrijswijk 11:4e3ef6150a2e 48
1856413 8:ceb9abb5a4a8 49 void changeDirectionButton()
1856413 1:c19fc63d555f 50 {
1856413 9:b002572e37fd 51 motor1_direction = 1 - motor1_direction; //
1856413 9:b002572e37fd 52 pc.printf("Motor direction value %i \r\n", motor1_direction);
1856413 1:c19fc63d555f 53 }
1856413 0:2e33035d4e86 54
nicollevanrijswijk 11:4e3ef6150a2e 55 /* double GetReferenceVelocity()
1856413 8:ceb9abb5a4a8 56 {
1856413 8:ceb9abb5a4a8 57 // Returns reference velocity in rad/s.
1856413 8:ceb9abb5a4a8 58 // Positive value means clockwise rotation.
1856413 8:ceb9abb5a4a8 59 // 60 rpm = 60*2*pi/60 = 6.28 ~ 6.2 rad/s
1856413 9:b002572e37fd 60 const double maxVelocity=6.2; // in rad/s of course!
1856413 8:ceb9abb5a4a8 61 double referenceVelocity; // in rad/s
1856413 9:b002572e37fd 62 switch (motor1_direction)
1856413 9:b002572e37fd 63 {
1856413 9:b002572e37fd 64 case 0: // check if motor1_direction is 0
1856413 8:ceb9abb5a4a8 65 // Clockwise rotation
1856413 9:b002572e37fd 66 referenceVelocity = potMeter1 * maxVelocity;
1856413 9:b002572e37fd 67 pc.printf("Reference velocity is %f. \r\n", referenceVelocity);
1856413 9:b002572e37fd 68 break;
1856413 9:b002572e37fd 69 case 1: // check if motor1_direction is 1
1856413 8:ceb9abb5a4a8 70 // Counterclockwise rotation
1856413 9:b002572e37fd 71 referenceVelocity = -1*potMeter1 * maxVelocity;
1856413 9:b002572e37fd 72 pc.printf("Reference velocity is %f. \r\n", referenceVelocity);
1856413 9:b002572e37fd 73 break;
1856413 9:b002572e37fd 74 default:
1856413 9:b002572e37fd 75 pc.printf("Something has gone wrong. \r \n");
1856413 8:ceb9abb5a4a8 76 }
1856413 8:ceb9abb5a4a8 77 return referenceVelocity;
nicollevanrijswijk 11:4e3ef6150a2e 78 } */
1856413 8:ceb9abb5a4a8 79
nicollevanrijswijk 11:4e3ef6150a2e 80 /*void GetMeasuredVelocity()
1856413 8:ceb9abb5a4a8 81 {
1856413 8:ceb9abb5a4a8 82 // Get actual velocity from the motor plant
1856413 8:ceb9abb5a4a8 83 // Use encoder (counts)
1856413 9:b002572e37fd 84 double OldRotationalPosition = RotationalPosition;
1856413 9:b002572e37fd 85 double counts = Encoder.getPulses();
1856413 9:b002572e37fd 86 RotationalPosition = ( counts / 4 * 64) * 6.28; // Rotational position in radians
1856413 8:ceb9abb5a4a8 87 // hier komt de berekening van measured velocity
1856413 9:b002572e37fd 88 double measuredVelocity = (RotationalPosition - OldRotationalPosition) / tickertime;
1856413 9:b002572e37fd 89 //measuredVelocity defined as global variable, doesn't have to be returned anymore, is stored in memory all the time;
nicollevanrijswijk 11:4e3ef6150a2e 90 }*/
nicollevanrijswijk 11:4e3ef6150a2e 91
nicollevanrijswijk 11:4e3ef6150a2e 92 double Error()
nicollevanrijswijk 11:4e3ef6150a2e 93 {
nicollevanrijswijk 11:4e3ef6150a2e 94 double Error = ReferencePosition()-EncoderPosition();
nicollevanrijswijk 11:4e3ef6150a2e 95 return Error;
1856413 8:ceb9abb5a4a8 96 }
1856413 8:ceb9abb5a4a8 97
1856413 0:2e33035d4e86 98 int main()
1856413 0:2e33035d4e86 99 {
1856413 6:bd73804c8cec 100 pc.baud(115200);
nicollevanrijswijk 11:4e3ef6150a2e 101 bool motor1_direction = 0;
1856413 0:2e33035d4e86 102 motor1_pwm.period_us(60.0); // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to done once), FastPWM variabele
nicollevanrijswijk 11:4e3ef6150a2e 103 //MotorSpeedCounts.attach(ReferencePosition, 0.5); // Ticker die de functie Motor aanroept elke halve seconde, meer tijd is tragere respons op potmeter
1856413 8:ceb9abb5a4a8 104 button2.rise(changeDirectionButton);
nicollevanrijswijk 11:4e3ef6150a2e 105 //measurevelocity.attach(GetMeasuredVelocity, tickertime);
1856413 9:b002572e37fd 106
nicollevanrijswijk 11:4e3ef6150a2e 107 //double referenceSnelheid = GetReferenceVelocity();
nicollevanrijswijk 11:4e3ef6150a2e 108 //pc.printf("Reference velocity is %f \r\n Measured velocity is %f. \r\n", referenceSnelheid, measuredVelocity);
1856413 9:b002572e37fd 109
nicollevanrijswijk 11:4e3ef6150a2e 110 while(Error ==! 0) // when reference postion is reached, stop with the while loop
nicollevanrijswijk 11:4e3ef6150a2e 111 {
nicollevanrijswijk 11:4e3ef6150a2e 112 MotorSpeedCounts.attach(PositionGain,0.5);
nicollevanrijswijk 11:4e3ef6150a2e 113 }
nicollevanrijswijk 11:4e3ef6150a2e 114 }