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@24:61d03692f92e, 2019-10-25 (annotated)

Committer:
viviien
Date:
Fri Oct 25 13:45:39 2019 +0000
Revision:
24:61d03692f92e
Parent:
23:18b0be02187f
Child:
25:eb3204e45d33
Nog steeds ticker problemen

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"
Feike 18:ab0fe311e7f3 6
Feike 17:6da57acb7bea 7 MODSERIAL pc(USBTX, USBRX);
Feike 18:ab0fe311e7f3 8 //Serial term (USBTX, USBRX);
Feike 17:6da57acb7bea 9 PwmOut motor1_pwm(PTC2);
Feike 17:6da57acb7bea 10 DigitalOut motor1_dir(PTC3);
Feike 18:ab0fe311e7f3 11 PwmOut motor2_pwm(PTA2);
Feike 18:ab0fe311e7f3 12 DigitalOut motor2_dir(PTB23);
Feike 18:ab0fe311e7f3 13 PwmOut motor3_pwm(PTC4);
Feike 18:ab0fe311e7f3 14 DigitalOut motor3_dir(PTC12);
Feike 4:bd21569250c7 15
viviien 23:18b0be02187f 16 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
viviien 23:18b0be02187f 17 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
viviien 23:18b0be02187f 18 QEI Encoder3(D2,D3,NC,64,QEI::X4_ENCODING);
Feike 7:e119b12e5e7f 19
Feike 19:9c8ab7922191 20 int check;
Feike 13:18dd7a15603f 21 int quit;
Feike 19:9c8ab7922191 22 int limit_pos = 8400;
Feike 19:9c8ab7922191 23 float steps;
Feike 19:9c8ab7922191 24 int g = 0;
Feike 19:9c8ab7922191 25
Feike 19:9c8ab7922191 26 bool check1;
Feike 19:9c8ab7922191 27 bool check2;
Feike 19:9c8ab7922191 28 bool check3;
Feike 18:ab0fe311e7f3 29 int counts1 = 0;
Feike 18:ab0fe311e7f3 30 int counts2 = 0;
Feike 18:ab0fe311e7f3 31 int counts3 = 0;
Feike 13:18dd7a15603f 32
Feike 18:ab0fe311e7f3 33 const float le = 15.0;
Feike 18:ab0fe311e7f3 34 const float f = 37.5;
Feike 18:ab0fe311e7f3 35 const float re = 174.0;
Feike 18:ab0fe311e7f3 36 const float rf = 50.0;
Feike 18:ab0fe311e7f3 37 const float pi = 3.14159265358979323846;
viviien 22:9f911405e096 38 const float cospi = -0.5;
viviien 22:9f911405e096 39 const float sinpi = 0.8660254;
Feike 18:ab0fe311e7f3 40 float y2;
Feike 18:ab0fe311e7f3 41 float y1;
Feike 18:ab0fe311e7f3 42 float z1;
Feike 18:ab0fe311e7f3 43 float z2;
Feike 18:ab0fe311e7f3 44 float rje2;
Feike 18:ab0fe311e7f3 45 float rje;
Feike 18:ab0fe311e7f3 46 float r2;
Feike 18:ab0fe311e7f3 47 float r;
Feike 19:9c8ab7922191 48
Feike 18:ab0fe311e7f3 49 float z0=-172;
Feike 18:ab0fe311e7f3 50 float y0=0;
Feike 18:ab0fe311e7f3 51 float x0=0;
viviien 21:c826abca79c3 52 float x00;
viviien 21:c826abca79c3 53 float y00;
viviien 21:c826abca79c3 54 float z00;
Feike 19:9c8ab7922191 55
Feike 18:ab0fe311e7f3 56 float theta1;
Feike 18:ab0fe311e7f3 57 float theta2;
Feike 18:ab0fe311e7f3 58 float theta3;
Feike 18:ab0fe311e7f3 59 float oldtheta1=0;
Feike 18:ab0fe311e7f3 60 float oldtheta2=0;
Feike 18:ab0fe311e7f3 61 float oldtheta3=0;
Feike 19:9c8ab7922191 62
viviien 23:18b0be02187f 63 Ticker pulses;
viviien 23:18b0be02187f 64 void getpulses() {
viviien 23:18b0be02187f 65 counts1 = Encoder1.getPulses();
viviien 23:18b0be02187f 66 counts2 = Encoder2.getPulses();
viviien 23:18b0be02187f 67 counts3 = Encoder3.getPulses();
viviien 23:18b0be02187f 68 }
viviien 23:18b0be02187f 69
viviien 21:c826abca79c3 70 float delta_calcangleyz(float x00, float y00, float z00) {
viviien 21:c826abca79c3 71 float y2 = y00 + le;
Feike 18:ab0fe311e7f3 72 float y1 = f;
Feike 18:ab0fe311e7f3 73 float z1 = 0.0;
viviien 21:c826abca79c3 74 float z2 = z00;
viviien 21:c826abca79c3 75 float rje2 = re*re - x00*x00;
Feike 18:ab0fe311e7f3 76 float rje = sqrt(rje2);
viviien 22:9f911405e096 77 float r2 = (y1-y2)*(y1-y2) + (z1-z00)*(z1-z00);
Feike 18:ab0fe311e7f3 78 float r = sqrt(r2);
Feike 18:ab0fe311e7f3 79
Feike 19:9c8ab7922191 80 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
Feike 19:9c8ab7922191 81 int check = 1;
Feike 19:9c8ab7922191 82 pc.printf("\n\rPunt bestaat niet");
Feike 19:9c8ab7922191 83 }
Feike 19:9c8ab7922191 84 else {
Feike 19:9c8ab7922191 85 float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
Feike 19:9c8ab7922191 86 float beta = atan((z1-z2)/(y1-y2));
Feike 19:9c8ab7922191 87 if(beta<0) {
Feike 18:ab0fe311e7f3 88 beta = beta + pi;
Feike 18:ab0fe311e7f3 89 }
Feike 19:9c8ab7922191 90 float theta1 = (beta - alpha)*180.0/pi;
Feike 19:9c8ab7922191 91 return theta1;
viviien 22:9f911405e096 92 }
Feike 18:ab0fe311e7f3 93 }
Feike 18:ab0fe311e7f3 94
viviien 21:c826abca79c3 95 float delta_calcinverse(float x00, float y00, float z00) {
viviien 21:c826abca79c3 96 theta1 = theta2 = theta3 = 0;
viviien 22:9f911405e096 97 x00=x0;
viviien 22:9f911405e096 98 y00=y0;
viviien 22:9f911405e096 99 z00=z0;
viviien 21:c826abca79c3 100 theta1 = delta_calcangleyz(x00, y00, z00);
viviien 21:c826abca79c3 101
viviien 21:c826abca79c3 102 if (check == 0) {
viviien 21:c826abca79c3 103 x00=x0*cospi+y0*sinpi;
viviien 21:c826abca79c3 104 y00=y0*cospi-x0*sinpi;
viviien 21:c826abca79c3 105 z00=z0;
viviien 22:9f911405e096 106 theta2 = delta_calcangleyz(x00, y00, z00);
viviien 21:c826abca79c3 107 x00=x0*cospi-y0*sinpi;
viviien 21:c826abca79c3 108 y00=y0*cospi+y0*sinpi;
viviien 21:c826abca79c3 109 z00=z0;
viviien 22:9f911405e096 110 theta3 = delta_calcangleyz(x00, y00, z00);
viviien 21:c826abca79c3 111 }
viviien 21:c826abca79c3 112
viviien 21:c826abca79c3 113 return theta1, theta2, theta3;
Feike 18:ab0fe311e7f3 114 }
Feike 18:ab0fe311e7f3 115
viviien 23:18b0be02187f 116
Feike 12:2382468d36a4 117
Feike 19:9c8ab7922191 118 float anglestep(float angle) {
Feike 18:ab0fe311e7f3 119 float steps;
Feike 18:ab0fe311e7f3 120 steps = angle / 360 * 8400;
Feike 18:ab0fe311e7f3 121 return steps;
Feike 17:6da57acb7bea 122 }
Feike 17:6da57acb7bea 123
viviien 21:c826abca79c3 124
viviien 22:9f911405e096 125 float movefunction() {
viviien 21:c826abca79c3 126
viviien 22:9f911405e096 127 theta1 = delta_calcinverse(x00,y00,z00);
viviien 21:c826abca79c3 128 pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3);
viviien 21:c826abca79c3 129 pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0);
viviien 21:c826abca79c3 130
viviien 21:c826abca79c3 131 theta1 = theta1 - oldtheta1;
viviien 21:c826abca79c3 132 theta2 = theta2 - oldtheta2;
viviien 21:c826abca79c3 133 theta3 = theta3 - oldtheta3;
viviien 21:c826abca79c3 134
viviien 21:c826abca79c3 135 float steps1 = anglestep(theta1);
viviien 21:c826abca79c3 136 float steps2 = anglestep(theta2);
viviien 21:c826abca79c3 137 float steps3 = anglestep(theta3);
viviien 21:c826abca79c3 138
viviien 21:c826abca79c3 139 // Set the direction of the motors.
viviien 21:c826abca79c3 140 if (theta1 < 0) {
viviien 21:c826abca79c3 141 motor1_dir.write(1);
viviien 21:c826abca79c3 142 }
viviien 21:c826abca79c3 143 else {
viviien 21:c826abca79c3 144 motor1_dir.write(0);
viviien 21:c826abca79c3 145 }
viviien 21:c826abca79c3 146
viviien 21:c826abca79c3 147 if (theta2 < 0) {
viviien 21:c826abca79c3 148 motor2_dir.write(0);
viviien 21:c826abca79c3 149 }
viviien 21:c826abca79c3 150 else {
viviien 21:c826abca79c3 151 motor2_dir.write(1);
viviien 21:c826abca79c3 152 }
viviien 21:c826abca79c3 153
viviien 21:c826abca79c3 154 if (theta3 < 0) {
viviien 21:c826abca79c3 155 motor3_dir.write(0);
viviien 21:c826abca79c3 156 }
viviien 21:c826abca79c3 157 else {
viviien 21:c826abca79c3 158 motor3_dir.write(1);
viviien 21:c826abca79c3 159 }
viviien 21:c826abca79c3 160
viviien 21:c826abca79c3 161 int frequency_pwm = 10000; //10 kHz PWM
viviien 21:c826abca79c3 162 motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 21:c826abca79c3 163 motor1_pwm.write(0.7); // write Duty Cycle
viviien 21:c826abca79c3 164
viviien 21:c826abca79c3 165 motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 21:c826abca79c3 166 motor2_pwm.write(0.7); // write Duty Cycle
viviien 21:c826abca79c3 167
viviien 21:c826abca79c3 168 motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 22:9f911405e096 169 motor3_pwm.write(0.7); // write Duty Cycle
viviien 22:9f911405e096 170
viviien 22:9f911405e096 171 check1 = true;
viviien 22:9f911405e096 172 check2 = true;
viviien 22:9f911405e096 173 check3 = true;
viviien 24:61d03692f92e 174
viviien 24:61d03692f92e 175 counts1=0;
viviien 24:61d03692f92e 176 counts2=0;
viviien 24:61d03692f92e 177 counts3=0;
viviien 24:61d03692f92e 178
viviien 24:61d03692f92e 179 pulses.attach(&getpulses, 1.0/1000);
viviien 24:61d03692f92e 180 for (int i=0; i<100; i++) {
viviien 24:61d03692f92e 181 pc.printf("\n\rcounts1 %i", counts1);
viviien 24:61d03692f92e 182 wait(0.001);
viviien 24:61d03692f92e 183 }
viviien 24:61d03692f92e 184
viviien 21:c826abca79c3 185
viviien 21:c826abca79c3 186 while (check1 || check2 || check3) {
viviien 23:18b0be02187f 187 pc.printf("\n\r counts1 %i", counts1);
viviien 24:61d03692f92e 188 pulses.attach(&getpulses, 1.0/10000);
viviien 24:61d03692f92e 189
viviien 24:61d03692f92e 190 wait(1/1000);
viviien 21:c826abca79c3 191 if(abs(counts1)>=abs(steps1)) {
viviien 24:61d03692f92e 192 // pc.printf("\n\r counts1 %i and steps1 %f", counts1, steps1);
viviien 24:61d03692f92e 193 pc.printf("\n 1 is false");
viviien 21:c826abca79c3 194 motor1_pwm.write(0);
viviien 21:c826abca79c3 195 check1=false;
viviien 21:c826abca79c3 196 counts1=0;
viviien 21:c826abca79c3 197 }
viviien 21:c826abca79c3 198 if (abs(counts2)>=abs(steps2)) {
viviien 24:61d03692f92e 199 pc.printf("\n 2 is false");
viviien 21:c826abca79c3 200 motor2_pwm.write(0);
viviien 21:c826abca79c3 201 check2=false;
viviien 21:c826abca79c3 202 counts2=0;
viviien 21:c826abca79c3 203 }
viviien 21:c826abca79c3 204 if (abs(counts3)>=abs(steps3)) {
viviien 24:61d03692f92e 205 pc.printf("\n 3 is false");
viviien 21:c826abca79c3 206 motor3_pwm.write(0);
viviien 21:c826abca79c3 207 check3=false;
viviien 21:c826abca79c3 208 //pc.printf("\n\rsteps %f, counts %f", steps3, counts3);
viviien 21:c826abca79c3 209 counts3=0;
viviien 21:c826abca79c3 210 }
viviien 21:c826abca79c3 211 }
viviien 22:9f911405e096 212 }
viviien 22:9f911405e096 213
viviien 22:9f911405e096 214 int main(void) {
Feike 18:ab0fe311e7f3 215 pc.baud(115200);
Feike 18:ab0fe311e7f3 216
Feike 17:6da57acb7bea 217 char cc = pc.getc();
Feike 19:9c8ab7922191 218
Feike 19:9c8ab7922191 219 while(true) {
Feike 18:ab0fe311e7f3 220
Feike 19:9c8ab7922191 221 delta_calcinverse(x0,y0,z0);
Feike 19:9c8ab7922191 222
Feike 19:9c8ab7922191 223 oldtheta1 = theta1;
Feike 19:9c8ab7922191 224 oldtheta2 = theta2;
viviien 21:c826abca79c3 225 oldtheta3 = theta3;
Feike 18:ab0fe311e7f3 226 char cc = pc.getc();
Feike 18:ab0fe311e7f3 227
viviien 21:c826abca79c3 228 if (cc=='d') {
viviien 24:61d03692f92e 229 x0=x0+5.0f;
viviien 22:9f911405e096 230 if (x0>=-75 && x0<=75) {
viviien 22:9f911405e096 231 movefunction ();
viviien 22:9f911405e096 232 }
viviien 22:9f911405e096 233 else {
viviien 24:61d03692f92e 234 x0=x0-5.0f;
viviien 22:9f911405e096 235 }
viviien 21:c826abca79c3 236 }
Feike 19:9c8ab7922191 237
viviien 21:c826abca79c3 238 if (cc=='a') {
viviien 24:61d03692f92e 239 x0=x0-5.0f;
viviien 21:c826abca79c3 240 if (x0>=-75 && x0<=75) {
viviien 21:c826abca79c3 241 movefunction ();
viviien 21:c826abca79c3 242 }
viviien 21:c826abca79c3 243 else {
viviien 24:61d03692f92e 244 x0=x0+5.0f;
viviien 21:c826abca79c3 245 }
viviien 21:c826abca79c3 246 }
viviien 21:c826abca79c3 247
viviien 21:c826abca79c3 248 if (cc=='w') {
viviien 24:61d03692f92e 249 y0=y0+5.0f;
viviien 21:c826abca79c3 250 if (y0>=-75 && y0<=75) {
viviien 21:c826abca79c3 251 movefunction ();
viviien 21:c826abca79c3 252 }
viviien 21:c826abca79c3 253 else {
viviien 24:61d03692f92e 254 y0=y0-5.0f;
viviien 21:c826abca79c3 255 }
Feike 19:9c8ab7922191 256 }
Feike 19:9c8ab7922191 257
viviien 21:c826abca79c3 258 if (cc=='s') {
viviien 24:61d03692f92e 259 y0=y0-5.0f;
viviien 21:c826abca79c3 260 if (y0>=-75 && y0<=75) {
viviien 21:c826abca79c3 261 movefunction ();
viviien 21:c826abca79c3 262 }
viviien 21:c826abca79c3 263 else {
viviien 24:61d03692f92e 264 y0=y0+5.0f;
Feike 19:9c8ab7922191 265 }
viviien 21:c826abca79c3 266 }
viviien 21:c826abca79c3 267
viviien 21:c826abca79c3 268 if (cc=='u') {
viviien 24:61d03692f92e 269 z0=z0+5.0f;
viviien 21:c826abca79c3 270 if (z0>=-210 && z0<=-130) {
viviien 21:c826abca79c3 271 movefunction ();
viviien 21:c826abca79c3 272 }
viviien 21:c826abca79c3 273 else {
viviien 24:61d03692f92e 274 z0=z0-5.0f;
viviien 21:c826abca79c3 275 }
viviien 21:c826abca79c3 276 }
Feike 18:ab0fe311e7f3 277
viviien 21:c826abca79c3 278 if (cc=='j') {
viviien 24:61d03692f92e 279 z0=z0-5.0f;
viviien 21:c826abca79c3 280 if (z0>=-210 && z0<=-130) {
viviien 21:c826abca79c3 281 movefunction ();
viviien 21:c826abca79c3 282 }
viviien 21:c826abca79c3 283 else {
viviien 24:61d03692f92e 284 z0=z0+5.0f;
Feike 18:ab0fe311e7f3 285 }
viviien 21:c826abca79c3 286 }
viviien 22:9f911405e096 287 }
viviien 22:9f911405e096 288 }