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@35:4cb2ed6dd2d2, 2019-11-01 (annotated)

Committer:
viviien
Date:
Fri Nov 01 08:24:42 2019 +0000
Revision:
35:4cb2ed6dd2d2
Parent:
33:88fbf14d8aaf
Child:
36:22d1bcb82061
Child:
37:15446e49ff3d
FInal without emg; ;

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 35:4cb2ed6dd2d2 25
Feike 13:18dd7a15603f 26 int quit;
Feike 19:9c8ab7922191 27 int limit_pos = 8400;
Feike 19:9c8ab7922191 28 float steps;
Feike 19:9c8ab7922191 29 int g = 0;
Feike 19:9c8ab7922191 30
Feike 18:ab0fe311e7f3 31 int counts1 = 0;
Feike 18:ab0fe311e7f3 32 int counts2 = 0;
Feike 18:ab0fe311e7f3 33 int counts3 = 0;
viviien 27:3eb181cbe183 34
viviien 35:4cb2ed6dd2d2 35 float savedX = 0;
viviien 35:4cb2ed6dd2d2 36 float savedY = 0;
viviien 35:4cb2ed6dd2d2 37 float savedZ = 0;
viviien 35:4cb2ed6dd2d2 38 int sign = 0;
viviien 35:4cb2ed6dd2d2 39
Feike 13:18dd7a15603f 40
Feike 18:ab0fe311e7f3 41 const float le = 15.0;
Feike 18:ab0fe311e7f3 42 const float f = 37.5;
Feike 18:ab0fe311e7f3 43 const float re = 174.0;
Feike 18:ab0fe311e7f3 44 const float rf = 50.0;
Feike 18:ab0fe311e7f3 45 const float pi = 3.14159265358979323846;
viviien 22:9f911405e096 46 const float cospi = -0.5;
viviien 22:9f911405e096 47 const float sinpi = 0.8660254;
Feike 18:ab0fe311e7f3 48 float y2;
Feike 18:ab0fe311e7f3 49 float y1;
Feike 18:ab0fe311e7f3 50 float z1;
Feike 18:ab0fe311e7f3 51 float z2;
Feike 18:ab0fe311e7f3 52 float rje2;
Feike 18:ab0fe311e7f3 53 float rje;
Feike 18:ab0fe311e7f3 54 float r2;
Feike 18:ab0fe311e7f3 55 float r;
Feike 19:9c8ab7922191 56
viviien 35:4cb2ed6dd2d2 57 float z0=-158;
Feike 18:ab0fe311e7f3 58 float y0=0;
Feike 18:ab0fe311e7f3 59 float x0=0;
Feike 19:9c8ab7922191 60
Feike 18:ab0fe311e7f3 61 float theta1;
Feike 18:ab0fe311e7f3 62 float theta2;
Feike 18:ab0fe311e7f3 63 float theta3;
viviien 35:4cb2ed6dd2d2 64
Feike 19:9c8ab7922191 65
viviien 28:43a1d67ff8ea 66 // Constant values for PI
viviien 30:390cab7cd6e6 67 float Kp;
viviien 30:390cab7cd6e6 68
viviien 30:390cab7cd6e6 69 float Ts = 0.0025;
viviien 28:43a1d67ff8ea 70 float error1 = 0;
viviien 30:390cab7cd6e6 71 float error2 = 0;
viviien 30:390cab7cd6e6 72 float error3 = 0;
viviien 28:43a1d67ff8ea 73
viviien 30:390cab7cd6e6 74 float newmotor1 = 1;
viviien 30:390cab7cd6e6 75 float newmotor2 = 1;
viviien 30:390cab7cd6e6 76 float newmotor3 = 1;
viviien 30:390cab7cd6e6 77
viviien 30:390cab7cd6e6 78 float u_k1 = 0;
viviien 30:390cab7cd6e6 79 float u_k2 = 0;
viviien 30:390cab7cd6e6 80 float u_k3 = 0;
viviien 30:390cab7cd6e6 81
viviien 32:60a71dcfdb7a 82 float angle1 = 0;
viviien 32:60a71dcfdb7a 83 float angle2 = 0;
viviien 32:60a71dcfdb7a 84 float angle3 = 0;
viviien 32:60a71dcfdb7a 85
viviien 32:60a71dcfdb7a 86 float time_sin = 0;
viviien 32:60a71dcfdb7a 87
viviien 32:60a71dcfdb7a 88
viviien 32:60a71dcfdb7a 89 void delta_calctheta1 () {
viviien 32:60a71dcfdb7a 90 float y2 = y0 + le;
viviien 32:60a71dcfdb7a 91 float y1 = f;
viviien 32:60a71dcfdb7a 92 float z1 = 0.0;
viviien 32:60a71dcfdb7a 93 float z2 = z0;
viviien 32:60a71dcfdb7a 94 float rje2 = re*re - x0*x0;
viviien 32:60a71dcfdb7a 95 float rje = sqrt(rje2);
viviien 32:60a71dcfdb7a 96 float r2 = (y1-y2)*(y1-y2) + (z1-z0)*(z1-z0);
viviien 32:60a71dcfdb7a 97 float r = sqrt(r2);
viviien 32:60a71dcfdb7a 98
viviien 32:60a71dcfdb7a 99 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
viviien 32:60a71dcfdb7a 100 int check = 1;
viviien 32:60a71dcfdb7a 101 pc.printf("\n\rPunt bestaat niet");
viviien 32:60a71dcfdb7a 102 }
viviien 32:60a71dcfdb7a 103 else {
viviien 32:60a71dcfdb7a 104 float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
viviien 35:4cb2ed6dd2d2 105 float beta = atan((z1+z2)/(y1-y2));
viviien 35:4cb2ed6dd2d2 106 if(beta<=0) {
viviien 32:60a71dcfdb7a 107 beta = beta + pi;
viviien 35:4cb2ed6dd2d2 108 if (beta>=alpha) {
viviien 35:4cb2ed6dd2d2 109 theta1 = beta-alpha;
viviien 35:4cb2ed6dd2d2 110 }
viviien 35:4cb2ed6dd2d2 111 else {
viviien 35:4cb2ed6dd2d2 112 theta1 = -alpha+beta;
viviien 35:4cb2ed6dd2d2 113 }
viviien 35:4cb2ed6dd2d2 114
viviien 32:60a71dcfdb7a 115 }
viviien 35:4cb2ed6dd2d2 116 if(beta>0) {
viviien 35:4cb2ed6dd2d2 117 if (beta<=alpha) {
viviien 35:4cb2ed6dd2d2 118 theta1 = -alpha+beta;
viviien 35:4cb2ed6dd2d2 119 }
viviien 35:4cb2ed6dd2d2 120 else {
viviien 35:4cb2ed6dd2d2 121 theta1 = beta-alpha;
viviien 35:4cb2ed6dd2d2 122 }
viviien 35:4cb2ed6dd2d2 123 }
viviien 32:60a71dcfdb7a 124 }
viviien 32:60a71dcfdb7a 125 }
viviien 32:60a71dcfdb7a 126
viviien 32:60a71dcfdb7a 127 void delta_calctheta2 () {
viviien 32:60a71dcfdb7a 128 float xref = x0*cospi+y0*sinpi;
viviien 32:60a71dcfdb7a 129 float yref = y0*cospi-x0*sinpi;
viviien 32:60a71dcfdb7a 130 float zref = z0;
viviien 32:60a71dcfdb7a 131 float y2 = yref + le;
viviien 32:60a71dcfdb7a 132 float y1 = f;
viviien 32:60a71dcfdb7a 133 float z1 = 0.0;
viviien 32:60a71dcfdb7a 134 float z2 = zref;
viviien 32:60a71dcfdb7a 135 float rje2 = re*re - xref*xref;
viviien 32:60a71dcfdb7a 136 float rje = sqrt(rje2);
viviien 32:60a71dcfdb7a 137 float r2 = (y1-y2)*(y1-y2) + (z1-zref)*(z1-zref);
viviien 32:60a71dcfdb7a 138 float r = sqrt(r2);
viviien 32:60a71dcfdb7a 139
viviien 32:60a71dcfdb7a 140 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
viviien 32:60a71dcfdb7a 141 int check = 1;
viviien 32:60a71dcfdb7a 142 pc.printf("\n\rPunt bestaat niet");
viviien 32:60a71dcfdb7a 143 }
viviien 35:4cb2ed6dd2d2 144 else {
viviien 35:4cb2ed6dd2d2 145 float alpha2 = acos((r2 + rf*rf -rje2)/(2*rf*r));
viviien 35:4cb2ed6dd2d2 146 float beta2 = atan((z1+z2)/(y1-y2));
viviien 35:4cb2ed6dd2d2 147 if(beta2<=0) {
viviien 35:4cb2ed6dd2d2 148 beta2 = beta2 + pi;
viviien 35:4cb2ed6dd2d2 149 if (beta2>=alpha2) {
viviien 35:4cb2ed6dd2d2 150 theta2 = beta2-alpha2;
viviien 35:4cb2ed6dd2d2 151 }
viviien 35:4cb2ed6dd2d2 152 else {
viviien 35:4cb2ed6dd2d2 153 theta2 = -alpha2+beta2;
viviien 35:4cb2ed6dd2d2 154 }
viviien 35:4cb2ed6dd2d2 155
viviien 32:60a71dcfdb7a 156 }
viviien 35:4cb2ed6dd2d2 157 if(beta2>0) {
viviien 35:4cb2ed6dd2d2 158 if (beta2<=alpha2) {
viviien 35:4cb2ed6dd2d2 159 theta2 = -alpha2+beta2;
viviien 35:4cb2ed6dd2d2 160 }
viviien 35:4cb2ed6dd2d2 161 else {
viviien 35:4cb2ed6dd2d2 162 theta2 = beta2-alpha2;
viviien 35:4cb2ed6dd2d2 163 }
viviien 35:4cb2ed6dd2d2 164 }
viviien 32:60a71dcfdb7a 165 }
viviien 32:60a71dcfdb7a 166 }
viviien 32:60a71dcfdb7a 167
viviien 32:60a71dcfdb7a 168 void delta_calctheta3 () {
viviien 32:60a71dcfdb7a 169 float xreff = x0*cospi-y0*sinpi;
viviien 35:4cb2ed6dd2d2 170 float yreff = y0*cospi+x0*sinpi;
viviien 32:60a71dcfdb7a 171 float zreff = z0;
viviien 32:60a71dcfdb7a 172 float y2 = yreff + le;
viviien 32:60a71dcfdb7a 173 float y1 = f;
viviien 32:60a71dcfdb7a 174 float z1 = 0.0;
viviien 32:60a71dcfdb7a 175 float z2 = zreff;
viviien 32:60a71dcfdb7a 176 float rje2 = re*re - xreff*xreff;
viviien 32:60a71dcfdb7a 177 float rje = sqrt(rje2);
viviien 32:60a71dcfdb7a 178 float r2 = (y1-y2)*(y1-y2) + (z1-zreff)*(z1-zreff);
viviien 32:60a71dcfdb7a 179 float r = sqrt(r2);
viviien 32:60a71dcfdb7a 180
viviien 32:60a71dcfdb7a 181 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
viviien 32:60a71dcfdb7a 182 int check = 1;
viviien 32:60a71dcfdb7a 183 pc.printf("\n\rPunt bestaat niet");
viviien 32:60a71dcfdb7a 184 }
viviien 35:4cb2ed6dd2d2 185 else {
viviien 35:4cb2ed6dd2d2 186 float alpha3 = acos((r2 + rf*rf -rje2)/(2*rf*r));
viviien 35:4cb2ed6dd2d2 187 float beta3 = atan((z1+z2)/(y1-y2));
viviien 35:4cb2ed6dd2d2 188 if(beta3<=0) {
viviien 35:4cb2ed6dd2d2 189 beta3 = beta3 + pi;
viviien 35:4cb2ed6dd2d2 190 if (beta3>=alpha3) {
viviien 35:4cb2ed6dd2d2 191 theta3 = beta3-alpha3;
viviien 35:4cb2ed6dd2d2 192 }
viviien 35:4cb2ed6dd2d2 193 else {
viviien 35:4cb2ed6dd2d2 194 theta3 = -alpha3+beta3;
viviien 35:4cb2ed6dd2d2 195 }
viviien 35:4cb2ed6dd2d2 196
viviien 32:60a71dcfdb7a 197 }
viviien 35:4cb2ed6dd2d2 198 if(beta3>0) {
viviien 35:4cb2ed6dd2d2 199 if (beta3<=alpha3) {
viviien 35:4cb2ed6dd2d2 200 theta3 = -alpha3+beta3;
viviien 35:4cb2ed6dd2d2 201 }
viviien 35:4cb2ed6dd2d2 202 else {
viviien 35:4cb2ed6dd2d2 203 theta3 = beta3-alpha3;
viviien 35:4cb2ed6dd2d2 204 }
viviien 35:4cb2ed6dd2d2 205 }
viviien 32:60a71dcfdb7a 206 }
viviien 32:60a71dcfdb7a 207 }
viviien 32:60a71dcfdb7a 208
viviien 32:60a71dcfdb7a 209
viviien 32:60a71dcfdb7a 210
viviien 30:390cab7cd6e6 211
viviien 30:390cab7cd6e6 212 Ticker motor_timer;
viviien 32:60a71dcfdb7a 213 void motor(){
viviien 32:60a71dcfdb7a 214
viviien 30:390cab7cd6e6 215 counts1 = Encoder1.getPulses();
viviien 30:390cab7cd6e6 216 counts2 = Encoder2.getPulses();
viviien 30:390cab7cd6e6 217 counts3 = Encoder3.getPulses();
viviien 32:60a71dcfdb7a 218
viviien 35:4cb2ed6dd2d2 219 //z0 = 158 - 60* sin(3*time_sin);
viviien 33:88fbf14d8aaf 220
viviien 35:4cb2ed6dd2d2 221 if (sign == 1) {
viviien 35:4cb2ed6dd2d2 222 float r = 20*(1-1/(1+(time_sin)));
viviien 35:4cb2ed6dd2d2 223 x0 = savedX0 - r*sin(3*time_sin);
viviien 35:4cb2ed6dd2d2 224 y0 = savedY0 - r*cos(3*time_sin);
viviien 35:4cb2ed6dd2d2 225 }
viviien 33:88fbf14d8aaf 226
viviien 29:7eb028b359a1 227
viviien 32:60a71dcfdb7a 228 delta_calctheta1 ();
viviien 32:60a71dcfdb7a 229 delta_calctheta2 ();
viviien 32:60a71dcfdb7a 230 delta_calctheta3 ();
viviien 30:390cab7cd6e6 231
viviien 32:60a71dcfdb7a 232 angle1 = counts1/(8400.0)*2.0*pi;
viviien 32:60a71dcfdb7a 233 angle2 = counts2/(8400.0)*2.0*pi;
viviien 32:60a71dcfdb7a 234 angle3 = counts3/(8400.0)*2.0*pi;
viviien 30:390cab7cd6e6 235
viviien 32:60a71dcfdb7a 236 error1 = theta1 - angle1;
viviien 32:60a71dcfdb7a 237 error2 = theta2 - angle2;
viviien 32:60a71dcfdb7a 238 error3 = theta3 - angle3;
viviien 30:390cab7cd6e6 239
viviien 35:4cb2ed6dd2d2 240 Kp = potmeter1 * 10;
viviien 30:390cab7cd6e6 241
viviien 35:4cb2ed6dd2d2 242 u_k1 = 10 * error1;
viviien 35:4cb2ed6dd2d2 243 u_k2 = 10 * error2;
viviien 35:4cb2ed6dd2d2 244 u_k3 = 10 * error3;
viviien 30:390cab7cd6e6 245
viviien 32:60a71dcfdb7a 246 newmotor1 = u_k1;
viviien 32:60a71dcfdb7a 247 newmotor2 = u_k2;
viviien 32:60a71dcfdb7a 248 newmotor3 = u_k3;
viviien 30:390cab7cd6e6 249
viviien 30:390cab7cd6e6 250 if (newmotor1>1.0f){
viviien 35:4cb2ed6dd2d2 251 newmotor1 = 1.0f;
viviien 23:18b0be02187f 252 }
viviien 30:390cab7cd6e6 253 if (newmotor1<-1.0f){
viviien 30:390cab7cd6e6 254 newmotor1 = -1.0f;
viviien 30:390cab7cd6e6 255 }
viviien 30:390cab7cd6e6 256
viviien 30:390cab7cd6e6 257 if (newmotor2>1.0f){
viviien 30:390cab7cd6e6 258 newmotor2 =1.0f;
viviien 30:390cab7cd6e6 259 }
viviien 30:390cab7cd6e6 260 if (newmotor2<-1.0f){
viviien 30:390cab7cd6e6 261 newmotor2 = -1.0f;
viviien 30:390cab7cd6e6 262 }
viviien 30:390cab7cd6e6 263
viviien 30:390cab7cd6e6 264 if (newmotor3>1.0f){
viviien 30:390cab7cd6e6 265 newmotor3 =1.0f;
viviien 30:390cab7cd6e6 266 }
viviien 30:390cab7cd6e6 267 if (newmotor3<-1.0f){
viviien 30:390cab7cd6e6 268 newmotor3 = -1.0f;
viviien 30:390cab7cd6e6 269 }
viviien 30:390cab7cd6e6 270
viviien 35:4cb2ed6dd2d2 271
viviien 30:390cab7cd6e6 272 motor1_pwm.write(fabs(newmotor1));
viviien 32:60a71dcfdb7a 273 motor1_dir.write(newmotor1<0);
viviien 30:390cab7cd6e6 274
viviien 30:390cab7cd6e6 275 motor2_pwm.write(fabs(newmotor2));
viviien 32:60a71dcfdb7a 276 motor2_dir.write(newmotor2>0);
viviien 30:390cab7cd6e6 277
viviien 30:390cab7cd6e6 278 motor3_pwm.write(fabs(newmotor3));
viviien 32:60a71dcfdb7a 279 motor3_dir.write(newmotor3<0);
viviien 30:390cab7cd6e6 280
viviien 32:60a71dcfdb7a 281 time_sin += 0.002;
viviien 30:390cab7cd6e6 282 }
viviien 25:eb3204e45d33 283
viviien 32:60a71dcfdb7a 284
viviien 28:43a1d67ff8ea 285
viviien 28:43a1d67ff8ea 286 ///////////////////
viviien 28:43a1d67ff8ea 287 // MAIN FUNCTION //
viviien 28:43a1d67ff8ea 288 ///////////////////
viviien 28:43a1d67ff8ea 289
viviien 35:4cb2ed6dd2d2 290 int main(void) {
viviien 35:4cb2ed6dd2d2 291
viviien 35:4cb2ed6dd2d2 292 delta_calctheta1 ();
viviien 35:4cb2ed6dd2d2 293 delta_calctheta2 ();
viviien 35:4cb2ed6dd2d2 294 delta_calctheta3 ();
viviien 35:4cb2ed6dd2d2 295
viviien 35:4cb2ed6dd2d2 296 pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 26:432d3519ba86 297
Feike 17:6da57acb7bea 298 char cc = pc.getc();
viviien 30:390cab7cd6e6 299
viviien 35:4cb2ed6dd2d2 300 motor_timer.attach(&motor, 0.002);
viviien 35:4cb2ed6dd2d2 301
viviien 35:4cb2ed6dd2d2 302
viviien 30:390cab7cd6e6 303 int frequency_pwm = 10000; //10 kHz PWM
viviien 33:88fbf14d8aaf 304 motor1_pwm.period_us(6);
viviien 33:88fbf14d8aaf 305 motor2_pwm.period_us(6);
viviien 33:88fbf14d8aaf 306 motor3_pwm.period_us(6);
viviien 35:4cb2ed6dd2d2 307
viviien 35:4cb2ed6dd2d2 308 pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 35:4cb2ed6dd2d2 309 pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 35:4cb2ed6dd2d2 310 pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
viviien 35:4cb2ed6dd2d2 311
viviien 35:4cb2ed6dd2d2 312
viviien 35:4cb2ed6dd2d2 313
viviien 35:4cb2ed6dd2d2 314 pc.baud(115200);
Feike 19:9c8ab7922191 315
viviien 28:43a1d67ff8ea 316 while(true){
viviien 33:88fbf14d8aaf 317
viviien 35:4cb2ed6dd2d2 318 while (boxcheckC == 0) {
viviien 35:4cb2ed6dd2d2 319 if (boxcheckA == 1 && x0>=-76 && x0<=75) {
viviien 35:4cb2ed6dd2d2 320 x0=x0+1.0f ;
viviien 35:4cb2ed6dd2d2 321 wait(1.0/20);
viviien 35:4cb2ed6dd2d2 322 }
viviien 35:4cb2ed6dd2d2 323 if (boxcheckB== 1 && x0>=-75 && x0<=76) {
viviien 35:4cb2ed6dd2d2 324 x0=x0-1.0f;
viviien 35:4cb2ed6dd2d2 325 wait(1.0/20);
viviien 35:4cb2ed6dd2d2 326 }
viviien 35:4cb2ed6dd2d2 327 }
viviien 33:88fbf14d8aaf 328
viviien 35:4cb2ed6dd2d2 329 wait(1.5);
viviien 35:4cb2ed6dd2d2 330 savedX = x0;
viviien 35:4cb2ed6dd2d2 331
viviien 35:4cb2ed6dd2d2 332 while (boxcheckC == 0) {
viviien 35:4cb2ed6dd2d2 333 if (boxcheckA==1 && y0>=-76 && y0<=75){
viviien 35:4cb2ed6dd2d2 334 y0=y0+1.0f;
viviien 35:4cb2ed6dd2d2 335 wait(1.0/20);
viviien 35:4cb2ed6dd2d2 336 }
viviien 35:4cb2ed6dd2d2 337 if (boxcheckB==1 && y0>=-75 && y0<=76){
viviien 35:4cb2ed6dd2d2 338 y0=y0-1.0f;
viviien 35:4cb2ed6dd2d2 339 wait(1.0/20);
viviien 35:4cb2ed6dd2d2 340 }
viviien 35:4cb2ed6dd2d2 341 }
viviien 35:4cb2ed6dd2d2 342
viviien 35:4cb2ed6dd2d2 343 wait(1.5);
viviien 35:4cb2ed6dd2d2 344 savedY = y0;
viviien 35:4cb2ed6dd2d2 345
viviien 35:4cb2ed6dd2d2 346 while (boxcheckC == 0) {
viviien 35:4cb2ed6dd2d2 347 if (boxcheckA == 1 && z0>=-179 && z0<=-158){
viviien 35:4cb2ed6dd2d2 348 z0=z0+1.0f;
viviien 35:4cb2ed6dd2d2 349 wait(1.0/20);
viviien 35:4cb2ed6dd2d2 350 }
viviien 35:4cb2ed6dd2d2 351 if (boxcheckB == 1 && z0>=-178 && z0<=-157){
viviien 35:4cb2ed6dd2d2 352 z0=z0-1.0f;
viviien 35:4cb2ed6dd2d2 353 wait(1.0/20);
viviien 35:4cb2ed6dd2d2 354 }
viviien 35:4cb2ed6dd2d2 355 }
viviien 35:4cb2ed6dd2d2 356
viviien 35:4cb2ed6dd2d2 357 wait(1.5);
viviien 35:4cb2ed6dd2d2 358 savedZ = z0;
viviien 35:4cb2ed6dd2d2 359
viviien 35:4cb2ed6dd2d2 360 if (boxcheckC == 1) {
viviien 35:4cb2ed6dd2d2 361 sign = 1;
viviien 35:4cb2ed6dd2d2 362 }
viviien 35:4cb2ed6dd2d2 363
viviien 35:4cb2ed6dd2d2 364 wait(5.0);
viviien 35:4cb2ed6dd2d2 365
viviien 35:4cb2ed6dd2d2 366 sign = 0;
viviien 35:4cb2ed6dd2d2 367
viviien 35:4cb2ed6dd2d2 368 float diffZ = -158 - z0;
viviien 35:4cb2ed6dd2d2 369 float diffX = 0 - x0;
viviien 35:4cb2ed6dd2d2 370 float diffY = 0 - y0;
viviien 35:4cb2ed6dd2d2 371
viviien 35:4cb2ed6dd2d2 372 for (int i=0; i<=diffZ; i++) {
viviien 35:4cb2ed6dd2d2 373 z0 = z0+1;
viviien 35:4cb2ed6dd2d2 374 }
viviien 35:4cb2ed6dd2d2 375
viviien 35:4cb2ed6dd2d2 376 for (int i=0; i<=diffY; i++) {
viviien 35:4cb2ed6dd2d2 377 if (diffY>0) {
viviien 35:4cb2ed6dd2d2 378 y0 = y0+1;
viviien 35:4cb2ed6dd2d2 379 }
viviien 35:4cb2ed6dd2d2 380 if (diffY<0) {
viviien 35:4cb2ed6dd2d2 381 y0 = y0-1;
viviien 35:4cb2ed6dd2d2 382 }
viviien 35:4cb2ed6dd2d2 383 }
viviien 35:4cb2ed6dd2d2 384
viviien 35:4cb2ed6dd2d2 385 for (int i=0; i<=diffX; i++) {
viviien 35:4cb2ed6dd2d2 386 if (diffX>0) {
viviien 35:4cb2ed6dd2d2 387 x0 = x0+1;
viviien 35:4cb2ed6dd2d2 388 }
viviien 35:4cb2ed6dd2d2 389 if (diffX<0) {
viviien 35:4cb2ed6dd2d2 390 x0 = x0-1;
viviien 35:4cb2ed6dd2d2 391 }
viviien 35:4cb2ed6dd2d2 392 }
viviien 35:4cb2ed6dd2d2 393
viviien 35:4cb2ed6dd2d2 394 break;
viviien 35:4cb2ed6dd2d2 395
viviien 35:4cb2ed6dd2d2 396
viviien 35:4cb2ed6dd2d2 397
viviien 35:4cb2ed6dd2d2 398 // pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 35:4cb2ed6dd2d2 399 // pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 35:4cb2ed6dd2d2 400 // wait(0.1);
viviien 35:4cb2ed6dd2d2 401
viviien 35:4cb2ed6dd2d2 402
viviien 35:4cb2ed6dd2d2 403 // char cc = pc.getc();
viviien 35:4cb2ed6dd2d2 404 // if (cc=='d' && x0>=-76 && x0<=75) {
viviien 35:4cb2ed6dd2d2 405 // x0=x0+1.0f ;
viviien 35:4cb2ed6dd2d2 406 // }
viviien 35:4cb2ed6dd2d2 407 //
viviien 35:4cb2ed6dd2d2 408 // if (cc=='a' && x0>=-75 && x0<=76) {
viviien 35:4cb2ed6dd2d2 409 //
viviien 35:4cb2ed6dd2d2 410 // x0=x0-1.0f;
viviien 35:4cb2ed6dd2d2 411 // }
viviien 35:4cb2ed6dd2d2 412 //
viviien 35:4cb2ed6dd2d2 413 // if (cc=='w' && y0>=-76 && y0<=75){
viviien 35:4cb2ed6dd2d2 414 // y0=y0+1.0f;
viviien 35:4cb2ed6dd2d2 415 // pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 35:4cb2ed6dd2d2 416 // pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 35:4cb2ed6dd2d2 417 // pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
viviien 35:4cb2ed6dd2d2 418 //
viviien 35:4cb2ed6dd2d2 419 // }
viviien 35:4cb2ed6dd2d2 420 //
viviien 35:4cb2ed6dd2d2 421 // if (cc=='s' && y0>=-75 && y0<=76){
viviien 35:4cb2ed6dd2d2 422 // y0=y0-1.0f;
viviien 35:4cb2ed6dd2d2 423 // pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 35:4cb2ed6dd2d2 424 // pc .printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 35:4cb2ed6dd2d2 425 // pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
viviien 35:4cb2ed6dd2d2 426 // }
viviien 35:4cb2ed6dd2d2 427 //
viviien 35:4cb2ed6dd2d2 428 // if (cc=='u' && z0>=-211 && z0<=-130){
viviien 35:4cb2ed6dd2d2 429 // pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 35:4cb2ed6dd2d2 430 // pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 35:4cb2ed6dd2d2 431 // pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
viviien 35:4cb2ed6dd2d2 432 // z0=z0+1.0f;
viviien 35:4cb2ed6dd2d2 433 // }
viviien 35:4cb2ed6dd2d2 434 //
viviien 35:4cb2ed6dd2d2 435 // if (cc=='j' && z0>=-210 && z0<=-129){
viviien 35:4cb2ed6dd2d2 436 // pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 35:4cb2ed6dd2d2 437 // pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 35:4cb2ed6dd2d2 438 // pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
viviien 35:4cb2ed6dd2d2 439 // z0=z0-1.0f;
viviien 35:4cb2ed6dd2d2 440 // }
viviien 22:9f911405e096 441 }
viviien 32:60a71dcfdb7a 442 //END MAIN
viviien 22:9f911405e096 443 }