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@42:cef1b3187e4c, 2018-10-31 (annotated)

Committer:
efvanmarrewijk
Date:
Wed Oct 31 14:49:57 2018 +0000
Revision:
42:cef1b3187e4c
Parent:
40:1be9dfad0a10 
!!Does not work

Who changed what in which revision?

UserRevisionLine numberNew contents of line
efvanmarrewijk 16:720365110953 1 // Inclusion of libraries
efvanmarrewijk 31:91ad5b188bd9 2 #include "mbed.h"
efvanmarrewijk 31:91ad5b188bd9 3 #include "FastPWM.h"
efvanmarrewijk 31:91ad5b188bd9 4 #include "QEI.h" // Includes library for encoder
efvanmarrewijk 31:91ad5b188bd9 5 #include "MODSERIAL.h"
efvanmarrewijk 31:91ad5b188bd9 6 #include "HIDScope.h"
efvanmarrewijk 31:91ad5b188bd9 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 31:91ad5b188bd9 14 InterruptIn emergencybutton(SW2); //The button SW2 on the K64F is the emergency button: if you press this, everything will abort as soon as possible
efvanmarrewijk 21:363271dcfe1f 15
efvanmarrewijk 32:a5b411833d1e 16 DigitalIn pin8(D8); // Encoder 1 B
efvanmarrewijk 32:a5b411833d1e 17 DigitalIn pin9(D9); // Encoder 1 A
efvanmarrewijk 32:a5b411833d1e 18 DigitalIn pin10(D10); // Encoder 2 B
efvanmarrewijk 32:a5b411833d1e 19 DigitalIn pin11(D11); // Encoder 2 A
efvanmarrewijk 32:a5b411833d1e 20 DigitalIn pin12(D12); // Encoder 3 B
efvanmarrewijk 32:a5b411833d1e 21 DigitalIn pin13(D13); // Encoder 3 A
efvanmarrewijk 9:65c52c1f4a57 22
efvanmarrewijk 14:e21cb701ccb8 23 // Output
efvanmarrewijk 35:ba556f2d0fcc 24 DigitalOut pin2(D2); // Motor 3 direction = motor flip
efvanmarrewijk 35:ba556f2d0fcc 25 FastPWM pin3(D3); // Motor 3 pwm = motor flip
efvanmarrewijk 35:ba556f2d0fcc 26 DigitalOut pin4(D4); // Motor 2 direction = motor right
efvanmarrewijk 35:ba556f2d0fcc 27 FastPWM pin5(D5); // Motor 2 pwm = motor right
efvanmarrewijk 35:ba556f2d0fcc 28 FastPWM pin6(D6); // Motor 1 pwm = motor left
efvanmarrewijk 35:ba556f2d0fcc 29 DigitalOut pin7(D7); // Motor 1 direction = motor left
efvanmarrewijk 26:b48708ed51ff 30
efvanmarrewijk 26:b48708ed51ff 31 DigitalOut greenled(LED_GREEN,1);
efvanmarrewijk 26:b48708ed51ff 32 DigitalOut redled(LED_RED,1);
efvanmarrewijk 26:b48708ed51ff 33 DigitalOut blueled(LED_BLUE,1);
Ramonwaninge 2:d8a552d1d33a 34
efvanmarrewijk 16:720365110953 35 // Utilisation of libraries
efvanmarrewijk 16:720365110953 36 MODSERIAL pc(USBTX, USBRX);
efvanmarrewijk 35:ba556f2d0fcc 37 QEI Encoderl(D9,D8,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk 39:dcf3e5019a63 38 QEI Encoderr(D10,D11,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk 35:ba556f2d0fcc 39 QEI Encoderf(D13,D12,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk 20:695140b8db2f 40 Ticker motor;
efvanmarrewijk 24:d255752065d1 41 Ticker encoders;
efvanmarrewijk 9:65c52c1f4a57 42
efvanmarrewijk 16:720365110953 43 // Global variables
efvanmarrewijk 32:a5b411833d1e 44 const float pi = 3.14159265358979f;
efvanmarrewijk 32:a5b411833d1e 45 float u3 = 0.0f; // Normalised variable for the movement of motor 3
efvanmarrewijk 32:a5b411833d1e 46 const float fCountsRad = 4200.0f;
efvanmarrewijk 42:cef1b3187e4c 47 const float dt = 0.01f;
efvanmarrewijk 16:720365110953 48
efvanmarrewijk 16:720365110953 49 // Functions
efvanmarrewijk 32:a5b411833d1e 50 void Emergency() // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
efvanmarrewijk 32:a5b411833d1e 51 { greenled = 1; // Red light on
efvanmarrewijk 30:c4a3e868ef04 52 blueled = 1;
efvanmarrewijk 30:c4a3e868ef04 53 redled = 0;
efvanmarrewijk 32:a5b411833d1e 54 pin3 = 0; // All motor forces to zero
efvanmarrewijk 30:c4a3e868ef04 55 pin5 = 0;
efvanmarrewijk 30:c4a3e868ef04 56 pin6 = 0;
efvanmarrewijk 32:a5b411833d1e 57 exit (0); // Abort mission!!
efvanmarrewijk 30:c4a3e868ef04 58 }
efvanmarrewijk 30:c4a3e868ef04 59
efvanmarrewijk 31:91ad5b188bd9 60 // Subfunctions
efvanmarrewijk 35:ba556f2d0fcc 61 int Countslinput() // Gets the counts from encoder 1
efvanmarrewijk 35:ba556f2d0fcc 62 { int countsl;
efvanmarrewijk 35:ba556f2d0fcc 63 countsl = Encoderl.getPulses();
efvanmarrewijk 35:ba556f2d0fcc 64 return countsl;
efvanmarrewijk 27:3430dfb4c9fb 65 }
efvanmarrewijk 35:ba556f2d0fcc 66 int Countsrinput() // Gets the counts from encoder 2
efvanmarrewijk 35:ba556f2d0fcc 67 { int countsr;
efvanmarrewijk 35:ba556f2d0fcc 68 countsr = Encoderr.getPulses();
efvanmarrewijk 35:ba556f2d0fcc 69 return countsr;
efvanmarrewijk 27:3430dfb4c9fb 70 }
efvanmarrewijk 35:ba556f2d0fcc 71 int Countsfinput() // Gets the counts from encoder 3
efvanmarrewijk 35:ba556f2d0fcc 72 { int countsf;
efvanmarrewijk 35:ba556f2d0fcc 73 countsf = Encoderf.getPulses();
efvanmarrewijk 35:ba556f2d0fcc 74 return countsf;
efvanmarrewijk 27:3430dfb4c9fb 75 }
efvanmarrewijk 27:3430dfb4c9fb 76
efvanmarrewijk 32:a5b411833d1e 77 float CurrentAngle(float counts) // Calculates the current angle of the motor (between -2*pi to 2*pi) based on the counts from the encoder
efvanmarrewijk 32:a5b411833d1e 78 { float angle = ((float)counts*2.0f*pi)/fCountsRad;
efvanmarrewijk 32:a5b411833d1e 79 while (angle > 2.0f*pi)
efvanmarrewijk 32:a5b411833d1e 80 { angle = angle-2.0f*pi;
efvanmarrewijk 27:3430dfb4c9fb 81 }
efvanmarrewijk 32:a5b411833d1e 82 while (angle < -2.0f*pi)
efvanmarrewijk 32:a5b411833d1e 83 { angle = angle+2.0f*pi;
efvanmarrewijk 30:c4a3e868ef04 84 }
efvanmarrewijk 30:c4a3e868ef04 85 return angle;
efvanmarrewijk 27:3430dfb4c9fb 86 }
efvanmarrewijk 27:3430dfb4c9fb 87
efvanmarrewijk 32:a5b411833d1e 88 float ErrorCalc(float refvalue,float CurAngle) // Calculates the error of the system, based on the current angle and the reference value
efvanmarrewijk 32:a5b411833d1e 89 { float error = refvalue - CurAngle;
efvanmarrewijk 30:c4a3e868ef04 90 return error;
efvanmarrewijk 30:c4a3e868ef04 91 }
efvanmarrewijk 30:c4a3e868ef04 92
efvanmarrewijk 32:a5b411833d1e 93 float Kpcontr() // Sets the Kp value for the controller dependent on the scaled angle of potmeter 2
efvanmarrewijk 32:a5b411833d1e 94 { float Kp = 20.0f*pot2;
efvanmarrewijk 30:c4a3e868ef04 95 return Kp;
efvanmarrewijk 30:c4a3e868ef04 96 }
efvanmarrewijk 30:c4a3e868ef04 97
efvanmarrewijk 32:a5b411833d1e 98 float Kdcontr() // Sets the Kd value for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 32:a5b411833d1e 99 { float Kd = 0.25f*pot1;
efvanmarrewijk 30:c4a3e868ef04 100 return Kd;
efvanmarrewijk 30:c4a3e868ef04 101 }
efvanmarrewijk 31:91ad5b188bd9 102
efvanmarrewijk 42:cef1b3187e4c 103 float PIDcontrollerl(float refvalue,float CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
efvanmarrewijk 30:c4a3e868ef04 104 { //float Kp = Kpcontr();
efvanmarrewijk 39:dcf3e5019a63 105 float Kp = 10.42f;
efvanmarrewijk 32:a5b411833d1e 106 float Ki = 1.02f;
efvanmarrewijk 39:dcf3e5019a63 107 float Kd = 0.0493f;
efvanmarrewijk 30:c4a3e868ef04 108 //float Kd = Kdcontr();
efvanmarrewijk 32:a5b411833d1e 109 float error = ErrorCalc(refvalue,CurAngle);
efvanmarrewijk 30:c4a3e868ef04 110 static float error_integral = 0.0;
efvanmarrewijk 30:c4a3e868ef04 111 static float error_prev = error; // initialization with this value only done once!
efvanmarrewijk 42:cef1b3187e4c 112 static BiQuad PIDfilterl(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
efvanmarrewijk 27:3430dfb4c9fb 113 // Proportional part:
efvanmarrewijk 30:c4a3e868ef04 114 float u_k = Kp * error;
efvanmarrewijk 27:3430dfb4c9fb 115 // Integral part
efvanmarrewijk 42:cef1b3187e4c 116 //error_integral = error_integral + error * dt;
efvanmarrewijk 42:cef1b3187e4c 117 //float u_i = Ki * error_integral;
efvanmarrewijk 30:c4a3e868ef04 118 // Derivative part
efvanmarrewijk 30:c4a3e868ef04 119 float error_derivative = (error - error_prev)/dt;
efvanmarrewijk 42:cef1b3187e4c 120 float filtered_error_derivative = PIDfilterl.step(error_derivative);
efvanmarrewijk 30:c4a3e868ef04 121 float u_d = Kd * filtered_error_derivative;
efvanmarrewijk 30:c4a3e868ef04 122 error_prev = error;
efvanmarrewijk 27:3430dfb4c9fb 123 // Sum all parts and return it
efvanmarrewijk 42:cef1b3187e4c 124 pc.printf ("l\t u_p = %f \tu_i = %f\t u_d = %f\t\t ",u_k,0,u_d);
efvanmarrewijk 42:cef1b3187e4c 125 return u_k + u_d;
efvanmarrewijk 27:3430dfb4c9fb 126 }
efvanmarrewijk 30:c4a3e868ef04 127
efvanmarrewijk 35:ba556f2d0fcc 128 float DesiredAnglel() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 42:cef1b3187e4c 129 { return (pot1*2.0f*pi)-pi;
efvanmarrewijk 42:cef1b3187e4c 130 //return angle;
efvanmarrewijk 30:c4a3e868ef04 131 }
efvanmarrewijk 35:ba556f2d0fcc 132
efvanmarrewijk 35:ba556f2d0fcc 133 float DesiredAngler() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 42:cef1b3187e4c 134 { return (pot2*2.0f*pi)-pi;
efvanmarrewijk 42:cef1b3187e4c 135 //return angle;
efvanmarrewijk 35:ba556f2d0fcc 136 }
efvanmarrewijk 35:ba556f2d0fcc 137
efvanmarrewijk 35:ba556f2d0fcc 138 float* turnl() // main function for movement of motor 1, all above functions with an extra tab are called
efvanmarrewijk 26:b48708ed51ff 139 {
efvanmarrewijk 35:ba556f2d0fcc 140 //float refvalue = pi/4.0f;
efvanmarrewijk 35:ba556f2d0fcc 141 float refvalue = DesiredAnglel(); // different minus sign per motor
efvanmarrewijk 35:ba556f2d0fcc 142 int counts = Countslinput(); // different encoder pins per motor
efvanmarrewijk 35:ba556f2d0fcc 143 float currentangle = CurrentAngle(counts); // different minus sign per motor
efvanmarrewijk 42:cef1b3187e4c 144 float PIDcontrol = PIDcontrollerl(refvalue,currentangle); // same for every motor
efvanmarrewijk 35:ba556f2d0fcc 145 float error = ErrorCalc(refvalue,currentangle); // same for every motor
efvanmarrewijk 30:c4a3e868ef04 146
efvanmarrewijk 42:cef1b3187e4c 147 //pin6 = fabs(PIDcontrol); // different pins for every motor
efvanmarrewijk 35:ba556f2d0fcc 148 pin7 = PIDcontrol > 0.0f; // different pins for every motor
efvanmarrewijk 31:91ad5b188bd9 149 //pin6 = 0.4+0.6*fabs(PIDcontr); //geschaald
efvanmarrewijk 31:91ad5b188bd9 150 //pin6 = fabs(PIDcontr)/pi;
efvanmarrewijk 42:cef1b3187e4c 151 //pc.printf(" countsl: %i currentanglel: %f PIDl: %f errorl: %f",counts,currentangle,PIDcontrol,error);
efvanmarrewijk 33:ec07e11676ec 152 float* outcome;
efvanmarrewijk 35:ba556f2d0fcc 153 float turninfo[3] = {error, refvalue, currentangle};
efvanmarrewijk 35:ba556f2d0fcc 154 //float ( &fillarr( float (&outcome)[3] ) )[3] { // no decay; argument must be size 3
efvanmarrewijk 35:ba556f2d0fcc 155 outcome = turninfo;
efvanmarrewijk 35:ba556f2d0fcc 156 return outcome;
efvanmarrewijk 35:ba556f2d0fcc 157 }
efvanmarrewijk 35:ba556f2d0fcc 158
efvanmarrewijk 42:cef1b3187e4c 159 float PIDcontrollerr(float refvalue,float CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
efvanmarrewijk 42:cef1b3187e4c 160 { //float Kp = Kpcontr();
efvanmarrewijk 42:cef1b3187e4c 161 float Kp = 10.42f;
efvanmarrewijk 42:cef1b3187e4c 162 float Ki = 1.02f;
efvanmarrewijk 42:cef1b3187e4c 163 float Kd = 0.0493f;
efvanmarrewijk 42:cef1b3187e4c 164 //float Kd = Kdcontr();
efvanmarrewijk 42:cef1b3187e4c 165 float error = ErrorCalc(refvalue,CurAngle);
efvanmarrewijk 42:cef1b3187e4c 166 static float error_integral = 0.0;
efvanmarrewijk 42:cef1b3187e4c 167 static float error_prev = error; // initialization with this value only done once!
efvanmarrewijk 42:cef1b3187e4c 168 static BiQuad PIDfilterr(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
efvanmarrewijk 42:cef1b3187e4c 169 // Proportional part:
efvanmarrewijk 42:cef1b3187e4c 170 float u_k = Kp * error;
efvanmarrewijk 42:cef1b3187e4c 171 // Integral part
efvanmarrewijk 42:cef1b3187e4c 172 error_integral = error_integral + error * dt;
efvanmarrewijk 42:cef1b3187e4c 173 float u_i = Ki * error_integral;
efvanmarrewijk 42:cef1b3187e4c 174 // Derivative part
efvanmarrewijk 42:cef1b3187e4c 175 float error_derivative = (error - error_prev)/dt;
efvanmarrewijk 42:cef1b3187e4c 176 float filtered_error_derivative = PIDfilterr.step(error_derivative);
efvanmarrewijk 42:cef1b3187e4c 177 float u_d = Kd * filtered_error_derivative;
efvanmarrewijk 42:cef1b3187e4c 178 error_prev = error;
efvanmarrewijk 42:cef1b3187e4c 179 // Sum all parts and return it
efvanmarrewijk 42:cef1b3187e4c 180 //pc.printf ("Kp = %f Kd = %f",Kp,Kd);
efvanmarrewijk 42:cef1b3187e4c 181 pc.printf ("r\t u_p = %f \tu_i = %f\t u_d = %f\r\n",u_k,u_i,u_d);
efvanmarrewijk 42:cef1b3187e4c 182 return u_k + u_d;
efvanmarrewijk 42:cef1b3187e4c 183 }
efvanmarrewijk 42:cef1b3187e4c 184
efvanmarrewijk 35:ba556f2d0fcc 185 float* turnr() // main function for movement of motor 1, all above functions with an extra tab are called
efvanmarrewijk 35:ba556f2d0fcc 186 {
efvanmarrewijk 35:ba556f2d0fcc 187 //float refvalue = pi/4.0f;
efvanmarrewijk 35:ba556f2d0fcc 188 float refvalue = DesiredAngler(); // different minus sign per motor
efvanmarrewijk 35:ba556f2d0fcc 189 int counts = Countsrinput(); // different encoder pins per motor
efvanmarrewijk 35:ba556f2d0fcc 190 float currentangle = CurrentAngle(counts); // different minus sign per motor
efvanmarrewijk 42:cef1b3187e4c 191 float PIDcontrol = PIDcontrollerr(refvalue,currentangle); // same for every motor
efvanmarrewijk 35:ba556f2d0fcc 192 float error = ErrorCalc(refvalue,currentangle); // same for every motor
efvanmarrewijk 35:ba556f2d0fcc 193
efvanmarrewijk 42:cef1b3187e4c 194 //pin5 = fabs(PIDcontrol); // different pins for every motor
efvanmarrewijk 37:c61d7768c18a 195 pin4 = PIDcontrol > 0.0f; // different pins for every motor
efvanmarrewijk 35:ba556f2d0fcc 196 //pin6 = 0.4+0.6*fabs(PIDcontr); //geschaald
efvanmarrewijk 35:ba556f2d0fcc 197 //pin6 = fabs(PIDcontr)/pi;
efvanmarrewijk 35:ba556f2d0fcc 198 //pc.printf(" error: %f ref: %f angle: %f \r\n",error,refvalue,currentangle);
efvanmarrewijk 42:cef1b3187e4c 199 //pc.printf(" countsr: %i currentangler: %f PIDr: %f errorr: %f\r\n",counts,currentangle,PIDcontrol,error);
efvanmarrewijk 35:ba556f2d0fcc 200 float* outcome;
efvanmarrewijk 35:ba556f2d0fcc 201 float turninfo[3] = {error, refvalue, currentangle};
efvanmarrewijk 33:ec07e11676ec 202 //float ( &fillarr( float (&outcome)[3] ) )[3] { // no decay; argument must be size 3
efvanmarrewijk 33:ec07e11676ec 203 outcome = turninfo;
efvanmarrewijk 33:ec07e11676ec 204 return outcome;
efvanmarrewijk 18:ca084c362855 205 }
efvanmarrewijk 25:76e9e5597416 206
efvanmarrewijk 32:a5b411833d1e 207 float ActualPosition(int counts, int offsetcounts) // After calibration, this function is used to return the counts (and thus the angle of the system) to 0
efvanmarrewijk 32:a5b411833d1e 208 { float MotorPosition = - (counts - offsetcounts) / fCountsRad;
efvanmarrewijk 25:76e9e5597416 209 // minus sign to correct for direction convention
efvanmarrewijk 32:a5b411833d1e 210 return MotorPosition;
efvanmarrewijk 25:76e9e5597416 211 }
efvanmarrewijk 11:3efd6a324f16 212
efvanmarrewijk 16:720365110953 213 // Main program
efvanmarrewijk 11:3efd6a324f16 214 int main()
efvanmarrewijk 18:ca084c362855 215 {
efvanmarrewijk 26:b48708ed51ff 216 pc.baud(115200);
efvanmarrewijk 32:a5b411833d1e 217 pin3.period_us(15); // If you give a period on one pin, c++ gives all pins this period
efvanmarrewijk 31:91ad5b188bd9 218
efvanmarrewijk 33:ec07e11676ec 219 //float motoroutcome1 = motor.attach(turn1,dt);
efvanmarrewijk 33:ec07e11676ec 220
efvanmarrewijk 26:b48708ed51ff 221 emergencybutton.rise(Emergency); //If the button is pressed, stop program
efvanmarrewijk 37:c61d7768c18a 222
efvanmarrewijk 16:720365110953 223 while (true)
efvanmarrewijk 25:76e9e5597416 224 {
efvanmarrewijk 40:1be9dfad0a10 225 turnl();
efvanmarrewijk 40:1be9dfad0a10 226 turnr();
efvanmarrewijk 40:1be9dfad0a10 227 wait(dt);
efvanmarrewijk 40:1be9dfad0a10 228 //float* motoroutcomel = turnl();
efvanmarrewijk 40:1be9dfad0a10 229 //float motorl1 = motoroutcomel[0];
efvanmarrewijk 40:1be9dfad0a10 230 //float motorl2 = motoroutcomel[1];
efvanmarrewijk 40:1be9dfad0a10 231 //float motorl3 = motoroutcomel[2];
efvanmarrewijk 40:1be9dfad0a10 232 //pc.printf(" errorl: %f refl: %f anglel: %f \r\n",motorl1,motorl2,motorl3);
efvanmarrewijk 33:ec07e11676ec 233
efvanmarrewijk 40:1be9dfad0a10 234 //float* motoroutcomer = turnr();
efvanmarrewijk 40:1be9dfad0a10 235 //float motorr1 = motoroutcomer[0];
efvanmarrewijk 40:1be9dfad0a10 236 //float motorr2 = motoroutcomer[1];
efvanmarrewijk 40:1be9dfad0a10 237 //float motorr3 = motoroutcomer[2];
efvanmarrewijk 40:1be9dfad0a10 238 //pc.printf(" errorr: %f refr: %f angler: %f \r\n",motorr1,motorr2,motorr3);
efvanmarrewijk 40:1be9dfad0a10 239
efvanmarrewijk 40:1be9dfad0a10 240 //wait(dt);
efvanmarrewijk 33:ec07e11676ec 241 //wait(dt*10);
efvanmarrewijk 33:ec07e11676ec 242 //wait(printingfreq); --> still needs to be defined
Ramonwaninge 3:d39285fdd103 243 }
efvanmarrewijk 30:c4a3e868ef04 244 }