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@29:d8e51f4cf080, 2018-10-30 (annotated)

Committer:
efvanmarrewijk
Date:
Tue Oct 30 09:51:51 2018 +0000
Revision:
29:d8e51f4cf080
Parent:
28:aec0d9bdb949
Child:
30:c4a3e868ef04
With PID controller

Who changed what in which revision?

UserRevisionLine numberNew contents of line
efvanmarrewijk 16:720365110953 1 // Inclusion of libraries
Ramonwaninge 0:3ea1bbfbeaae 2 #include "mbed.h"
efvanmarrewijk 11:3efd6a324f16 3 #include "FastPWM.h"
efvanmarrewijk 11:3efd6a324f16 4 #include "QEI.h" // Includes library for encoder
efvanmarrewijk 13:6556cd086d07 5 #include "MODSERIAL.h"
efvanmarrewijk 13:6556cd086d07 6 #include "HIDScope.h"
efvanmarrewijk 13:6556cd086d07 7 #include "BiQuad.h"
Ramonwaninge 0:3ea1bbfbeaae 8
efvanmarrewijk 14:e21cb701ccb8 9 // Input
efvanmarrewijk 27:3430dfb4c9fb 10 AnalogIn pot1(A1);
efvanmarrewijk 27:3430dfb4c9fb 11 AnalogIn pot2(A2);
efvanmarrewijk 21:363271dcfe1f 12 InterruptIn button1(D0);
efvanmarrewijk 21:363271dcfe1f 13 InterruptIn button2(D1);
efvanmarrewijk 29:d8e51f4cf080 14 InterruptIn emergencybutton(SW2);
efvanmarrewijk 29:d8e51f4cf080 15 //The button SW2 on the K64F is the emergency button: if you press this, everything will abort as soon as possible
efvanmarrewijk 29:d8e51f4cf080 16
efvanmarrewijk 21:363271dcfe1f 17
efvanmarrewijk 27:3430dfb4c9fb 18 DigitalIn pin8(D8); // Encoder 1 B
efvanmarrewijk 27:3430dfb4c9fb 19 DigitalIn pin9(D9); // Encoder 1 A
efvanmarrewijk 27:3430dfb4c9fb 20 DigitalIn pin10(D10); // Encoder 2 B
efvanmarrewijk 27:3430dfb4c9fb 21 DigitalIn pin11(D11); // Encoder 2 A
efvanmarrewijk 27:3430dfb4c9fb 22 DigitalIn pin12(D12); // Encoder 3 B
efvanmarrewijk 27:3430dfb4c9fb 23 DigitalIn pin13(D13); // Encoder 3 A
efvanmarrewijk 9:65c52c1f4a57 24
efvanmarrewijk 14:e21cb701ccb8 25 // Output
efvanmarrewijk 26:b48708ed51ff 26 DigitalOut pin2(D2); // Motor 3 direction
efvanmarrewijk 26:b48708ed51ff 27 FastPWM pin3(D3); // Motor 3 pwm
efvanmarrewijk 26:b48708ed51ff 28 DigitalOut pin4(D4); // Motor 2 direction
efvanmarrewijk 26:b48708ed51ff 29 FastPWM pin5(D5); // Motor 2 pwm
efvanmarrewijk 26:b48708ed51ff 30 FastPWM pin6(D6); // Motor 1 pwm
efvanmarrewijk 26:b48708ed51ff 31 DigitalOut pin7(D7); // Motor 1 direction
efvanmarrewijk 26:b48708ed51ff 32
efvanmarrewijk 26:b48708ed51ff 33 DigitalOut greenled(LED_GREEN,1);
efvanmarrewijk 26:b48708ed51ff 34 DigitalOut redled(LED_RED,1);
efvanmarrewijk 26:b48708ed51ff 35 DigitalOut blueled(LED_BLUE,1);
Ramonwaninge 2:d8a552d1d33a 36
efvanmarrewijk 16:720365110953 37 // Utilisation of libraries
efvanmarrewijk 16:720365110953 38 MODSERIAL pc(USBTX, USBRX);
efvanmarrewijk 18:ca084c362855 39 QEI Encoder1(D11,D10,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk 18:ca084c362855 40 QEI Encoder2(D9,D8,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk 18:ca084c362855 41 QEI Encoder3(D13,D12,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk 20:695140b8db2f 42 Ticker motor;
efvanmarrewijk 24:d255752065d1 43 Ticker encoders;
efvanmarrewijk 9:65c52c1f4a57 44
efvanmarrewijk 16:720365110953 45 // Global variables
efvanmarrewijk 24:d255752065d1 46 const float pi = 3.14159265358979;
efvanmarrewijk 25:76e9e5597416 47 float u3 = 0.0; // Normalised variable for the movement of motor 3
efvanmarrewijk 24:d255752065d1 48 const float fCountsRad = 4200.0;
efvanmarrewijk 27:3430dfb4c9fb 49 const double dt = 0.5;
efvanmarrewijk 26:b48708ed51ff 50 const double Kp = 17.5; // given value is 17.5
efvanmarrewijk 26:b48708ed51ff 51 const double Ki = 1.02; // given value is 1.02
efvanmarrewijk 29:d8e51f4cf080 52 const double Kd = 23.2; // given value is 23.2
efvanmarrewijk 26:b48708ed51ff 53 //const double Ts = 0.0025; // Sample time in seconds
efvanmarrewijk 16:720365110953 54
efvanmarrewijk 16:720365110953 55 // Functions
efvanmarrewijk 28:aec0d9bdb949 56 //Subfunctions
efvanmarrewijk 27:3430dfb4c9fb 57 int Counts1input()
efvanmarrewijk 27:3430dfb4c9fb 58 { int counts1;
efvanmarrewijk 27:3430dfb4c9fb 59 counts1 = Encoder1.getPulses();
efvanmarrewijk 27:3430dfb4c9fb 60 return counts1;
efvanmarrewijk 27:3430dfb4c9fb 61 }
efvanmarrewijk 27:3430dfb4c9fb 62 int Counts2input()
efvanmarrewijk 27:3430dfb4c9fb 63 { int counts2;
efvanmarrewijk 27:3430dfb4c9fb 64 counts2 = Encoder2.getPulses();
efvanmarrewijk 27:3430dfb4c9fb 65 return counts2;
efvanmarrewijk 27:3430dfb4c9fb 66 }
efvanmarrewijk 27:3430dfb4c9fb 67 int Counts3input()
efvanmarrewijk 27:3430dfb4c9fb 68 { int counts3;
efvanmarrewijk 27:3430dfb4c9fb 69 counts3 = Encoder3.getPulses();
efvanmarrewijk 27:3430dfb4c9fb 70 return counts3;
efvanmarrewijk 27:3430dfb4c9fb 71 }
efvanmarrewijk 27:3430dfb4c9fb 72
efvanmarrewijk 27:3430dfb4c9fb 73 float CurrentAngle(float counts)
efvanmarrewijk 27:3430dfb4c9fb 74 { float angle = ((float)counts*2.0*pi)/fCountsRad;
efvanmarrewijk 27:3430dfb4c9fb 75 while (angle > pi)
efvanmarrewijk 27:3430dfb4c9fb 76 { angle = angle - 2.0*pi;
efvanmarrewijk 27:3430dfb4c9fb 77 }
efvanmarrewijk 27:3430dfb4c9fb 78 while (angle < pi)
efvanmarrewijk 27:3430dfb4c9fb 79 { angle = angle + 2.0*pi;
efvanmarrewijk 27:3430dfb4c9fb 80 }
efvanmarrewijk 27:3430dfb4c9fb 81 return angle;
efvanmarrewijk 27:3430dfb4c9fb 82 }
efvanmarrewijk 27:3430dfb4c9fb 83
efvanmarrewijk 27:3430dfb4c9fb 84 double ErrorCalc(double yref,double CurAngle)
efvanmarrewijk 27:3430dfb4c9fb 85 { double error = yref - CurAngle;
efvanmarrewijk 28:aec0d9bdb949 86 return error;
efvanmarrewijk 27:3430dfb4c9fb 87 }
efvanmarrewijk 27:3430dfb4c9fb 88
efvanmarrewijk 27:3430dfb4c9fb 89 double Pcontroller(double yref,double CurAngle)
efvanmarrewijk 27:3430dfb4c9fb 90 { double error = ErrorCalc(yref,CurAngle);
efvanmarrewijk 27:3430dfb4c9fb 91 //double Kp = 50.0*pot1; // Normalised variable for value of potmeter 1
efvanmarrewijk 27:3430dfb4c9fb 92 // Proportional part:
efvanmarrewijk 27:3430dfb4c9fb 93 double u_k = Kp * error;
efvanmarrewijk 27:3430dfb4c9fb 94 // Sum all parts and return it
efvanmarrewijk 27:3430dfb4c9fb 95 //pc.printf("error: %d",error);
efvanmarrewijk 27:3430dfb4c9fb 96 return u_k;
efvanmarrewijk 27:3430dfb4c9fb 97 }
efvanmarrewijk 27:3430dfb4c9fb 98
efvanmarrewijk 27:3430dfb4c9fb 99 double PIcontroller(double yref,double CurAngle)
efvanmarrewijk 29:d8e51f4cf080 100 { double error = ErrorCalc(yref,CurAngle);
efvanmarrewijk 27:3430dfb4c9fb 101 static double error_integral = 0;
efvanmarrewijk 27:3430dfb4c9fb 102 // Proportional part:
efvanmarrewijk 27:3430dfb4c9fb 103 double u_k = Kp * error;
efvanmarrewijk 27:3430dfb4c9fb 104 // Integral part
efvanmarrewijk 27:3430dfb4c9fb 105 error_integral = error_integral + error * dt;
efvanmarrewijk 27:3430dfb4c9fb 106 double u_i = Ki * error_integral;
efvanmarrewijk 27:3430dfb4c9fb 107 // Sum all parts and return it
efvanmarrewijk 27:3430dfb4c9fb 108 return u_k + u_i;
efvanmarrewijk 27:3430dfb4c9fb 109 }
efvanmarrewijk 27:3430dfb4c9fb 110
efvanmarrewijk 29:d8e51f4cf080 111 double PIDcontroller(double yref,double CurAngle)
efvanmarrewijk 29:d8e51f4cf080 112 { double error = ErrorCalc(yref,CurAngle);
efvanmarrewijk 29:d8e51f4cf080 113 static double error_integral = 0;
efvanmarrewijk 29:d8e51f4cf080 114 static double error_prev = error; // initialization with this value only done once!
efvanmarrewijk 29:d8e51f4cf080 115 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
efvanmarrewijk 29:d8e51f4cf080 116 // Proportional part:
efvanmarrewijk 29:d8e51f4cf080 117 double u_k = Kp * error;
efvanmarrewijk 29:d8e51f4cf080 118 // Integral part
efvanmarrewijk 29:d8e51f4cf080 119 error_integral = error_integral + error * dt;
efvanmarrewijk 29:d8e51f4cf080 120 double u_i = Ki * error_integral;
efvanmarrewijk 29:d8e51f4cf080 121 // Derivative part
efvanmarrewijk 29:d8e51f4cf080 122 double error_derivative = (error - error_prev)/dt;
efvanmarrewijk 29:d8e51f4cf080 123 double filtered_error_derivative = LowPassFilter.step(error_derivative);
efvanmarrewijk 29:d8e51f4cf080 124 double u_d = Kd * filtered_error_derivative;
efvanmarrewijk 29:d8e51f4cf080 125 error_prev = error;
efvanmarrewijk 29:d8e51f4cf080 126 // Sum all parts and return it
efvanmarrewijk 29:d8e51f4cf080 127 return u_k + u_i + u_d;
efvanmarrewijk 29:d8e51f4cf080 128 }
efvanmarrewijk 27:3430dfb4c9fb 129
efvanmarrewijk 27:3430dfb4c9fb 130 void turn1() // main function, all below functions with an extra tab are called
efvanmarrewijk 26:b48708ed51ff 131 {
efvanmarrewijk 27:3430dfb4c9fb 132 double refvalue1 = pi/4;
efvanmarrewijk 27:3430dfb4c9fb 133 int counts1 = Counts1input();
efvanmarrewijk 27:3430dfb4c9fb 134 float angle1 = CurrentAngle(counts1);
efvanmarrewijk 29:d8e51f4cf080 135 double PIDcontr = PIDcontroller(refvalue1,angle1);
efvanmarrewijk 27:3430dfb4c9fb 136 double error = ErrorCalc(refvalue1,angle1);
efvanmarrewijk 27:3430dfb4c9fb 137
efvanmarrewijk 29:d8e51f4cf080 138 pin6 = fabs(PIDcontr)/pi;
efvanmarrewijk 27:3430dfb4c9fb 139 if (error > 0)
efvanmarrewijk 26:b48708ed51ff 140 { pin7 = true;
efvanmarrewijk 24:d255752065d1 141 }
efvanmarrewijk 27:3430dfb4c9fb 142 else if (error < 0)
efvanmarrewijk 26:b48708ed51ff 143 { pin7 = false;
efvanmarrewijk 27:3430dfb4c9fb 144 }
efvanmarrewijk 26:b48708ed51ff 145 //pc.printf("%i\r\n",refvalue1);
efvanmarrewijk 26:b48708ed51ff 146 //pc.printf("Counts1,2,3: %i Angle1,2,3: %f \r\n",counts1,angle1);
efvanmarrewijk 18:ca084c362855 147 }
efvanmarrewijk 26:b48708ed51ff 148
efvanmarrewijk 25:76e9e5597416 149 /*double RefVelocity(float pot)
efvanmarrewijk 25:76e9e5597416 150 { // Returns reference velocity in rad/s.
efvanmarrewijk 25:76e9e5597416 151 // Positive value means clockwise rotation.
efvanmarrewijk 25:76e9e5597416 152 const float maxVelocity=8.4; // in rad/s of course!
efvanmarrewijk 25:76e9e5597416 153 double RefVel; // in rad/s
efvanmarrewijk 25:76e9e5597416 154
efvanmarrewijk 25:76e9e5597416 155 if (button1 == 1)
efvanmarrewijk 25:76e9e5597416 156 { // Clockwise rotation
efvanmarrewijk 25:76e9e5597416 157 RefVel = pot*maxVelocity;
efvanmarrewijk 25:76e9e5597416 158 }
efvanmarrewijk 11:3efd6a324f16 159 else
efvanmarrewijk 25:76e9e5597416 160 { // Counterclockwise rotation
efvanmarrewijk 25:76e9e5597416 161 RefVel = -1*pot*maxVelocity;
efvanmarrewijk 14:e21cb701ccb8 162 }
efvanmarrewijk 25:76e9e5597416 163 return RefVel;
Ramonwaninge 3:d39285fdd103 164 }
efvanmarrewijk 25:76e9e5597416 165 */
efvanmarrewijk 25:76e9e5597416 166
efvanmarrewijk 29:d8e51f4cf080 167
efvanmarrewijk 25:76e9e5597416 168 double ActualPosition(int counts, int offsetcounts)
efvanmarrewijk 25:76e9e5597416 169 { // After calibration, the counts are returned to 0;
efvanmarrewijk 29:d8e51f4cf080 170 double MotorPos = - (counts - offsetcounts) / fCountsRad;
efvanmarrewijk 25:76e9e5597416 171 // minus sign to correct for direction convention
efvanmarrewijk 25:76e9e5597416 172 return MotorPos;
efvanmarrewijk 25:76e9e5597416 173 }
efvanmarrewijk 11:3efd6a324f16 174
efvanmarrewijk 26:b48708ed51ff 175 void Emergency()
efvanmarrewijk 26:b48708ed51ff 176 { // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
efvanmarrewijk 26:b48708ed51ff 177 greenled = 1;
efvanmarrewijk 26:b48708ed51ff 178 blueled = 1;
efvanmarrewijk 26:b48708ed51ff 179 redled = 0;
efvanmarrewijk 26:b48708ed51ff 180 pin3 = 0;
efvanmarrewijk 26:b48708ed51ff 181 pin5 = 0;
efvanmarrewijk 26:b48708ed51ff 182 pin6 = 0;
efvanmarrewijk 26:b48708ed51ff 183 exit (0); //Abort mission!!
efvanmarrewijk 26:b48708ed51ff 184 }
efvanmarrewijk 26:b48708ed51ff 185
efvanmarrewijk 16:720365110953 186 // Main program
efvanmarrewijk 11:3efd6a324f16 187 int main()
efvanmarrewijk 18:ca084c362855 188 {
efvanmarrewijk 26:b48708ed51ff 189 pc.baud(115200);
efvanmarrewijk 18:ca084c362855 190
efvanmarrewijk 25:76e9e5597416 191 pin3.period(0.1);
efvanmarrewijk 25:76e9e5597416 192 pin5.period(0.1);
efvanmarrewijk 25:76e9e5597416 193 pin6.period(0.1);
efvanmarrewijk 25:76e9e5597416 194 motor.attach(turn1,dt);
efvanmarrewijk 28:aec0d9bdb949 195
efvanmarrewijk 26:b48708ed51ff 196 emergencybutton.rise(Emergency); //If the button is pressed, stop program
efvanmarrewijk 17:0ae9e8c958f8 197
efvanmarrewijk 16:720365110953 198 while (true)
efvanmarrewijk 25:76e9e5597416 199 {
efvanmarrewijk 27:3430dfb4c9fb 200 // Nothing here
Ramonwaninge 3:d39285fdd103 201 }
Ramonwaninge 0:3ea1bbfbeaae 202 }