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@21:c826abca79c3, 2019-10-25 (annotated)

Committer:
viviien
Date:
Fri Oct 25 11:23:15 2019 +0000
Revision:
21:c826abca79c3
Parent:
19:9c8ab7922191
Child:
22:9f911405e096
It moves

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