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@34:0af87f9cfb7b, 2019-10-31 (annotated)

Committer:
viviien
Date:
Thu Oct 31 15:16:16 2019 +0000
Revision:
34:0af87f9cfb7b
Parent:
33:88fbf14d8aaf 
Werkend met cc.get

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 34:0af87f9cfb7b 196 //float r = 20*(1-1/(1+(time_sin)));
viviien 34:0af87f9cfb7b 197 // x0 = r*sin(6.28 * time_sin);
viviien 34:0af87f9cfb7b 198 // y0 = r*cos(6.28 * time_sin);
viviien 29:7eb028b359a1 199
viviien 32:60a71dcfdb7a 200 delta_calctheta1 ();
viviien 32:60a71dcfdb7a 201 delta_calctheta2 ();
viviien 32:60a71dcfdb7a 202 delta_calctheta3 ();
viviien 30:390cab7cd6e6 203
viviien 32:60a71dcfdb7a 204 angle1 = counts1/(8400.0)*2.0*pi;
viviien 32:60a71dcfdb7a 205 angle2 = counts2/(8400.0)*2.0*pi;
viviien 32:60a71dcfdb7a 206 angle3 = counts3/(8400.0)*2.0*pi;
viviien 30:390cab7cd6e6 207
viviien 32:60a71dcfdb7a 208 error1 = theta1 - angle1;
viviien 32:60a71dcfdb7a 209 error2 = theta2 - angle2;
viviien 32:60a71dcfdb7a 210 error3 = theta3 - angle3;
viviien 30:390cab7cd6e6 211
viviien 33:88fbf14d8aaf 212 Kp = potmeter1 * 25;
viviien 30:390cab7cd6e6 213
viviien 33:88fbf14d8aaf 214 u_k1 = Kp * error1;
viviien 30:390cab7cd6e6 215 u_k2 = Kp * error2;
viviien 30:390cab7cd6e6 216 u_k3 = Kp * error3;
viviien 30:390cab7cd6e6 217
viviien 32:60a71dcfdb7a 218 newmotor1 = u_k1;
viviien 32:60a71dcfdb7a 219 newmotor2 = u_k2;
viviien 32:60a71dcfdb7a 220 newmotor3 = u_k3;
viviien 30:390cab7cd6e6 221
viviien 30:390cab7cd6e6 222 if (newmotor1>1.0f){
viviien 30:390cab7cd6e6 223 newmotor1 =1.0f;
viviien 23:18b0be02187f 224 }
viviien 30:390cab7cd6e6 225 if (newmotor1<-1.0f){
viviien 30:390cab7cd6e6 226 newmotor1 = -1.0f;
viviien 30:390cab7cd6e6 227 }
viviien 30:390cab7cd6e6 228
viviien 30:390cab7cd6e6 229 if (newmotor2>1.0f){
viviien 30:390cab7cd6e6 230 newmotor2 =1.0f;
viviien 30:390cab7cd6e6 231 }
viviien 30:390cab7cd6e6 232 if (newmotor2<-1.0f){
viviien 30:390cab7cd6e6 233 newmotor2 = -1.0f;
viviien 30:390cab7cd6e6 234 }
viviien 30:390cab7cd6e6 235
viviien 30:390cab7cd6e6 236 if (newmotor3>1.0f){
viviien 30:390cab7cd6e6 237 newmotor3 =1.0f;
viviien 30:390cab7cd6e6 238 }
viviien 30:390cab7cd6e6 239 if (newmotor3<-1.0f){
viviien 30:390cab7cd6e6 240 newmotor3 = -1.0f;
viviien 30:390cab7cd6e6 241 }
viviien 30:390cab7cd6e6 242
viviien 30:390cab7cd6e6 243 motor1_pwm.write(fabs(newmotor1));
viviien 32:60a71dcfdb7a 244 motor1_dir.write(newmotor1<0);
viviien 30:390cab7cd6e6 245
viviien 30:390cab7cd6e6 246 motor2_pwm.write(fabs(newmotor2));
viviien 32:60a71dcfdb7a 247 motor2_dir.write(newmotor2>0);
viviien 30:390cab7cd6e6 248
viviien 30:390cab7cd6e6 249 motor3_pwm.write(fabs(newmotor3));
viviien 32:60a71dcfdb7a 250 motor3_dir.write(newmotor3<0);
viviien 30:390cab7cd6e6 251
viviien 32:60a71dcfdb7a 252 time_sin += 0.002;
viviien 30:390cab7cd6e6 253 }
viviien 25:eb3204e45d33 254
viviien 32:60a71dcfdb7a 255
viviien 28:43a1d67ff8ea 256
viviien 28:43a1d67ff8ea 257 ///////////////////
viviien 28:43a1d67ff8ea 258 // MAIN FUNCTION //
viviien 28:43a1d67ff8ea 259 ///////////////////
viviien 28:43a1d67ff8ea 260
viviien 22:9f911405e096 261 int main(void) {
viviien 30:390cab7cd6e6 262 motor_timer.attach(&motor, 0.002);
viviien 26:432d3519ba86 263
viviien 28:43a1d67ff8ea 264 pc.baud(115200);
Feike 17:6da57acb7bea 265 char cc = pc.getc();
viviien 30:390cab7cd6e6 266
viviien 30:390cab7cd6e6 267 int frequency_pwm = 10000; //10 kHz PWM
viviien 33:88fbf14d8aaf 268 motor1_pwm.period_us(6);
viviien 33:88fbf14d8aaf 269 motor2_pwm.period_us(6);
viviien 33:88fbf14d8aaf 270 motor3_pwm.period_us(6);
Feike 19:9c8ab7922191 271
viviien 28:43a1d67ff8ea 272 while(true){
viviien 34:0af87f9cfb7b 273 char cc = pc.getc();
viviien 34:0af87f9cfb7b 274
viviien 34:0af87f9cfb7b 275 if (cc=='d' && x0>=-76 && x0<=75) {
viviien 34:0af87f9cfb7b 276 x0=x0+1.0f ;
viviien 34:0af87f9cfb7b 277 }
viviien 34:0af87f9cfb7b 278
viviien 34:0af87f9cfb7b 279 if (cc=='a' && x0>=-75 && x0<=76) {
viviien 34:0af87f9cfb7b 280 x0=x0-1.0f;
viviien 34:0af87f9cfb7b 281 }
viviien 34:0af87f9cfb7b 282
viviien 34:0af87f9cfb7b 283 if (cc=='w' && y0>=-76 && y0<=75){
viviien 34:0af87f9cfb7b 284 y0=y0+1.0f;
viviien 34:0af87f9cfb7b 285 }
viviien 34:0af87f9cfb7b 286
viviien 34:0af87f9cfb7b 287 if (cc=='s' && y0>=-75 && y0<=76){
viviien 34:0af87f9cfb7b 288 y0=y0-1.0f;
viviien 34:0af87f9cfb7b 289 }
viviien 34:0af87f9cfb7b 290
viviien 34:0af87f9cfb7b 291 if (cc=='u' && z0>=-211 && z0<=-130){
viviien 34:0af87f9cfb7b 292 z0=z0+1.0f;
viviien 34:0af87f9cfb7b 293 }
viviien 34:0af87f9cfb7b 294
viviien 34:0af87f9cfb7b 295 if (cc=='j' && z0>=-210 && z0<=-129){
viviien 34:0af87f9cfb7b 296 z0=z0-1.0f;
viviien 34:0af87f9cfb7b 297 }
viviien 33:88fbf14d8aaf 298
viviien 33:88fbf14d8aaf 299
viviien 22:9f911405e096 300 }
viviien 32:60a71dcfdb7a 301 //END MAIN
viviien 22:9f911405e096 302 }