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@16:29d3851cfd52, 2019-10-22 (annotated)

Committer:
paulstuiver
Date:
Tue Oct 22 15:05:12 2019 +0000
Revision:
16:29d3851cfd52
Parent:
11:ca91fc47ad02
Child:
20:e20c26a1f6ba
Insane double motor control (at least kind of)

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