Ramon Waninge / Mbed 2 deprecated Milestone1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FastPWM mbed QEI biquadFilter HIDScope MODSERIAL

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

Committer:
efvanmarrewijk
Date:
Mon Oct 15 14:40:50 2018 +0000
Revision:
15:a52be6368cd5
Parent:
14:e21cb701ccb8
Child:
16:720365110953
Update met PID controller

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ramonwaninge 0:3ea1bbfbeaae 1 #include "mbed.h"
efvanmarrewijk 11:3efd6a324f16 2 #include "FastPWM.h"
efvanmarrewijk 11:3efd6a324f16 3 #include "QEI.h" // Includes library for encoder
efvanmarrewijk 13:6556cd086d07 4 #include "MODSERIAL.h"
efvanmarrewijk 13:6556cd086d07 5 #include "HIDScope.h"
efvanmarrewijk 13:6556cd086d07 6 #include "BiQuad.h"
Ramonwaninge 0:3ea1bbfbeaae 7
efvanmarrewijk 14:e21cb701ccb8 8 // Input
Ramonwaninge 4:0c27632b453a 9 AnalogIn pot1(A1);
Ramonwaninge 5:047db32db712 10 AnalogIn pot2(A2);
efvanmarrewijk 11:3efd6a324f16 11 InterruptIn button1(D0);
efvanmarrewijk 11:3efd6a324f16 12 InterruptIn button2(D1);
efvanmarrewijk 11:3efd6a324f16 13 InterruptIn emergencybutton(SW2); /* This is not yet implemented!
efvanmarrewijk 11:3efd6a324f16 14 The button SW2 on the K64F is the emergency button: if you press this,
efvanmarrewijk 11:3efd6a324f16 15 everything will abort as soon as possible
efvanmarrewijk 11:3efd6a324f16 16 */
efvanmarrewijk 11:3efd6a324f16 17
efvanmarrewijk 14:e21cb701ccb8 18 DigitalIn pin8(D8); // Encoder 1 B
efvanmarrewijk 11:3efd6a324f16 19 DigitalIn pin9(D9); // Encoder 1 A
efvanmarrewijk 14:e21cb701ccb8 20 DigitalIn pin10(D10); // Encoder 2 B
efvanmarrewijk 11:3efd6a324f16 21 DigitalIn pin11(D11); // Encoder 2 A
efvanmarrewijk 14:e21cb701ccb8 22 DigitalIn pin12(D12); // Encoder 3 B
efvanmarrewijk 11:3efd6a324f16 23 DigitalIn pin13(D13); // Encoder 3 A
efvanmarrewijk 9:65c52c1f4a57 24
efvanmarrewijk 14:e21cb701ccb8 25 // Output
efvanmarrewijk 10:ac36f9a204dd 26 DigitalOut pin2(D2); // Motor 3 direction
efvanmarrewijk 9:65c52c1f4a57 27 FastPWM pin3(D3); // Motor 3 pwm
efvanmarrewijk 10:ac36f9a204dd 28 DigitalOut pin4(D4); // Motor 2 direction
efvanmarrewijk 6:3c9569087274 29 FastPWM pin5(D5); // Motor 2 pwm
efvanmarrewijk 6:3c9569087274 30 FastPWM pin6(D6); // Motor 1 pwm
efvanmarrewijk 6:3c9569087274 31 DigitalOut pin7(D7); // Motor 1 direction
efvanmarrewijk 15:a52be6368cd5 32 //double u1 = pot1;
Ramonwaninge 2:d8a552d1d33a 33
efvanmarrewijk 14:e21cb701ccb8 34 MODSERIAL pc(USBTX, USBRX);
efvanmarrewijk 13:6556cd086d07 35 Ticker motor;
efvanmarrewijk 15:a52be6368cd5 36 double u3 = 0.0; // Normalised variable for the movement of motor 3
efvanmarrewijk 9:65c52c1f4a57 37
efvanmarrewijk 11:3efd6a324f16 38 void draaibuttons()
efvanmarrewijk 11:3efd6a324f16 39 { /* Pressing button 2 concludes in a change of speed. While button 1 is pressed,
efvanmarrewijk 11:3efd6a324f16 40 the direction of change of speed is reversed. So pressing button 1 and 2
efvanmarrewijk 11:3efd6a324f16 41 simultaneously results for the turning speed of motor 3 in a slower movement,
efvanmarrewijk 11:3efd6a324f16 42 and eventually the motor will turn the other way around.
efvanmarrewijk 11:3efd6a324f16 43 */
efvanmarrewijk 14:e21cb701ccb8 44 if (button1 == 1 && button2 == 1)
efvanmarrewijk 14:e21cb701ccb8 45 { u3 = u3 + 0.1f; //In stapjes van 0.1
efvanmarrewijk 13:6556cd086d07 46 if (u3>1.0f)
efvanmarrewijk 13:6556cd086d07 47 { u3 = 1.0f;
efvanmarrewijk 11:3efd6a324f16 48 }
efvanmarrewijk 10:ac36f9a204dd 49 }
efvanmarrewijk 14:e21cb701ccb8 50
efvanmarrewijk 14:e21cb701ccb8 51 else if (button1 == 0 && button2 == 1)
efvanmarrewijk 13:6556cd086d07 52 { u3 = u3 - 0.1f;
efvanmarrewijk 13:6556cd086d07 53 if (u3>1.0f)
efvanmarrewijk 13:6556cd086d07 54 { u3 = 1.0f;
efvanmarrewijk 11:3efd6a324f16 55 }
efvanmarrewijk 9:65c52c1f4a57 56 }
efvanmarrewijk 11:3efd6a324f16 57 }
efvanmarrewijk 9:65c52c1f4a57 58
efvanmarrewijk 11:3efd6a324f16 59 void draai()
efvanmarrewijk 11:3efd6a324f16 60 /* Function for the movement of all motors, using the potmeters for the moving
efvanmarrewijk 11:3efd6a324f16 61 direction and speed of motor 1 and 2, and using button 1 and 2 on the biorobotics
efvanmarrewijk 11:3efd6a324f16 62 shield for the moving direction and speed of motor 3.
efvanmarrewijk 11:3efd6a324f16 63 */
efvanmarrewijk 11:3efd6a324f16 64 {
efvanmarrewijk 15:a52be6368cd5 65 double u1 = 2.0*(pot1 - 0.5); // Normalised variable for the movement of motor 1
efvanmarrewijk 6:3c9569087274 66 if (u1>0)
efvanmarrewijk 6:3c9569087274 67 { pin4 = true;
efvanmarrewijk 6:3c9569087274 68 }
efvanmarrewijk 6:3c9569087274 69 else if(u1<0)
efvanmarrewijk 6:3c9569087274 70 { pin4 = false;
efvanmarrewijk 6:3c9569087274 71 }
efvanmarrewijk 6:3c9569087274 72 pin5 = fabs(u1);
efvanmarrewijk 6:3c9569087274 73
efvanmarrewijk 15:a52be6368cd5 74 double u2 = 2.0*(pot2 - 0.5); // Normalised variable for the movement of motor 2
efvanmarrewijk 11:3efd6a324f16 75 if (u2<0)
efvanmarrewijk 6:3c9569087274 76 { pin7 = true;
efvanmarrewijk 6:3c9569087274 77 }
efvanmarrewijk 11:3efd6a324f16 78 else if(u2>0)
efvanmarrewijk 6:3c9569087274 79 { pin7 = false;
efvanmarrewijk 6:3c9569087274 80 }
efvanmarrewijk 11:3efd6a324f16 81 pin6 = fabs(u2);
efvanmarrewijk 11:3efd6a324f16 82
efvanmarrewijk 15:a52be6368cd5 83 if (u3>0) //misschien weg?
efvanmarrewijk 11:3efd6a324f16 84 { pin2 = true;
efvanmarrewijk 11:3efd6a324f16 85 }
efvanmarrewijk 11:3efd6a324f16 86 else if(u3<0)
efvanmarrewijk 11:3efd6a324f16 87 { pin2 = false;
efvanmarrewijk 11:3efd6a324f16 88 }
efvanmarrewijk 11:3efd6a324f16 89 else
efvanmarrewijk 11:3efd6a324f16 90 { pin3 = 0;
efvanmarrewijk 14:e21cb701ccb8 91 }
efvanmarrewijk 14:e21cb701ccb8 92 pin3 = fabs(u3);
Ramonwaninge 3:d39285fdd103 93 }
efvanmarrewijk 15:a52be6368cd5 94 /*
efvanmarrewijk 15:a52be6368cd5 95 double Kp = 17.5;
efvanmarrewijk 15:a52be6368cd5 96 double Ki = 1.02;
efvanmarrewijk 15:a52be6368cd5 97 double Kd = 23.2;
efvanmarrewijk 15:a52be6368cd5 98 double Ts = 0.01; // Sample time in seconds
efvanmarrewijk 15:a52be6368cd5 99
efvanmarrewijk 15:a52be6368cd5 100 double PID_controller(double error)
efvanmarrewijk 15:a52be6368cd5 101 { static double error_integral = 0;
efvanmarrewijk 15:a52be6368cd5 102 static double error_prev = error;
efvanmarrewijk 15:a52be6368cd5 103
efvanmarrewijk 15:a52be6368cd5 104 // initialization with this value only done once!
efvanmarrewijk 15:a52be6368cd5 105 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
efvanmarrewijk 15:a52be6368cd5 106
efvanmarrewijk 15:a52be6368cd5 107 // Proportional part:
efvanmarrewijk 15:a52be6368cd5 108 double u_k = Kp * e;
efvanmarrewijk 15:a52be6368cd5 109 // Integral part
efvanmarrewijk 15:a52be6368cd5 110 error_integral = error_integral + error * Ts;
efvanmarrewijk 15:a52be6368cd5 111 double u_i = Ki * error_integral;
efvanmarrewijk 15:a52be6368cd5 112 // Derivative part
efvanmarrewijk 15:a52be6368cd5 113 double error_derivative = (error - error_prev)/Ts;
efvanmarrewijk 15:a52be6368cd5 114 double filtered_error_derivative = LowPassFilter.step(error_derivative);
efvanmarrewijk 15:a52be6368cd5 115 double u_d = Kd * filtered_error_derivative; error_prev = error;
efvanmarrewijk 15:a52be6368cd5 116
efvanmarrewijk 15:a52be6368cd5 117 // Sum all parts and return it
efvanmarrewijk 15:a52be6368cd5 118 return u_k + u_i + u_d;
efvanmarrewijk 15:a52be6368cd5 119 }
efvanmarrewijk 15:a52be6368cd5 120 */
efvanmarrewijk 11:3efd6a324f16 121
efvanmarrewijk 11:3efd6a324f16 122 int main()
efvanmarrewijk 11:3efd6a324f16 123 {
efvanmarrewijk 14:e21cb701ccb8 124 pc.baud(115200);
efvanmarrewijk 14:e21cb701ccb8 125
efvanmarrewijk 9:65c52c1f4a57 126 pin5.period(1.0/10000);
efvanmarrewijk 15:a52be6368cd5 127 pin3.period_us(50);
efvanmarrewijk 15:a52be6368cd5 128 pin5.period_us(50);
efvanmarrewijk 15:a52be6368cd5 129 pin6.period_us(50);
efvanmarrewijk 15:a52be6368cd5 130
efvanmarrewijk 15:a52be6368cd5 131 //Ticker
efvanmarrewijk 15:a52be6368cd5 132 motor.attach(draai, 0.001);
efvanmarrewijk 15:a52be6368cd5 133
efvanmarrewijk 15:a52be6368cd5 134 //Interrupts
efvanmarrewijk 11:3efd6a324f16 135 button1.rise(&draaibuttons);
efvanmarrewijk 11:3efd6a324f16 136 button2.rise(&draaibuttons);
efvanmarrewijk 11:3efd6a324f16 137 while(true)
efvanmarrewijk 11:3efd6a324f16 138 {
Ramonwaninge 3:d39285fdd103 139 }
Ramonwaninge 0:3ea1bbfbeaae 140 }