Floris Hoek / Mbed 2 deprecated template_biorobotics_Group_18

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:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@20:e20c26a1f6ba, 2019-10-25 (annotated)

Committer:
paulstuiver
Date:
Fri Oct 25 14:19:01 2019 +0000
Revision:
20:e20c26a1f6ba
Parent:
16:29d3851cfd52
Child:
21:245c676f9d72
Sick tuning and demo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
RobertoO 1:b862262a9d14 2 #include "MODSERIAL.h"
paulstuiver 2:75b2f713161c 3 #include "FastPWM.h"
paulstuiver 5:2ae500da8fe1 4 #include "QEI.h"
paulstuiver 5:2ae500da8fe1 5 #include <math.h>
paulstuiver 6:ea3b3f50c893 6 #include "BiQuad.h"
paulstuiver 2:75b2f713161c 7
paulstuiver 16:29d3851cfd52 8 // To play with buttons and potmeters
paulstuiver 2:75b2f713161c 9 AnalogIn pot2(A0);
paulstuiver 7:08fd3bc7a3cf 10 AnalogIn pot1(A1);
paulstuiver 20:e20c26a1f6ba 11 DigitalIn but1(D2);
paulstuiver 20:e20c26a1f6ba 12 DigitalIn but2(D3);
paulstuiver 20:e20c26a1f6ba 13 DigitalIn but3(SW2);
paulstuiver 20:e20c26a1f6ba 14
paulstuiver 16:29d3851cfd52 15
paulstuiver 16:29d3851cfd52 16 // Usual stuff
paulstuiver 6:ea3b3f50c893 17 MODSERIAL pc(USBTX, USBRX);
paulstuiver 16:29d3851cfd52 18
paulstuiver 16:29d3851cfd52 19 //ticker to call motor values
paulstuiver 16:29d3851cfd52 20 Ticker motor;
paulstuiver 16:29d3851cfd52 21
paulstuiver 16:29d3851cfd52 22 // Direction and Velocity of the motors
paulstuiver 16:29d3851cfd52 23 DigitalOut motor1Direction(D7);
paulstuiver 16:29d3851cfd52 24 FastPWM motor1Velocity(D6);
paulstuiver 16:29d3851cfd52 25 DigitalOut motor2Direction(D4);
paulstuiver 16:29d3851cfd52 26 FastPWM motor2Velocity(D5);
paulstuiver 16:29d3851cfd52 27
paulstuiver 16:29d3851cfd52 28 // Encoders 1 and 2 respectively
paulstuiver 16:29d3851cfd52 29 QEI Encoder1(D8,D9,NC,8400);
paulstuiver 16:29d3851cfd52 30 QEI Encoder2(D11,D10,NC,8400);
paulstuiver 7:08fd3bc7a3cf 31
paulstuiver 7:08fd3bc7a3cf 32
paulstuiver 2:75b2f713161c 33 volatile double frequency = 17000.0;
paulstuiver 2:75b2f713161c 34 volatile double period_signal = 1.0/frequency;
paulstuiver 7:08fd3bc7a3cf 35 float timeinterval = 0.001f;
paulstuiver 16:29d3851cfd52 36 float motorvalue1;
paulstuiver 16:29d3851cfd52 37 float motorvalue2;
paulstuiver 16:29d3851cfd52 38 float yendeffector = 10.6159;
paulstuiver 11:ca91fc47ad02 39 float xendeffector = 0;
paulstuiver 20:e20c26a1f6ba 40 float potmeter1 = 5+pot1.read()*60;
paulstuiver 20:e20c26a1f6ba 41 float potmeter2 = pot2.read()*60;
paulstuiver 11:ca91fc47ad02 42
paulstuiver 16:29d3851cfd52 43 // ANTI WIND UP NEEDED
paulstuiver 6:ea3b3f50c893 44
paulstuiver 6:ea3b3f50c893 45 // -------------------- README ------------------------------------
paulstuiver 6:ea3b3f50c893 46 // positive referenceposition corresponds to a counterclockwise motion
paulstuiver 6:ea3b3f50c893 47 // negative referenceposition corresponds to a clockwise motion
paulstuiver 6:ea3b3f50c893 48
paulstuiver 6:ea3b3f50c893 49 //PID control implementation (BiQuead)
paulstuiver 16:29d3851cfd52 50 double PID_controller1(float error1)
paulstuiver 5:2ae500da8fe1 51 {
paulstuiver 16:29d3851cfd52 52 // Define errors for motor 1 and 2
paulstuiver 16:29d3851cfd52 53 static double error_integral1 = 0;
paulstuiver 16:29d3851cfd52 54 static double error_prev1 = error1;
paulstuiver 16:29d3851cfd52 55
paulstuiver 16:29d3851cfd52 56 // Low-pass filter
paulstuiver 6:ea3b3f50c893 57 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
paulstuiver 16:29d3851cfd52 58
paulstuiver 16:29d3851cfd52 59 // PID variables: we assume them to be the same for both motors
paulstuiver 20:e20c26a1f6ba 60 float Kp = 64.9;
paulstuiver 20:e20c26a1f6ba 61 float Ki = 3.64;
paulstuiver 20:e20c26a1f6ba 62 float Kd = 5;
paulstuiver 6:ea3b3f50c893 63
paulstuiver 5:2ae500da8fe1 64 //Proportional part:
paulstuiver 16:29d3851cfd52 65 double u_k1 = Kp * error1;
paulstuiver 5:2ae500da8fe1 66
paulstuiver 6:ea3b3f50c893 67 // Integreal part
paulstuiver 16:29d3851cfd52 68 error_integral1 = error_integral1 + error1 * timeinterval;
paulstuiver 16:29d3851cfd52 69 double u_i1 = Ki*error_integral1;
paulstuiver 16:29d3851cfd52 70
paulstuiver 6:ea3b3f50c893 71 // Derivate part
paulstuiver 16:29d3851cfd52 72 double error_derivative1 = (error1 - error_prev1)/timeinterval;
paulstuiver 16:29d3851cfd52 73 double filtered_error_derivative1 = LowPassFilter.step(error_derivative1);
paulstuiver 16:29d3851cfd52 74 double u_d1 = Kd * filtered_error_derivative1;
paulstuiver 16:29d3851cfd52 75 error_prev1 = error1;
paulstuiver 6:ea3b3f50c893 76
paulstuiver 5:2ae500da8fe1 77 //sum all parts and return it
paulstuiver 16:29d3851cfd52 78 return u_k1 + u_i1 + u_d1;
paulstuiver 16:29d3851cfd52 79 }
paulstuiver 16:29d3851cfd52 80
paulstuiver 16:29d3851cfd52 81 double PID_controller2(float error2)
paulstuiver 16:29d3851cfd52 82 {
paulstuiver 16:29d3851cfd52 83 // Define errors for motor 1 and 2
paulstuiver 16:29d3851cfd52 84 static double error_integral2 = 0;
paulstuiver 16:29d3851cfd52 85 static double error_prev2 = error2;
paulstuiver 16:29d3851cfd52 86
paulstuiver 16:29d3851cfd52 87 // Low-pass filter
paulstuiver 16:29d3851cfd52 88 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
paulstuiver 16:29d3851cfd52 89
paulstuiver 16:29d3851cfd52 90 // PID variables: we assume them to be the same for both motors
paulstuiver 20:e20c26a1f6ba 91 float Kp = 64.9;
paulstuiver 20:e20c26a1f6ba 92 float Ki = 3.64;
paulstuiver 20:e20c26a1f6ba 93 float Kd = 5;
paulstuiver 16:29d3851cfd52 94
paulstuiver 16:29d3851cfd52 95 //Proportional part:
paulstuiver 16:29d3851cfd52 96 double u_k2 = Kp * error2;
paulstuiver 16:29d3851cfd52 97
paulstuiver 16:29d3851cfd52 98 // Integreal part
paulstuiver 16:29d3851cfd52 99 error_integral2 = error_integral2 + error2 * timeinterval;
paulstuiver 16:29d3851cfd52 100 double u_i2 = Ki*error_integral2;
paulstuiver 16:29d3851cfd52 101
paulstuiver 16:29d3851cfd52 102 // Derivate part
paulstuiver 16:29d3851cfd52 103 double error_derivative2 = (error2 - error_prev2)/timeinterval;
paulstuiver 16:29d3851cfd52 104 double filtered_error_derivative2 = LowPassFilter.step(error_derivative2);
paulstuiver 16:29d3851cfd52 105 double u_d2 = Kd * filtered_error_derivative2;
paulstuiver 16:29d3851cfd52 106 error_prev2 = error2;
paulstuiver 16:29d3851cfd52 107
paulstuiver 16:29d3851cfd52 108 //sum all parts and return it
paulstuiver 16:29d3851cfd52 109 return u_k2 + u_i2 + u_d2;
paulstuiver 5:2ae500da8fe1 110 }
paulstuiver 2:75b2f713161c 111
paulstuiver 6:ea3b3f50c893 112 //get the measured position
paulstuiver 16:29d3851cfd52 113 void getmeasuredposition(float & measuredposition1, float & measuredposition2)
paulstuiver 2:75b2f713161c 114 {
paulstuiver 16:29d3851cfd52 115 // Obtain the counts of motors 1 and 2 from the encoder
paulstuiver 16:29d3851cfd52 116 int countsmotor1;
paulstuiver 16:29d3851cfd52 117 int countsmotor2;
paulstuiver 16:29d3851cfd52 118 countsmotor1 = Encoder1.getPulses();
paulstuiver 16:29d3851cfd52 119 countsmotor2 = Encoder2.getPulses();
paulstuiver 6:ea3b3f50c893 120
paulstuiver 16:29d3851cfd52 121 // Obtain the measured position for motor 1 and 2
paulstuiver 16:29d3851cfd52 122 measuredposition1 = ((float)countsmotor1) / 8400.0f * 2.0f;
paulstuiver 16:29d3851cfd52 123 measuredposition2 = ((float)countsmotor2) / 8400.0f * 2.0f;
paulstuiver 2:75b2f713161c 124 }
paulstuiver 2:75b2f713161c 125
paulstuiver 16:29d3851cfd52 126 //get the reference of the
paulstuiver 16:29d3851cfd52 127 void getreferenceposition(float & referenceposition1, float & referenceposition2)
paulstuiver 2:75b2f713161c 128 {
paulstuiver 16:29d3851cfd52 129 //Measurements of the arm
paulstuiver 16:29d3851cfd52 130 float L0=1.95;
paulstuiver 11:ca91fc47ad02 131 float L1=15;
paulstuiver 11:ca91fc47ad02 132 float L2=20;
paulstuiver 16:29d3851cfd52 133
paulstuiver 16:29d3851cfd52 134 //Define the new counts that are needed
paulstuiver 16:29d3851cfd52 135 float desiredmotorangle1;
paulstuiver 16:29d3851cfd52 136 float desiredmotorangle2;
paulstuiver 16:29d3851cfd52 137
paulstuiver 16:29d3851cfd52 138 //Inverse kinematics: given the end position, what are the desired motor angles of 1 and 2
paulstuiver 16:29d3851cfd52 139 desiredmotorangle1 = atan2(yendeffector,(L0+xendeffector))*180/3.1415 + acos((pow(L1,2)+pow(L0+xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0+xendeffector,2)+pow(yendeffector,2))))*180/3.1415;
paulstuiver 16:29d3851cfd52 140 desiredmotorangle2 = atan2(yendeffector,(L0-xendeffector))*180/3.1415 + acos((pow(L1,2)+pow(L0-xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0-xendeffector,2)+pow(yendeffector,2))))*180/3.1415;
paulstuiver 16:29d3851cfd52 141
paulstuiver 16:29d3851cfd52 142 //Convert motor angles to counts
paulstuiver 16:29d3851cfd52 143 float desiredmotorrounds1;
paulstuiver 16:29d3851cfd52 144 float desiredmotorrounds2;
paulstuiver 16:29d3851cfd52 145 desiredmotorrounds1 = (desiredmotorangle1-180)/360;
paulstuiver 16:29d3851cfd52 146 desiredmotorrounds2 = (desiredmotorangle2-180)/360;
paulstuiver 16:29d3851cfd52 147
paulstuiver 16:29d3851cfd52 148 //Assign this to new variables because otherwise it doesn't work
paulstuiver 16:29d3851cfd52 149 referenceposition1 = desiredmotorrounds1;
paulstuiver 16:29d3851cfd52 150 referenceposition2 = desiredmotorrounds2;
paulstuiver 2:75b2f713161c 151 }
RobertoO 0:67c50348f842 152
paulstuiver 2:75b2f713161c 153 //send value to motor
paulstuiver 16:29d3851cfd52 154 void sendtomotor(float & motorvalue1, float & motorvalue2)
paulstuiver 2:75b2f713161c 155 {
paulstuiver 16:29d3851cfd52 156 // Define the absolute motor values
paulstuiver 16:29d3851cfd52 157 float absolutemotorvalue1 = 0.0f;
paulstuiver 16:29d3851cfd52 158 float absolutemotorvalue2 = 0.0f;
paulstuiver 16:29d3851cfd52 159 absolutemotorvalue1 = fabs(motorvalue1);
paulstuiver 16:29d3851cfd52 160 absolutemotorvalue2 = fabs(motorvalue2);
paulstuiver 16:29d3851cfd52 161
paulstuiver 16:29d3851cfd52 162 // If absolutemotorvalueocity is greater than 1, reduce to 1, otherwise remain absolutemotorvalue
paulstuiver 16:29d3851cfd52 163 absolutemotorvalue1 = absolutemotorvalue1 > 1.0f ? 1.0f : absolutemotorvalue1;
paulstuiver 16:29d3851cfd52 164 absolutemotorvalue2 = absolutemotorvalue2 > 1.0f ? 1.0f : absolutemotorvalue2;
paulstuiver 16:29d3851cfd52 165
paulstuiver 16:29d3851cfd52 166 // Send the absolutemotorvalue to the motors
paulstuiver 16:29d3851cfd52 167 motor1Velocity = absolutemotorvalue1;
paulstuiver 16:29d3851cfd52 168 motor2Velocity = absolutemotorvalue2;
paulstuiver 16:29d3851cfd52 169
paulstuiver 16:29d3851cfd52 170 // Determine the motor direction. Boolean output: true gives counterclockwise direction, false gives clockwise direction
paulstuiver 16:29d3851cfd52 171 motor1Direction = (motorvalue1 > 0.0f);
paulstuiver 16:29d3851cfd52 172 motor2Direction = (motorvalue2 > 0.0f);
paulstuiver 2:75b2f713161c 173 }
RobertoO 0:67c50348f842 174
paulstuiver 7:08fd3bc7a3cf 175 // function to call reference absolutemotorvalueocity, measured absolutemotorvalueocity and controls motor with feedback
paulstuiver 2:75b2f713161c 176 void measureandcontrol(void)
paulstuiver 2:75b2f713161c 177 {
paulstuiver 16:29d3851cfd52 178 // Get the reference positions of motor 1 and 2
paulstuiver 16:29d3851cfd52 179 float reference1, reference2;
paulstuiver 16:29d3851cfd52 180 getreferenceposition(reference1, reference2);
paulstuiver 16:29d3851cfd52 181
paulstuiver 16:29d3851cfd52 182 // Get the measured positions of motor 1 and 2
paulstuiver 16:29d3851cfd52 183 float measured1, measured2;
paulstuiver 16:29d3851cfd52 184 getmeasuredposition(measured1, measured2);
paulstuiver 16:29d3851cfd52 185
paulstuiver 16:29d3851cfd52 186 // Calculate the motor values
paulstuiver 16:29d3851cfd52 187 float motorvalue1, motorvalue2;
paulstuiver 16:29d3851cfd52 188 motorvalue1 = PID_controller1(reference1 - measured1);
paulstuiver 16:29d3851cfd52 189 motorvalue2 = PID_controller2(reference2 - measured2);
paulstuiver 16:29d3851cfd52 190 sendtomotor(motorvalue1, motorvalue2);
paulstuiver 2:75b2f713161c 191 }
RobertoO 0:67c50348f842 192 int main()
RobertoO 0:67c50348f842 193 {
paulstuiver 16:29d3851cfd52 194 // Usual stuff
RobertoO 0:67c50348f842 195 pc.baud(115200);
paulstuiver 2:75b2f713161c 196 pc.printf("Starting...\r\n");
paulstuiver 16:29d3851cfd52 197
paulstuiver 16:29d3851cfd52 198 // Something with pulses
paulstuiver 16:29d3851cfd52 199 motor1Velocity.period(period_signal);
paulstuiver 16:29d3851cfd52 200 motor2Velocity.period(period_signal);
paulstuiver 16:29d3851cfd52 201
paulstuiver 16:29d3851cfd52 202 // Call the ticker function
paulstuiver 6:ea3b3f50c893 203 motor.attach(measureandcontrol, timeinterval);
paulstuiver 2:75b2f713161c 204 while(true)
paulstuiver 2:75b2f713161c 205 {
paulstuiver 20:e20c26a1f6ba 206 wait(0.01);
paulstuiver 20:e20c26a1f6ba 207 float potmeter1 = 5+pot1.read()*60;
paulstuiver 20:e20c26a1f6ba 208 float potmeter2 = pot2.read()*60;
paulstuiver 16:29d3851cfd52 209 pc.printf("Kp gives %f\r\n", potmeter1);
paulstuiver 16:29d3851cfd52 210 pc.printf("Ki gives %f\r\n", potmeter2);
paulstuiver 11:ca91fc47ad02 211 pc.printf("x is %f\r\n",xendeffector);
paulstuiver 20:e20c26a1f6ba 212 pc.printf("y is %f\r\n",yendeffector);
paulstuiver 11:ca91fc47ad02 213
paulstuiver 20:e20c26a1f6ba 214 // Control position
paulstuiver 20:e20c26a1f6ba 215 char a = pc.getcNb();
paulstuiver 11:ca91fc47ad02 216
paulstuiver 20:e20c26a1f6ba 217 if(a == 'r' && xendeffector < 14){
paulstuiver 20:e20c26a1f6ba 218 xendeffector=xendeffector+1;}
paulstuiver 20:e20c26a1f6ba 219 else if(a=='l' && xendeffector > -14){
paulstuiver 16:29d3851cfd52 220 xendeffector=xendeffector-1;}
paulstuiver 20:e20c26a1f6ba 221 else if(a=='u' && yendeffector < 30){
paulstuiver 16:29d3851cfd52 222 yendeffector=yendeffector+1;}
paulstuiver 20:e20c26a1f6ba 223 else if(a=='d' && yendeffector > 5){
paulstuiver 16:29d3851cfd52 224 yendeffector=yendeffector-1;}
paulstuiver 20:e20c26a1f6ba 225
paulstuiver 20:e20c26a1f6ba 226 // Go back to starting position
paulstuiver 20:e20c26a1f6ba 227 if (but1.read() == 0) {
paulstuiver 20:e20c26a1f6ba 228 xendeffector=0;
paulstuiver 20:e20c26a1f6ba 229 yendeffector=10.6159;
paulstuiver 20:e20c26a1f6ba 230 }
paulstuiver 20:e20c26a1f6ba 231
paulstuiver 20:e20c26a1f6ba 232 if (but2.read() == 0){
paulstuiver 20:e20c26a1f6ba 233 xendeffector=-5;
paulstuiver 20:e20c26a1f6ba 234 yendeffector=10.6159;
paulstuiver 20:e20c26a1f6ba 235 wait(0.5);
paulstuiver 20:e20c26a1f6ba 236
paulstuiver 20:e20c26a1f6ba 237 xendeffector=10;
paulstuiver 20:e20c26a1f6ba 238 yendeffector=25.6159;
paulstuiver 20:e20c26a1f6ba 239 wait(0.5);
paulstuiver 20:e20c26a1f6ba 240
paulstuiver 20:e20c26a1f6ba 241 xendeffector=-14;
paulstuiver 20:e20c26a1f6ba 242 yendeffector=21.6159;
paulstuiver 20:e20c26a1f6ba 243 wait(0.5);
paulstuiver 20:e20c26a1f6ba 244
paulstuiver 20:e20c26a1f6ba 245 xendeffector=10;
paulstuiver 20:e20c26a1f6ba 246 yendeffector=11.6159;
paulstuiver 20:e20c26a1f6ba 247 wait(0.5);
paulstuiver 20:e20c26a1f6ba 248
paulstuiver 20:e20c26a1f6ba 249 xendeffector=0;
paulstuiver 20:e20c26a1f6ba 250 yendeffector=10.6159;
paulstuiver 20:e20c26a1f6ba 251 wait(0.5);
paulstuiver 20:e20c26a1f6ba 252 }
paulstuiver 20:e20c26a1f6ba 253
paulstuiver 20:e20c26a1f6ba 254 if (but3.read() ==0){
paulstuiver 20:e20c26a1f6ba 255
paulstuiver 20:e20c26a1f6ba 256 xendeffector=-5;
paulstuiver 20:e20c26a1f6ba 257 yendeffector=10.6159;
paulstuiver 20:e20c26a1f6ba 258 int i=0;
paulstuiver 20:e20c26a1f6ba 259 wait(0.5);
paulstuiver 20:e20c26a1f6ba 260 while(i<100){
paulstuiver 20:e20c26a1f6ba 261 xendeffector=xendeffector+0.1;
paulstuiver 20:e20c26a1f6ba 262 yendeffector=yendeffector+0.1;
paulstuiver 20:e20c26a1f6ba 263 i++;
paulstuiver 20:e20c26a1f6ba 264 wait(0.001);
paulstuiver 20:e20c26a1f6ba 265 }
paulstuiver 20:e20c26a1f6ba 266 }
paulstuiver 20:e20c26a1f6ba 267
paulstuiver 20:e20c26a1f6ba 268
paulstuiver 11:ca91fc47ad02 269
paulstuiver 11:ca91fc47ad02 270
paulstuiver 20:e20c26a1f6ba 271 // type c to stop the program
paulstuiver 6:ea3b3f50c893 272 char c = pc.getcNb();
paulstuiver 6:ea3b3f50c893 273 if (c == 'c')
paulstuiver 6:ea3b3f50c893 274 {
paulstuiver 7:08fd3bc7a3cf 275 pc.printf("Program stopped running");
paulstuiver 16:29d3851cfd52 276 motorvalue1 = 0;
paulstuiver 16:29d3851cfd52 277 motorvalue2 = 0;
paulstuiver 16:29d3851cfd52 278 sendtomotor(motorvalue1, motorvalue2);
paulstuiver 7:08fd3bc7a3cf 279 break;
paulstuiver 6:ea3b3f50c893 280 }
RobertoO 0:67c50348f842 281 }
paulstuiver 2:75b2f713161c 282 }