Mathieu van der Donck / Mbed 2 deprecated gr14_assignment_PES

Assignment 1 compiled

Dependencies:   FastPWM MODSERIAL QEI mbed

Fork of gr14_assignment_PES by Marjolein Scheffers

main.cpp@2:7a8531d95f0b, 2018-10-15 (annotated)

Committer:
s1737619
Date:
Mon Oct 15 12:19:49 2018 +0000
Revision:
2:7a8531d95f0b
Parent:
1:a602cde74178 
Assigment 1, kan compilen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1680897 0:55255afaa7a4 1 #include "mbed.h"
s1680897 0:55255afaa7a4 2 #include "FastPWM.h"
s1680897 0:55255afaa7a4 3 #include "MODSERIAL.h"
s1680897 0:55255afaa7a4 4 #include "QEI.h"
s1680897 0:55255afaa7a4 5
s1680897 0:55255afaa7a4 6 DigitalOut gpo(D0);
s1680897 0:55255afaa7a4 7 DigitalOut led(LED_RED);
s1680897 0:55255afaa7a4 8 AnalogIn pot1(A0);
s1680897 0:55255afaa7a4 9 AnalogIn pot2(A1);
s1680897 0:55255afaa7a4 10 QEI encoder1(D10, D11, NC, 32);
s1680897 0:55255afaa7a4 11 QEI encoder2(D12, D13, NC, 32);
s1680897 0:55255afaa7a4 12 FastPWM motor1_pwm(D6);
s1680897 0:55255afaa7a4 13 DigitalOut motor1_richting(D7);
s1680897 0:55255afaa7a4 14
s1680897 0:55255afaa7a4 15 MODSERIAL pc(USBTX, USBRX);
s1680897 0:55255afaa7a4 16
s1680897 0:55255afaa7a4 17 int count = 0;
s1680897 0:55255afaa7a4 18
s1680897 0:55255afaa7a4 19 Ticker MotorTicker;
s1680897 0:55255afaa7a4 20
s1737619 2:7a8531d95f0b 21 void read_pots(volatile double &pot1_value, volatile double &pot2_value) // read pot 1 en pot 2
s1680897 0:55255afaa7a4 22 {
s1680897 0:55255afaa7a4 23 pot1_value = pot1.read();
s1680897 0:55255afaa7a4 24 pot2_value = pot2.read();
s1680897 0:55255afaa7a4 25 }
s1680897 0:55255afaa7a4 26
s1680897 0:55255afaa7a4 27 double det_ref(double ref_value) //zet referentiewaarde om in positie
s1680897 0:55255afaa7a4 28 {
s1680897 0:55255afaa7a4 29 return ref_value * 2 * 3.1416;
s1680897 0:55255afaa7a4 30 }
s1680897 0:55255afaa7a4 31
s1680897 0:55255afaa7a4 32 double meas_pos () //Encoder functie
s1680897 0:55255afaa7a4 33 {
s1680897 0:55255afaa7a4 34 return encoder1.getPulses()/32.0/131.25*2.0*3.1416;
s1680897 0:55255afaa7a4 35 }
s1680897 0:55255afaa7a4 36
s1737619 2:7a8531d95f0b 37 double PID_controller (double meas_pos, double ref_pos, double gain)
s1680897 0:55255afaa7a4 38 {
s1680897 0:55255afaa7a4 39 double error = ref_pos - meas_pos;
s1680897 0:55255afaa7a4 40
s1680897 0:55255afaa7a4 41 // Proportional control
s1680897 0:55255afaa7a4 42 double u = gain * error;
s1680897 0:55255afaa7a4 43
s1680897 0:55255afaa7a4 44 if (u > 1.0)
s1680897 0:55255afaa7a4 45 {
s1680897 0:55255afaa7a4 46 u = 1.0;
s1680897 0:55255afaa7a4 47 }
s1680897 0:55255afaa7a4 48
s1680897 0:55255afaa7a4 49 else if (u < -1.0)
s1680897 0:55255afaa7a4 50 {
s1737619 2:7a8531d95f0b 51 u = -1.0;
s1680897 0:55255afaa7a4 52 }
s1680897 0:55255afaa7a4 53 return u;
s1680897 0:55255afaa7a4 54 }
s1680897 0:55255afaa7a4 55
s1680897 0:55255afaa7a4 56 void move_mot(double u)
s1680897 0:55255afaa7a4 57 {
s1680897 0:55255afaa7a4 58
s1680897 0:55255afaa7a4 59 if (u <= 0)
s1680897 0:55255afaa7a4 60 {
s1680897 0:55255afaa7a4 61 motor1_richting = 0;
s1680897 1:a602cde74178 62 motor1_pwm.write(-u); //write Duty cycle
s1680897 0:55255afaa7a4 63 }
s1680897 0:55255afaa7a4 64
s1680897 0:55255afaa7a4 65 if (u >= 0)
s1680897 0:55255afaa7a4 66 {
s1680897 0:55255afaa7a4 67 motor1_richting = 1;
s1680897 0:55255afaa7a4 68 motor1_pwm.write(u); //write Duty cycle
s1680897 0:55255afaa7a4 69 }
s1680897 0:55255afaa7a4 70 }
s1680897 0:55255afaa7a4 71
s1680897 0:55255afaa7a4 72
s1680897 0:55255afaa7a4 73 void Motor_control()
s1680897 0:55255afaa7a4 74 {
s1737619 2:7a8531d95f0b 75 volatile double pot_value;
s1737619 2:7a8531d95f0b 76
s1737619 2:7a8531d95f0b 77 volatile double gain;
s1680897 0:55255afaa7a4 78
s1680897 0:55255afaa7a4 79 read_pots(pot_value, gain);
s1680897 0:55255afaa7a4 80
s1680897 0:55255afaa7a4 81 volatile double yref = det_ref(pot_value); // reference position
s1680897 0:55255afaa7a4 82
s1680897 0:55255afaa7a4 83 volatile double y = meas_pos(); // measured position
s1680897 0:55255afaa7a4 84
s1680897 0:55255afaa7a4 85 volatile double u = PID_controller(y, yref, gain); // output PID controller
s1680897 0:55255afaa7a4 86
s1680897 0:55255afaa7a4 87 move_mot(u); //functie die motor laat bewegen.
s1680897 0:55255afaa7a4 88
s1680897 0:55255afaa7a4 89 count++;
s1680897 0:55255afaa7a4 90
s1680897 0:55255afaa7a4 91 if (count == 400) // print 1x per seconde waardes.
s1680897 0:55255afaa7a4 92 {
s1737619 2:7a8531d95f0b 93 pc.printf("u = %lf, measure position y = %lf, reference position yref = %lf, gain = %lf \n", u, y, yref, gain);
s1680897 0:55255afaa7a4 94 count = 0;
s1680897 0:55255afaa7a4 95 }
s1680897 0:55255afaa7a4 96 }
s1680897 0:55255afaa7a4 97
s1680897 0:55255afaa7a4 98
s1680897 0:55255afaa7a4 99 int main()
s1680897 0:55255afaa7a4 100 {
s1680897 0:55255afaa7a4 101 pc.baud(115200);
s1680897 0:55255afaa7a4 102
s1680897 0:55255afaa7a4 103 int frequency_pwm = 16700; //16.7 kHz PWM
s1680897 0:55255afaa7a4 104 motor1_pwm.period(1.0/frequency_pwm); // T = 1/f
s1680897 0:55255afaa7a4 105 MotorTicker.attach(Motor_control, 0.0025);
s1680897 0:55255afaa7a4 106
s1680897 0:55255afaa7a4 107 while (true)
s1680897 0:55255afaa7a4 108 {}
s1680897 0:55255afaa7a4 109 }