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@37:15446e49ff3d, 2019-11-02 (annotated)

Committer:
viviien
Date:
Sat Nov 02 14:48:42 2019 +0000
Revision:
37:15446e49ff3d
Parent:
35:4cb2ed6dd2d2 
Versie voor tijdens de demo (cirkel)

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 float r = 20*(1-1/(1+(time_sin)));
viviien 37:15446e49ff3d 222 x0 = r*sin(3*time_sin);
viviien 37:15446e49ff3d 223 y0 = r*cos(3*time_sin);
viviien 33:88fbf14d8aaf 224
viviien 29:7eb028b359a1 225
viviien 32:60a71dcfdb7a 226 delta_calctheta1 ();
viviien 32:60a71dcfdb7a 227 delta_calctheta2 ();
viviien 32:60a71dcfdb7a 228 delta_calctheta3 ();
viviien 30:390cab7cd6e6 229
viviien 32:60a71dcfdb7a 230 angle1 = counts1/(8400.0)*2.0*pi;
viviien 32:60a71dcfdb7a 231 angle2 = counts2/(8400.0)*2.0*pi;
viviien 32:60a71dcfdb7a 232 angle3 = counts3/(8400.0)*2.0*pi;
viviien 30:390cab7cd6e6 233
viviien 32:60a71dcfdb7a 234 error1 = theta1 - angle1;
viviien 32:60a71dcfdb7a 235 error2 = theta2 - angle2;
viviien 32:60a71dcfdb7a 236 error3 = theta3 - angle3;
viviien 30:390cab7cd6e6 237
viviien 35:4cb2ed6dd2d2 238 Kp = potmeter1 * 10;
viviien 30:390cab7cd6e6 239
viviien 35:4cb2ed6dd2d2 240 u_k1 = 10 * error1;
viviien 35:4cb2ed6dd2d2 241 u_k2 = 10 * error2;
viviien 35:4cb2ed6dd2d2 242 u_k3 = 10 * error3;
viviien 30:390cab7cd6e6 243
viviien 32:60a71dcfdb7a 244 newmotor1 = u_k1;
viviien 32:60a71dcfdb7a 245 newmotor2 = u_k2;
viviien 32:60a71dcfdb7a 246 newmotor3 = u_k3;
viviien 30:390cab7cd6e6 247
viviien 30:390cab7cd6e6 248 if (newmotor1>1.0f){
viviien 35:4cb2ed6dd2d2 249 newmotor1 = 1.0f;
viviien 23:18b0be02187f 250 }
viviien 30:390cab7cd6e6 251 if (newmotor1<-1.0f){
viviien 30:390cab7cd6e6 252 newmotor1 = -1.0f;
viviien 30:390cab7cd6e6 253 }
viviien 30:390cab7cd6e6 254
viviien 30:390cab7cd6e6 255 if (newmotor2>1.0f){
viviien 30:390cab7cd6e6 256 newmotor2 =1.0f;
viviien 30:390cab7cd6e6 257 }
viviien 30:390cab7cd6e6 258 if (newmotor2<-1.0f){
viviien 30:390cab7cd6e6 259 newmotor2 = -1.0f;
viviien 30:390cab7cd6e6 260 }
viviien 30:390cab7cd6e6 261
viviien 30:390cab7cd6e6 262 if (newmotor3>1.0f){
viviien 30:390cab7cd6e6 263 newmotor3 =1.0f;
viviien 30:390cab7cd6e6 264 }
viviien 30:390cab7cd6e6 265 if (newmotor3<-1.0f){
viviien 30:390cab7cd6e6 266 newmotor3 = -1.0f;
viviien 30:390cab7cd6e6 267 }
viviien 30:390cab7cd6e6 268
viviien 35:4cb2ed6dd2d2 269
viviien 30:390cab7cd6e6 270 motor1_pwm.write(fabs(newmotor1));
viviien 32:60a71dcfdb7a 271 motor1_dir.write(newmotor1<0);
viviien 30:390cab7cd6e6 272
viviien 30:390cab7cd6e6 273 motor2_pwm.write(fabs(newmotor2));
viviien 32:60a71dcfdb7a 274 motor2_dir.write(newmotor2>0);
viviien 30:390cab7cd6e6 275
viviien 30:390cab7cd6e6 276 motor3_pwm.write(fabs(newmotor3));
viviien 32:60a71dcfdb7a 277 motor3_dir.write(newmotor3<0);
viviien 30:390cab7cd6e6 278
viviien 32:60a71dcfdb7a 279 time_sin += 0.002;
viviien 30:390cab7cd6e6 280 }
viviien 25:eb3204e45d33 281
viviien 32:60a71dcfdb7a 282
viviien 28:43a1d67ff8ea 283
viviien 28:43a1d67ff8ea 284 ///////////////////
viviien 28:43a1d67ff8ea 285 // MAIN FUNCTION //
viviien 28:43a1d67ff8ea 286 ///////////////////
viviien 28:43a1d67ff8ea 287
viviien 35:4cb2ed6dd2d2 288 int main(void) {
viviien 35:4cb2ed6dd2d2 289
viviien 35:4cb2ed6dd2d2 290 delta_calctheta1 ();
viviien 35:4cb2ed6dd2d2 291 delta_calctheta2 ();
viviien 35:4cb2ed6dd2d2 292 delta_calctheta3 ();
viviien 35:4cb2ed6dd2d2 293
viviien 35:4cb2ed6dd2d2 294 pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 30:390cab7cd6e6 295
viviien 35:4cb2ed6dd2d2 296 motor_timer.attach(&motor, 0.002);
viviien 35:4cb2ed6dd2d2 297
viviien 35:4cb2ed6dd2d2 298
viviien 30:390cab7cd6e6 299 int frequency_pwm = 10000; //10 kHz PWM
viviien 33:88fbf14d8aaf 300 motor1_pwm.period_us(6);
viviien 33:88fbf14d8aaf 301 motor2_pwm.period_us(6);
viviien 33:88fbf14d8aaf 302 motor3_pwm.period_us(6);
viviien 35:4cb2ed6dd2d2 303
viviien 35:4cb2ed6dd2d2 304 pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 35:4cb2ed6dd2d2 305 pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 35:4cb2ed6dd2d2 306 pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
viviien 37:15446e49ff3d 307
viviien 37:15446e49ff3d 308 char cc = pc.getc();
viviien 37:15446e49ff3d 309
viviien 35:4cb2ed6dd2d2 310
viviien 35:4cb2ed6dd2d2 311
viviien 35:4cb2ed6dd2d2 312
viviien 35:4cb2ed6dd2d2 313 pc.baud(115200);
Feike 19:9c8ab7922191 314
viviien 28:43a1d67ff8ea 315 while(true){
viviien 33:88fbf14d8aaf 316
viviien 37:15446e49ff3d 317 // while (boxcheckC == 0) {
viviien 37:15446e49ff3d 318 // if (boxcheckA == 1 && x0>=-76 && x0<=75) {
viviien 37:15446e49ff3d 319 // x0=x0+1.0f ;
viviien 37:15446e49ff3d 320 // wait(1.0/20);
viviien 37:15446e49ff3d 321 // }
viviien 37:15446e49ff3d 322 // if (boxcheckB== 1 && x0>=-75 && x0<=76) {
viviien 37:15446e49ff3d 323 // x0=x0-1.0f;
viviien 37:15446e49ff3d 324 // wait(1.0/20);
viviien 37:15446e49ff3d 325 // }
viviien 37:15446e49ff3d 326 // }
viviien 37:15446e49ff3d 327 //
viviien 37:15446e49ff3d 328 // wait(1.5);
viviien 37:15446e49ff3d 329 // savedX = x0;
viviien 37:15446e49ff3d 330 //
viviien 37:15446e49ff3d 331 // while (boxcheckC == 0) {
viviien 37:15446e49ff3d 332 // if (boxcheckA==1 && y0>=-76 && y0<=75){
viviien 37:15446e49ff3d 333 // y0=y0+1.0f;
viviien 37:15446e49ff3d 334 // wait(1.0/20);
viviien 37:15446e49ff3d 335 // }
viviien 37:15446e49ff3d 336 // if (boxcheckB==1 && y0>=-75 && y0<=76){
viviien 37:15446e49ff3d 337 // y0=y0-1.0f;
viviien 37:15446e49ff3d 338 // wait(1.0/20);
viviien 37:15446e49ff3d 339 // }
viviien 37:15446e49ff3d 340 // }
viviien 37:15446e49ff3d 341 //
viviien 37:15446e49ff3d 342 // wait(1.5);
viviien 37:15446e49ff3d 343 // savedY = y0;
viviien 37:15446e49ff3d 344 //
viviien 37:15446e49ff3d 345 // while (boxcheckC == 0) {
viviien 37:15446e49ff3d 346 // if (boxcheckA == 1 && z0>=-179 && z0<=-158){
viviien 37:15446e49ff3d 347 // z0=z0+1.0f;
viviien 37:15446e49ff3d 348 // wait(1.0/20);
viviien 37:15446e49ff3d 349 // }
viviien 37:15446e49ff3d 350 // if (boxcheckB == 1 && z0>=-178 && z0<=-157){
viviien 37:15446e49ff3d 351 // z0=z0-1.0f;
viviien 37:15446e49ff3d 352 // wait(1.0/20);
viviien 37:15446e49ff3d 353 // }
viviien 37:15446e49ff3d 354 // }
viviien 37:15446e49ff3d 355 //
viviien 37:15446e49ff3d 356 // wait(1.5);
viviien 37:15446e49ff3d 357 // savedZ = z0;
viviien 37:15446e49ff3d 358 //
viviien 37:15446e49ff3d 359 // if (boxcheckC == 1) {
viviien 37:15446e49ff3d 360 // sign = 1;
viviien 37:15446e49ff3d 361 // }
viviien 37:15446e49ff3d 362 //
viviien 37:15446e49ff3d 363 // wait(5.0);
viviien 37:15446e49ff3d 364 //
viviien 37:15446e49ff3d 365 // sign = 0;
viviien 37:15446e49ff3d 366 //
viviien 37:15446e49ff3d 367 // float diffZ = -158 - z0;
viviien 37:15446e49ff3d 368 // float diffX = 0 - x0;
viviien 37:15446e49ff3d 369 // float diffY = 0 - y0;
viviien 37:15446e49ff3d 370 //
viviien 37:15446e49ff3d 371 // for (int i=0; i<=diffZ; i++) {
viviien 37:15446e49ff3d 372 // z0 = z0+1;
viviien 37:15446e49ff3d 373 // }
viviien 37:15446e49ff3d 374 //
viviien 37:15446e49ff3d 375 // for (int i=0; i<=diffY; i++) {
viviien 37:15446e49ff3d 376 // if (diffY>0) {
viviien 37:15446e49ff3d 377 // y0 = y0+1;
viviien 37:15446e49ff3d 378 // }
viviien 37:15446e49ff3d 379 // if (diffY<0) {
viviien 37:15446e49ff3d 380 // y0 = y0-1;
viviien 37:15446e49ff3d 381 // }
viviien 37:15446e49ff3d 382 // }
viviien 37:15446e49ff3d 383 //
viviien 37:15446e49ff3d 384 // for (int i=0; i<=diffX; i++) {
viviien 37:15446e49ff3d 385 // if (diffX>0) {
viviien 37:15446e49ff3d 386 // x0 = x0+1;
viviien 37:15446e49ff3d 387 // }
viviien 37:15446e49ff3d 388 // if (diffX<0) {
viviien 37:15446e49ff3d 389 // x0 = x0-1;
viviien 37:15446e49ff3d 390 // }
viviien 37:15446e49ff3d 391 // }
viviien 37:15446e49ff3d 392 //
viviien 37:15446e49ff3d 393 //break;
viviien 35:4cb2ed6dd2d2 394
viviien 35:4cb2ed6dd2d2 395
viviien 35:4cb2ed6dd2d2 396
viviien 35:4cb2ed6dd2d2 397 // pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 35:4cb2ed6dd2d2 398 // pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 35:4cb2ed6dd2d2 399 // wait(0.1);
viviien 35:4cb2ed6dd2d2 400
viviien 35:4cb2ed6dd2d2 401
viviien 37:15446e49ff3d 402 char cc = pc.getc();
viviien 37:15446e49ff3d 403 if (cc=='d' && x0>=-76 && x0<=75) {
viviien 37:15446e49ff3d 404 x0=x0+1.0f ;
viviien 37:15446e49ff3d 405 }
viviien 37:15446e49ff3d 406
viviien 37:15446e49ff3d 407 if (cc=='a' && x0>=-75 && x0<=76) {
viviien 37:15446e49ff3d 408
viviien 37:15446e49ff3d 409 x0=x0-1.0f;
viviien 37:15446e49ff3d 410 }
viviien 37:15446e49ff3d 411
viviien 37:15446e49ff3d 412 if (cc=='w' && y0>=-76 && y0<=75){
viviien 37:15446e49ff3d 413 y0=y0+1.0f;
viviien 37:15446e49ff3d 414 pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 37:15446e49ff3d 415 pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 37:15446e49ff3d 416 pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
viviien 37:15446e49ff3d 417
viviien 37:15446e49ff3d 418 }
viviien 37:15446e49ff3d 419
viviien 37:15446e49ff3d 420 if (cc=='s' && y0>=-75 && y0<=76){
viviien 37:15446e49ff3d 421 y0=y0-1.0f;
viviien 37:15446e49ff3d 422 pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 37:15446e49ff3d 423 pc .printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 37:15446e49ff3d 424 pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
viviien 37:15446e49ff3d 425 }
viviien 37:15446e49ff3d 426
viviien 37:15446e49ff3d 427 if (cc=='u' && z0>=-211 && z0<=-130){
viviien 37:15446e49ff3d 428 pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 37:15446e49ff3d 429 pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 37:15446e49ff3d 430 pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
viviien 37:15446e49ff3d 431 z0=z0+1.0f;
viviien 37:15446e49ff3d 432 }
viviien 37:15446e49ff3d 433
viviien 37:15446e49ff3d 434 if (cc=='j' && z0>=-210 && z0<=-129){
viviien 37:15446e49ff3d 435 pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 37:15446e49ff3d 436 pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 37:15446e49ff3d 437 pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
viviien 37:15446e49ff3d 438 z0=z0-1.0f;
viviien 37:15446e49ff3d 439 }
viviien 22:9f911405e096 440 }
viviien 32:60a71dcfdb7a 441 //END MAIN
viviien 22:9f911405e096 442 }