Vivien van Veldhuizen / Mbed 2 deprecated Signa-bot

Signa-bot code for project BioRobotics, at University of Twente.

Dependencies:   mbed QEI MODSERIAL FastPWM ttmath Math

Motor_tryout.cpp@33:88fbf14d8aaf, 2019-10-31 (annotated)

Committer:
viviien
Date:
Thu Oct 31 15:02:17 2019 +0000
Revision:
33:88fbf14d8aaf
Parent:
32:60a71dcfdb7a
Child:
34:0af87f9cfb7b
Child:
35:4cb2ed6dd2d2
werkend rondje in xy vlak;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Feike 4:bd21569250c7 1 #include "mbed.h"
Feike 8:017b813c72bb 2 #include "MODSERIAL.h"
Feike 12:2382468d36a4 3 #include "QEI.h"
Feike 18:ab0fe311e7f3 4 #include "Math.h"
Feike 18:ab0fe311e7f3 5 #include "ttmath.h"
viviien 33:88fbf14d8aaf 6 #include "FastPWM.h"
Feike 18:ab0fe311e7f3 7
Feike 17:6da57acb7bea 8 MODSERIAL pc(USBTX, USBRX);
Feike 18:ab0fe311e7f3 9 //Serial term (USBTX, USBRX);
viviien 33:88fbf14d8aaf 10 FastPWM motor1_pwm(PTC2);
Feike 17:6da57acb7bea 11 DigitalOut motor1_dir(PTC3);
viviien 33:88fbf14d8aaf 12 FastPWM motor2_pwm(PTA2);
Feike 18:ab0fe311e7f3 13 DigitalOut motor2_dir(PTB23);
viviien 33:88fbf14d8aaf 14 FastPWM motor3_pwm(PTC4);
Feike 18:ab0fe311e7f3 15 DigitalOut motor3_dir(PTC12);
Feike 4:bd21569250c7 16
viviien 30:390cab7cd6e6 17 AnalogIn potmeter1(A1);
viviien 30:390cab7cd6e6 18
viviien 30:390cab7cd6e6 19
viviien 30:390cab7cd6e6 20
viviien 23:18b0be02187f 21 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
viviien 23:18b0be02187f 22 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
viviien 23:18b0be02187f 23 QEI Encoder3(D2,D3,NC,64,QEI::X4_ENCODING);
Feike 7:e119b12e5e7f 24
viviien 25:eb3204e45d33 25 float steps1 = 0;
viviien 25:eb3204e45d33 26 float steps2 = 0;
viviien 25:eb3204e45d33 27 float steps3 = 0;
Feike 19:9c8ab7922191 28 int check;
Feike 13:18dd7a15603f 29 int quit;
Feike 19:9c8ab7922191 30 int limit_pos = 8400;
Feike 19:9c8ab7922191 31 float steps;
Feike 19:9c8ab7922191 32 int g = 0;
Feike 19:9c8ab7922191 33
Feike 19:9c8ab7922191 34 bool check1;
Feike 19:9c8ab7922191 35 bool check2;
Feike 19:9c8ab7922191 36 bool check3;
Feike 18:ab0fe311e7f3 37 int counts1 = 0;
Feike 18:ab0fe311e7f3 38 int counts2 = 0;
Feike 18:ab0fe311e7f3 39 int counts3 = 0;
viviien 27:3eb181cbe183 40
Feike 13:18dd7a15603f 41
Feike 18:ab0fe311e7f3 42 const float le = 15.0;
Feike 18:ab0fe311e7f3 43 const float f = 37.5;
Feike 18:ab0fe311e7f3 44 const float re = 174.0;
Feike 18:ab0fe311e7f3 45 const float rf = 50.0;
Feike 18:ab0fe311e7f3 46 const float pi = 3.14159265358979323846;
viviien 22:9f911405e096 47 const float cospi = -0.5;
viviien 22:9f911405e096 48 const float sinpi = 0.8660254;
Feike 18:ab0fe311e7f3 49 float y2;
Feike 18:ab0fe311e7f3 50 float y1;
Feike 18:ab0fe311e7f3 51 float z1;
Feike 18:ab0fe311e7f3 52 float z2;
Feike 18:ab0fe311e7f3 53 float rje2;
Feike 18:ab0fe311e7f3 54 float rje;
Feike 18:ab0fe311e7f3 55 float r2;
Feike 18:ab0fe311e7f3 56 float r;
Feike 19:9c8ab7922191 57
Feike 18:ab0fe311e7f3 58 float z0=-172;
Feike 18:ab0fe311e7f3 59 float y0=0;
Feike 18:ab0fe311e7f3 60 float x0=0;
Feike 19:9c8ab7922191 61
Feike 18:ab0fe311e7f3 62 float theta1;
Feike 18:ab0fe311e7f3 63 float theta2;
Feike 18:ab0fe311e7f3 64 float theta3;
Feike 18:ab0fe311e7f3 65 float oldtheta1=0;
Feike 18:ab0fe311e7f3 66 float oldtheta2=0;
Feike 18:ab0fe311e7f3 67 float oldtheta3=0;
Feike 19:9c8ab7922191 68
viviien 28:43a1d67ff8ea 69 // Constant values for PI
viviien 30:390cab7cd6e6 70 float Kp;
viviien 30:390cab7cd6e6 71 float Kd;
viviien 30:390cab7cd6e6 72
viviien 30:390cab7cd6e6 73 float Ts = 0.0025;
viviien 28:43a1d67ff8ea 74 float error1 = 0;
viviien 30:390cab7cd6e6 75 float error2 = 0;
viviien 30:390cab7cd6e6 76 float error3 = 0;
viviien 28:43a1d67ff8ea 77
viviien 30:390cab7cd6e6 78 float newmotor1 = 1;
viviien 30:390cab7cd6e6 79 float newmotor2 = 1;
viviien 30:390cab7cd6e6 80 float newmotor3 = 1;
viviien 30:390cab7cd6e6 81
viviien 30:390cab7cd6e6 82 float u_k1 = 0;
viviien 30:390cab7cd6e6 83 float u_k2 = 0;
viviien 30:390cab7cd6e6 84 float u_k3 = 0;
viviien 30:390cab7cd6e6 85
viviien 30:390cab7cd6e6 86 float u_d1 = 0;
viviien 30:390cab7cd6e6 87 float u_d2 = 0;
viviien 30:390cab7cd6e6 88 float u_d3 = 0;
viviien 30:390cab7cd6e6 89
viviien 30:390cab7cd6e6 90 float lasterror1 = 0;
viviien 30:390cab7cd6e6 91 float lasterror2 = 0;
viviien 30:390cab7cd6e6 92 float lasterror3 = 0;
viviien 30:390cab7cd6e6 93
viviien 30:390cab7cd6e6 94 float derivative1 = 0;
viviien 30:390cab7cd6e6 95 float derivative2 = 0;
viviien 30:390cab7cd6e6 96 float derivative3 = 0;
viviien 30:390cab7cd6e6 97
viviien 32:60a71dcfdb7a 98 float angle1 = 0;
viviien 32:60a71dcfdb7a 99 float angle2 = 0;
viviien 32:60a71dcfdb7a 100 float angle3 = 0;
viviien 32:60a71dcfdb7a 101
viviien 32:60a71dcfdb7a 102 bool timer = false;
viviien 32:60a71dcfdb7a 103 float time_sin = 0;
viviien 32:60a71dcfdb7a 104
viviien 32:60a71dcfdb7a 105
viviien 32:60a71dcfdb7a 106 void delta_calctheta1 () {
viviien 32:60a71dcfdb7a 107 float y2 = y0 + le;
viviien 32:60a71dcfdb7a 108 float y1 = f;
viviien 32:60a71dcfdb7a 109 float z1 = 0.0;
viviien 32:60a71dcfdb7a 110 float z2 = z0;
viviien 32:60a71dcfdb7a 111 float rje2 = re*re - x0*x0;
viviien 32:60a71dcfdb7a 112 float rje = sqrt(rje2);
viviien 32:60a71dcfdb7a 113 float r2 = (y1-y2)*(y1-y2) + (z1-z0)*(z1-z0);
viviien 32:60a71dcfdb7a 114 float r = sqrt(r2);
viviien 32:60a71dcfdb7a 115
viviien 32:60a71dcfdb7a 116 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
viviien 32:60a71dcfdb7a 117 int check = 1;
viviien 32:60a71dcfdb7a 118 pc.printf("\n\rPunt bestaat niet");
viviien 32:60a71dcfdb7a 119 }
viviien 32:60a71dcfdb7a 120 else {
viviien 32:60a71dcfdb7a 121 float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
viviien 32:60a71dcfdb7a 122 float beta = atan((z1-z2)/(y1-y2));
viviien 32:60a71dcfdb7a 123 if(beta<0) {
viviien 32:60a71dcfdb7a 124 beta = beta + pi;
viviien 32:60a71dcfdb7a 125 }
viviien 32:60a71dcfdb7a 126 theta1 = (beta - alpha);
viviien 32:60a71dcfdb7a 127 }
viviien 32:60a71dcfdb7a 128 }
viviien 32:60a71dcfdb7a 129
viviien 32:60a71dcfdb7a 130 void delta_calctheta2 () {
viviien 32:60a71dcfdb7a 131 float xref = x0*cospi+y0*sinpi;
viviien 32:60a71dcfdb7a 132 float yref = y0*cospi-x0*sinpi;
viviien 32:60a71dcfdb7a 133 float zref = z0;
viviien 32:60a71dcfdb7a 134 float y2 = yref + le;
viviien 32:60a71dcfdb7a 135 float y1 = f;
viviien 32:60a71dcfdb7a 136 float z1 = 0.0;
viviien 32:60a71dcfdb7a 137 float z2 = zref;
viviien 32:60a71dcfdb7a 138 float rje2 = re*re - xref*xref;
viviien 32:60a71dcfdb7a 139 float rje = sqrt(rje2);
viviien 32:60a71dcfdb7a 140 float r2 = (y1-y2)*(y1-y2) + (z1-zref)*(z1-zref);
viviien 32:60a71dcfdb7a 141 float r = sqrt(r2);
viviien 32:60a71dcfdb7a 142
viviien 32:60a71dcfdb7a 143 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
viviien 32:60a71dcfdb7a 144 int check = 1;
viviien 32:60a71dcfdb7a 145 pc.printf("\n\rPunt bestaat niet");
viviien 32:60a71dcfdb7a 146 }
viviien 32:60a71dcfdb7a 147 else {
viviien 32:60a71dcfdb7a 148 float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
viviien 32:60a71dcfdb7a 149 float beta = atan((z1-z2)/(y1-y2));
viviien 32:60a71dcfdb7a 150 if(beta<0) {
viviien 32:60a71dcfdb7a 151 beta = beta + pi;
viviien 32:60a71dcfdb7a 152 }
viviien 32:60a71dcfdb7a 153 theta2 = (beta - alpha);
viviien 32:60a71dcfdb7a 154 }
viviien 32:60a71dcfdb7a 155 }
viviien 32:60a71dcfdb7a 156
viviien 32:60a71dcfdb7a 157 void delta_calctheta3 () {
viviien 32:60a71dcfdb7a 158 float xreff = x0*cospi-y0*sinpi;
viviien 32:60a71dcfdb7a 159 float yreff = y0*cospi+y0*sinpi;
viviien 32:60a71dcfdb7a 160 float zreff = z0;
viviien 32:60a71dcfdb7a 161 float y2 = yreff + le;
viviien 32:60a71dcfdb7a 162 float y1 = f;
viviien 32:60a71dcfdb7a 163 float z1 = 0.0;
viviien 32:60a71dcfdb7a 164 float z2 = zreff;
viviien 32:60a71dcfdb7a 165 float rje2 = re*re - xreff*xreff;
viviien 32:60a71dcfdb7a 166 float rje = sqrt(rje2);
viviien 32:60a71dcfdb7a 167 float r2 = (y1-y2)*(y1-y2) + (z1-zreff)*(z1-zreff);
viviien 32:60a71dcfdb7a 168 float r = sqrt(r2);
viviien 32:60a71dcfdb7a 169
viviien 32:60a71dcfdb7a 170 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
viviien 32:60a71dcfdb7a 171 int check = 1;
viviien 32:60a71dcfdb7a 172 pc.printf("\n\rPunt bestaat niet");
viviien 32:60a71dcfdb7a 173 }
viviien 32:60a71dcfdb7a 174 else {
viviien 32:60a71dcfdb7a 175 float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
viviien 32:60a71dcfdb7a 176 float beta = atan((z1-z2)/(y1-y2));
viviien 32:60a71dcfdb7a 177 if(beta<0) {
viviien 32:60a71dcfdb7a 178 beta = beta + pi;
viviien 32:60a71dcfdb7a 179 }
viviien 32:60a71dcfdb7a 180 theta3 = (beta - alpha);
viviien 32:60a71dcfdb7a 181 }
viviien 32:60a71dcfdb7a 182 }
viviien 32:60a71dcfdb7a 183
viviien 32:60a71dcfdb7a 184
viviien 32:60a71dcfdb7a 185
viviien 30:390cab7cd6e6 186
viviien 30:390cab7cd6e6 187 Ticker motor_timer;
viviien 32:60a71dcfdb7a 188 void motor(){
viviien 32:60a71dcfdb7a 189
viviien 30:390cab7cd6e6 190 counts1 = Encoder1.getPulses();
viviien 30:390cab7cd6e6 191 counts2 = Encoder2.getPulses();
viviien 30:390cab7cd6e6 192 counts3 = Encoder3.getPulses();
viviien 32:60a71dcfdb7a 193
viviien 33:88fbf14d8aaf 194 // z0 = -172.0 + 10* sin(time_sin);
viviien 33:88fbf14d8aaf 195
viviien 33:88fbf14d8aaf 196 float r = 20*(1-1/(1+(time_sin)));
viviien 33:88fbf14d8aaf 197 x0 = r*sin(6.28 * time_sin);
viviien 33:88fbf14d8aaf 198 y0 = r*cos(6.28 * time_sin);
viviien 33:88fbf14d8aaf 199
viviien 29:7eb028b359a1 200
viviien 32:60a71dcfdb7a 201 delta_calctheta1 ();
viviien 32:60a71dcfdb7a 202 delta_calctheta2 ();
viviien 32:60a71dcfdb7a 203 delta_calctheta3 ();
viviien 30:390cab7cd6e6 204
viviien 32:60a71dcfdb7a 205 angle1 = counts1/(8400.0)*2.0*pi;
viviien 32:60a71dcfdb7a 206 angle2 = counts2/(8400.0)*2.0*pi;
viviien 32:60a71dcfdb7a 207 angle3 = counts3/(8400.0)*2.0*pi;
viviien 30:390cab7cd6e6 208
viviien 32:60a71dcfdb7a 209 error1 = theta1 - angle1;
viviien 32:60a71dcfdb7a 210 error2 = theta2 - angle2;
viviien 32:60a71dcfdb7a 211 error3 = theta3 - angle3;
viviien 30:390cab7cd6e6 212
viviien 33:88fbf14d8aaf 213 Kp = potmeter1 * 25;
viviien 30:390cab7cd6e6 214
viviien 33:88fbf14d8aaf 215 u_k1 = Kp * error1;
viviien 30:390cab7cd6e6 216 u_k2 = Kp * error2;
viviien 30:390cab7cd6e6 217 u_k3 = Kp * error3;
viviien 30:390cab7cd6e6 218
viviien 32:60a71dcfdb7a 219 newmotor1 = u_k1;
viviien 32:60a71dcfdb7a 220 newmotor2 = u_k2;
viviien 32:60a71dcfdb7a 221 newmotor3 = u_k3;
viviien 30:390cab7cd6e6 222
viviien 30:390cab7cd6e6 223 if (newmotor1>1.0f){
viviien 30:390cab7cd6e6 224 newmotor1 =1.0f;
viviien 23:18b0be02187f 225 }
viviien 30:390cab7cd6e6 226 if (newmotor1<-1.0f){
viviien 30:390cab7cd6e6 227 newmotor1 = -1.0f;
viviien 30:390cab7cd6e6 228 }
viviien 30:390cab7cd6e6 229
viviien 30:390cab7cd6e6 230 if (newmotor2>1.0f){
viviien 30:390cab7cd6e6 231 newmotor2 =1.0f;
viviien 30:390cab7cd6e6 232 }
viviien 30:390cab7cd6e6 233 if (newmotor2<-1.0f){
viviien 30:390cab7cd6e6 234 newmotor2 = -1.0f;
viviien 30:390cab7cd6e6 235 }
viviien 30:390cab7cd6e6 236
viviien 30:390cab7cd6e6 237 if (newmotor3>1.0f){
viviien 30:390cab7cd6e6 238 newmotor3 =1.0f;
viviien 30:390cab7cd6e6 239 }
viviien 30:390cab7cd6e6 240 if (newmotor3<-1.0f){
viviien 30:390cab7cd6e6 241 newmotor3 = -1.0f;
viviien 30:390cab7cd6e6 242 }
viviien 30:390cab7cd6e6 243
viviien 30:390cab7cd6e6 244 motor1_pwm.write(fabs(newmotor1));
viviien 32:60a71dcfdb7a 245 motor1_dir.write(newmotor1<0);
viviien 30:390cab7cd6e6 246
viviien 30:390cab7cd6e6 247 motor2_pwm.write(fabs(newmotor2));
viviien 32:60a71dcfdb7a 248 motor2_dir.write(newmotor2>0);
viviien 30:390cab7cd6e6 249
viviien 30:390cab7cd6e6 250 motor3_pwm.write(fabs(newmotor3));
viviien 32:60a71dcfdb7a 251 motor3_dir.write(newmotor3<0);
viviien 30:390cab7cd6e6 252
viviien 32:60a71dcfdb7a 253 time_sin += 0.002;
viviien 30:390cab7cd6e6 254 }
viviien 25:eb3204e45d33 255
viviien 32:60a71dcfdb7a 256
viviien 28:43a1d67ff8ea 257
viviien 28:43a1d67ff8ea 258 ///////////////////
viviien 28:43a1d67ff8ea 259 // MAIN FUNCTION //
viviien 28:43a1d67ff8ea 260 ///////////////////
viviien 28:43a1d67ff8ea 261
viviien 22:9f911405e096 262 int main(void) {
viviien 30:390cab7cd6e6 263 motor_timer.attach(&motor, 0.002);
viviien 26:432d3519ba86 264
viviien 28:43a1d67ff8ea 265 pc.baud(115200);
Feike 17:6da57acb7bea 266 char cc = pc.getc();
viviien 30:390cab7cd6e6 267
viviien 30:390cab7cd6e6 268 int frequency_pwm = 10000; //10 kHz PWM
viviien 33:88fbf14d8aaf 269 motor1_pwm.period_us(6);
viviien 33:88fbf14d8aaf 270 motor2_pwm.period_us(6);
viviien 33:88fbf14d8aaf 271 motor3_pwm.period_us(6);
Feike 19:9c8ab7922191 272
viviien 28:43a1d67ff8ea 273 while(true){
viviien 33:88fbf14d8aaf 274
viviien 33:88fbf14d8aaf 275
viviien 22:9f911405e096 276 }
viviien 32:60a71dcfdb7a 277 //END MAIN
viviien 22:9f911405e096 278 }