Václav Lízner / Mbed 2 deprecated 2048

Dependencies:   MMA8451Q mbed

main.cpp@6:5711a5b57d17, 2016-01-12 (annotated)

Committer:
xlizne01
Date:
Tue Jan 12 22:41:40 2016 +0000
Revision:
6:5711a5b57d17
Parent:
5:d6738e1238d8
Child:
7:29f6c0e0b427
funkcni seskupovani }chzb v yobrayovan na LED1 a LED2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xlizne01 0:ae588eb31707 1 #include "mbed.h"
xlizne01 1:b23831b703fe 2 #include "MMA8451Q.h"
xlizne01 1:b23831b703fe 3
xlizne01 1:b23831b703fe 4 #define MMA8451_I2C_ADDRESS (0x1d<<1)
xlizne01 5:d6738e1238d8 5 #define MOVE_ANGLE 35 // > degrees for move
xlizne01 2:3278e4fd8fc2 6
xlizne01 2:3278e4fd8fc2 7 MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);
xlizne01 2:3278e4fd8fc2 8
xlizne01 2:3278e4fd8fc2 9 void accelerometer(void);
xlizne01 3:94bfc4de4ab1 10 void zobrazeni(void);
xlizne01 3:94bfc4de4ab1 11 void barva(int,int);
xlizne01 4:4930b1cb20bd 12 void pohyb(void);
xlizne01 6:5711a5b57d17 13 void proved(int);
xlizne01 2:3278e4fd8fc2 14
xlizne01 6:5711a5b57d17 15 int i,j,k;
xlizne01 6:5711a5b57d17 16 int p=1;
xlizne01 2:3278e4fd8fc2 17 int RED, GREEN, BLUE;
xlizne01 2:3278e4fd8fc2 18 int RledA, RledB, GledA, GledB, BledA, BledB;
xlizne01 4:4930b1cb20bd 19 float xAngle, yAngle;
xlizne01 3:94bfc4de4ab1 20
xlizne01 2:3278e4fd8fc2 21 char acc_dir;
xlizne01 3:94bfc4de4ab1 22
xlizne01 6:5711a5b57d17 23 int pole[4][4]={{2,0,0,0}, {0,0,0,2}, {2,0,0,0}, {2,0,0,0}};
xlizne01 1:b23831b703fe 24
xlizne01 0:ae588eb31707 25 DigitalOut prvni(PTE5);
xlizne01 0:ae588eb31707 26 DigitalOut druhy(PTE4);
xlizne01 0:ae588eb31707 27 DigitalOut treti(PTE3);
xlizne01 0:ae588eb31707 28 DigitalOut ctvrty(PTE2);
xlizne01 0:ae588eb31707 29 DigitalOut paty(PTB11);
xlizne01 0:ae588eb31707 30 DigitalOut sesty(PTB10);
xlizne01 0:ae588eb31707 31 DigitalOut sedmy(PTB9);
xlizne01 0:ae588eb31707 32 DigitalOut osmy(PTB8);
xlizne01 0:ae588eb31707 33
xlizne01 0:ae588eb31707 34 DigitalOut Rled1(PTC9);
xlizne01 0:ae588eb31707 35 DigitalOut Rled2(PTC8);
xlizne01 0:ae588eb31707 36 DigitalOut Rled3(PTA5);
xlizne01 0:ae588eb31707 37 DigitalOut Rled4(PTA4);
xlizne01 0:ae588eb31707 38 DigitalOut Rled5(PTA12);
xlizne01 0:ae588eb31707 39 DigitalOut Rled6(PTD4);
xlizne01 0:ae588eb31707 40 DigitalOut Rled7(PTA1);
xlizne01 0:ae588eb31707 41 DigitalOut Rled8(PTA2);
xlizne01 0:ae588eb31707 42
xlizne01 0:ae588eb31707 43 DigitalOut Gled1(PTA13);
xlizne01 0:ae588eb31707 44 DigitalOut Gled2(PTD5);
xlizne01 0:ae588eb31707 45 DigitalOut Gled3(PTD0);
xlizne01 0:ae588eb31707 46 DigitalOut Gled4(PTD2);
xlizne01 0:ae588eb31707 47 DigitalOut Gled5(PTD3);
xlizne01 0:ae588eb31707 48 DigitalOut Gled6(PTD1);
xlizne01 0:ae588eb31707 49 DigitalOut Gled7(PTE31);
xlizne01 0:ae588eb31707 50 DigitalOut Gled8(PTB0);
xlizne01 0:ae588eb31707 51
xlizne01 0:ae588eb31707 52 DigitalOut Bled1(PTC1);
xlizne01 0:ae588eb31707 53 DigitalOut Bled2(PTE29);
xlizne01 0:ae588eb31707 54 DigitalOut Bled3(PTC2);
xlizne01 0:ae588eb31707 55 DigitalOut Bled4(PTB3);
xlizne01 0:ae588eb31707 56 DigitalOut Bled5(PTB2);
xlizne01 0:ae588eb31707 57 DigitalOut Bled6(PTE21);
xlizne01 0:ae588eb31707 58 DigitalOut Bled7(PTE20);
xlizne01 0:ae588eb31707 59 DigitalOut Bled8(PTB1);
xlizne01 0:ae588eb31707 60
xlizne01 0:ae588eb31707 61
xlizne01 0:ae588eb31707 62 int main()
xlizne01 0:ae588eb31707 63 {
xlizne01 3:94bfc4de4ab1 64 while(1)
xlizne01 3:94bfc4de4ab1 65 {
xlizne01 4:4930b1cb20bd 66 accelerometer();
xlizne01 4:4930b1cb20bd 67 zobrazeni();
xlizne01 4:4930b1cb20bd 68 pohyb();
xlizne01 0:ae588eb31707 69 }
xlizne01 3:94bfc4de4ab1 70 }
xlizne01 4:4930b1cb20bd 71
xlizne01 4:4930b1cb20bd 72 void pohyb()
xlizne01 4:4930b1cb20bd 73 {
xlizne01 4:4930b1cb20bd 74 switch(acc_dir)
xlizne01 4:4930b1cb20bd 75 {
xlizne01 5:d6738e1238d8 76 case 'D': //pohyb smerem dolu
xlizne01 5:d6738e1238d8 77 if(p==0)
xlizne01 5:d6738e1238d8 78 {
xlizne01 6:5711a5b57d17 79 proved(1);
xlizne01 5:d6738e1238d8 80 p=1;
xlizne01 5:d6738e1238d8 81 }
xlizne01 4:4930b1cb20bd 82 break;
xlizne01 6:5711a5b57d17 83 case 'R': // pohyb smerem vpravo
xlizne01 5:d6738e1238d8 84 if(p==0)
xlizne01 5:d6738e1238d8 85 {
xlizne01 6:5711a5b57d17 86 proved(2);
xlizne01 5:d6738e1238d8 87 p=1;
xlizne01 5:d6738e1238d8 88 }
xlizne01 4:4930b1cb20bd 89 break;
xlizne01 5:d6738e1238d8 90 case 'U': // pohyb smerem nahoru
xlizne01 5:d6738e1238d8 91 if(p==0)
xlizne01 5:d6738e1238d8 92 {
xlizne01 6:5711a5b57d17 93 proved(3);
xlizne01 5:d6738e1238d8 94 p=1;
xlizne01 5:d6738e1238d8 95 }
xlizne01 4:4930b1cb20bd 96 break;
xlizne01 6:5711a5b57d17 97 case 'L': // pohyb smerem vlevo
xlizne01 5:d6738e1238d8 98 if(p==0)
xlizne01 5:d6738e1238d8 99 {
xlizne01 6:5711a5b57d17 100 proved(4);
xlizne01 5:d6738e1238d8 101 p=1;
xlizne01 5:d6738e1238d8 102 }
xlizne01 4:4930b1cb20bd 103 break;
xlizne01 5:d6738e1238d8 104 case '0': // detekovana nulova pozice
xlizne01 5:d6738e1238d8 105 p=0;
xlizne01 5:d6738e1238d8 106 break;
xlizne01 4:4930b1cb20bd 107 default:
xlizne01 5:d6738e1238d8 108 break;
xlizne01 5:d6738e1238d8 109
xlizne01 4:4930b1cb20bd 110 }
xlizne01 4:4930b1cb20bd 111 }
xlizne01 4:4930b1cb20bd 112
xlizne01 6:5711a5b57d17 113 void proved(int x)
xlizne01 6:5711a5b57d17 114 {
xlizne01 6:5711a5b57d17 115 int a,b,c,d,n;
xlizne01 6:5711a5b57d17 116
xlizne01 6:5711a5b57d17 117 for(n=0;n<4;n++)
xlizne01 6:5711a5b57d17 118 {
xlizne01 6:5711a5b57d17 119
xlizne01 6:5711a5b57d17 120 if(x==1) // dolu
xlizne01 6:5711a5b57d17 121 {
xlizne01 6:5711a5b57d17 122 a=pole[n][0];
xlizne01 6:5711a5b57d17 123 b=pole[n][1];
xlizne01 6:5711a5b57d17 124 c=pole[n][2];
xlizne01 6:5711a5b57d17 125 d=pole[n][3];
xlizne01 6:5711a5b57d17 126 }
xlizne01 6:5711a5b57d17 127
xlizne01 6:5711a5b57d17 128 if(x==2) // vpravo
xlizne01 6:5711a5b57d17 129 {
xlizne01 6:5711a5b57d17 130 a=pole[3][n];
xlizne01 6:5711a5b57d17 131 b=pole[2][n];
xlizne01 6:5711a5b57d17 132 c=pole[1][n];
xlizne01 6:5711a5b57d17 133 d=pole[0][n];
xlizne01 6:5711a5b57d17 134 }
xlizne01 6:5711a5b57d17 135
xlizne01 6:5711a5b57d17 136 if(x==3) // nahoru
xlizne01 6:5711a5b57d17 137 {
xlizne01 6:5711a5b57d17 138 a=pole[n][3];
xlizne01 6:5711a5b57d17 139 b=pole[n][2];
xlizne01 6:5711a5b57d17 140 c=pole[n][1];
xlizne01 6:5711a5b57d17 141 d=pole[n][0];
xlizne01 6:5711a5b57d17 142 }
xlizne01 6:5711a5b57d17 143
xlizne01 6:5711a5b57d17 144 if(x==4) // vlevo
xlizne01 6:5711a5b57d17 145 {
xlizne01 6:5711a5b57d17 146 a=pole[0][n];
xlizne01 6:5711a5b57d17 147 b=pole[1][n];
xlizne01 6:5711a5b57d17 148 c=pole[2][n];
xlizne01 6:5711a5b57d17 149 d=pole[3][n];
xlizne01 6:5711a5b57d17 150 }
xlizne01 6:5711a5b57d17 151
xlizne01 6:5711a5b57d17 152 //sesypani
xlizne01 6:5711a5b57d17 153 if(a==0)
xlizne01 6:5711a5b57d17 154 {a=b; b=c; c=d; d=0;}
xlizne01 6:5711a5b57d17 155 if(a==0)
xlizne01 6:5711a5b57d17 156 {a=b; b=c; c=d; d=0;}
xlizne01 6:5711a5b57d17 157 if(a==0)
xlizne01 6:5711a5b57d17 158 {a=b; b=c; c=d; d=0;}
xlizne01 6:5711a5b57d17 159 if(b==0)
xlizne01 6:5711a5b57d17 160 {b=c; c=d; d=0;}
xlizne01 6:5711a5b57d17 161 if(b==0)
xlizne01 6:5711a5b57d17 162 {b=c; c=d; d=0;}
xlizne01 6:5711a5b57d17 163 if(c==0)
xlizne01 6:5711a5b57d17 164 {c=d; d=0;}
xlizne01 6:5711a5b57d17 165
xlizne01 6:5711a5b57d17 166 //spojeni
xlizne01 6:5711a5b57d17 167 if(a==b)
xlizne01 6:5711a5b57d17 168 {a=2*a; b=c; c=d; d=0;}
xlizne01 6:5711a5b57d17 169 if(b==c)
xlizne01 6:5711a5b57d17 170 {b=2*b; c=d; d=0;}
xlizne01 6:5711a5b57d17 171 if(c==d)
xlizne01 6:5711a5b57d17 172 {c=2*c; d=0;}
xlizne01 6:5711a5b57d17 173
xlizne01 6:5711a5b57d17 174 if(x==1) // dolu
xlizne01 6:5711a5b57d17 175 {
xlizne01 6:5711a5b57d17 176 pole[n][0]=a;
xlizne01 6:5711a5b57d17 177 pole[n][1]=b;
xlizne01 6:5711a5b57d17 178 pole[n][2]=c;
xlizne01 6:5711a5b57d17 179 pole[n][3]=d;
xlizne01 6:5711a5b57d17 180 }
xlizne01 6:5711a5b57d17 181 if(x==2) // vpravo
xlizne01 6:5711a5b57d17 182 {
xlizne01 6:5711a5b57d17 183 pole[3][n]=a;
xlizne01 6:5711a5b57d17 184 pole[2][n]=b;
xlizne01 6:5711a5b57d17 185 pole[1][n]=c;
xlizne01 6:5711a5b57d17 186 pole[0][n]=d;
xlizne01 6:5711a5b57d17 187 }
xlizne01 6:5711a5b57d17 188
xlizne01 6:5711a5b57d17 189 if(x==3) // nahoru
xlizne01 6:5711a5b57d17 190 {
xlizne01 6:5711a5b57d17 191 pole[n][3]=a;
xlizne01 6:5711a5b57d17 192 pole[n][2]=b;
xlizne01 6:5711a5b57d17 193 pole[n][1]=c;
xlizne01 6:5711a5b57d17 194 pole[n][0]=d;
xlizne01 6:5711a5b57d17 195 }
xlizne01 6:5711a5b57d17 196
xlizne01 6:5711a5b57d17 197 if(x==4) // vlevo
xlizne01 6:5711a5b57d17 198 {
xlizne01 6:5711a5b57d17 199 pole[0][n]=a;
xlizne01 6:5711a5b57d17 200 pole[1][n]=b;
xlizne01 6:5711a5b57d17 201 pole[2][n]=c;
xlizne01 6:5711a5b57d17 202 pole[3][n]=d;
xlizne01 6:5711a5b57d17 203 }
xlizne01 6:5711a5b57d17 204 }
xlizne01 6:5711a5b57d17 205 }
xlizne01 3:94bfc4de4ab1 206
xlizne01 3:94bfc4de4ab1 207 void zobrazeni()
xlizne01 3:94bfc4de4ab1 208 {
xlizne01 3:94bfc4de4ab1 209 for(k=0;k<4;k++)
xlizne01 3:94bfc4de4ab1 210 {
xlizne01 3:94bfc4de4ab1 211 prvni=0;
xlizne01 3:94bfc4de4ab1 212 druhy=0;
xlizne01 3:94bfc4de4ab1 213 treti=0;
xlizne01 3:94bfc4de4ab1 214 ctvrty=0;
xlizne01 3:94bfc4de4ab1 215 paty=0;
xlizne01 3:94bfc4de4ab1 216 sesty=0;
xlizne01 3:94bfc4de4ab1 217 sedmy=0;
xlizne01 3:94bfc4de4ab1 218 osmy=0;
xlizne01 3:94bfc4de4ab1 219
xlizne01 3:94bfc4de4ab1 220 if(k==0)
xlizne01 0:ae588eb31707 221 {
xlizne01 3:94bfc4de4ab1 222 prvni=1;
xlizne01 3:94bfc4de4ab1 223 druhy=1;
xlizne01 0:ae588eb31707 224 }
xlizne01 3:94bfc4de4ab1 225
xlizne01 3:94bfc4de4ab1 226 if(k==1)
xlizne01 0:ae588eb31707 227 {
xlizne01 3:94bfc4de4ab1 228 treti=1;
xlizne01 3:94bfc4de4ab1 229 ctvrty=1;
xlizne01 0:ae588eb31707 230 }
xlizne01 3:94bfc4de4ab1 231
xlizne01 3:94bfc4de4ab1 232 if(k==2)
xlizne01 0:ae588eb31707 233 {
xlizne01 3:94bfc4de4ab1 234 paty=1;
xlizne01 3:94bfc4de4ab1 235 sesty=1;
xlizne01 0:ae588eb31707 236 }
xlizne01 3:94bfc4de4ab1 237
xlizne01 3:94bfc4de4ab1 238 if(k==3)
xlizne01 0:ae588eb31707 239 {
xlizne01 3:94bfc4de4ab1 240 sedmy=1;
xlizne01 3:94bfc4de4ab1 241 osmy=1;
xlizne01 0:ae588eb31707 242 }
xlizne01 0:ae588eb31707 243
xlizne01 3:94bfc4de4ab1 244 for(j=0;j<4;j++)
xlizne01 3:94bfc4de4ab1 245 {
xlizne01 3:94bfc4de4ab1 246
xlizne01 3:94bfc4de4ab1 247 barva(j,k);
xlizne01 3:94bfc4de4ab1 248
xlizne01 6:5711a5b57d17 249 for(i=15;i>0;i--)
xlizne01 3:94bfc4de4ab1 250 {
xlizne01 3:94bfc4de4ab1 251 if(RED>0)
xlizne01 3:94bfc4de4ab1 252 {
xlizne01 3:94bfc4de4ab1 253 RledA=1;
xlizne01 3:94bfc4de4ab1 254 RledB=1;
xlizne01 3:94bfc4de4ab1 255 }
xlizne01 3:94bfc4de4ab1 256 else
xlizne01 3:94bfc4de4ab1 257 {
xlizne01 3:94bfc4de4ab1 258 RledA=0;
xlizne01 3:94bfc4de4ab1 259 RledB=0;
xlizne01 3:94bfc4de4ab1 260 }
xlizne01 0:ae588eb31707 261
xlizne01 3:94bfc4de4ab1 262 if(GREEN>0)
xlizne01 3:94bfc4de4ab1 263 {
xlizne01 3:94bfc4de4ab1 264 GledA=1;
xlizne01 3:94bfc4de4ab1 265 GledB=1;
xlizne01 3:94bfc4de4ab1 266 }
xlizne01 3:94bfc4de4ab1 267 else
xlizne01 3:94bfc4de4ab1 268 {
xlizne01 3:94bfc4de4ab1 269 GledA=0;
xlizne01 3:94bfc4de4ab1 270 GledB=0;
xlizne01 3:94bfc4de4ab1 271 }
xlizne01 3:94bfc4de4ab1 272
xlizne01 3:94bfc4de4ab1 273 if(BLUE>0)
xlizne01 3:94bfc4de4ab1 274 {
xlizne01 3:94bfc4de4ab1 275 BledA=1;
xlizne01 3:94bfc4de4ab1 276 BledB=1;
xlizne01 3:94bfc4de4ab1 277 }
xlizne01 3:94bfc4de4ab1 278 else
xlizne01 3:94bfc4de4ab1 279 {
xlizne01 3:94bfc4de4ab1 280 BledA=0;
xlizne01 3:94bfc4de4ab1 281 BledB=0;
xlizne01 3:94bfc4de4ab1 282 }
xlizne01 0:ae588eb31707 283
xlizne01 3:94bfc4de4ab1 284 if(j==0)
xlizne01 3:94bfc4de4ab1 285 {
xlizne01 3:94bfc4de4ab1 286 Rled1=RledA;
xlizne01 3:94bfc4de4ab1 287 Rled2=RledB;
xlizne01 3:94bfc4de4ab1 288 Gled1=GledA;
xlizne01 3:94bfc4de4ab1 289 Gled2=GledB;
xlizne01 3:94bfc4de4ab1 290 Bled1=BledA;
xlizne01 3:94bfc4de4ab1 291 Bled2=BledB;
xlizne01 3:94bfc4de4ab1 292 }
xlizne01 3:94bfc4de4ab1 293
xlizne01 3:94bfc4de4ab1 294 if(j==1)
xlizne01 3:94bfc4de4ab1 295 {
xlizne01 3:94bfc4de4ab1 296 Rled3=RledA;
xlizne01 3:94bfc4de4ab1 297 Rled4=RledB;
xlizne01 3:94bfc4de4ab1 298 Gled3=GledA;
xlizne01 3:94bfc4de4ab1 299 Gled4=GledB;
xlizne01 3:94bfc4de4ab1 300 Bled3=BledA;
xlizne01 3:94bfc4de4ab1 301 Bled4=BledB;
xlizne01 3:94bfc4de4ab1 302 }
xlizne01 3:94bfc4de4ab1 303
xlizne01 3:94bfc4de4ab1 304 if(j==2)
xlizne01 3:94bfc4de4ab1 305 {
xlizne01 3:94bfc4de4ab1 306 Rled5=RledA;
xlizne01 3:94bfc4de4ab1 307 Rled6=RledB;
xlizne01 3:94bfc4de4ab1 308 Gled5=GledA;
xlizne01 3:94bfc4de4ab1 309 Gled6=GledB;
xlizne01 3:94bfc4de4ab1 310 Bled5=BledA;
xlizne01 3:94bfc4de4ab1 311 Bled6=BledB;
xlizne01 3:94bfc4de4ab1 312 }
xlizne01 3:94bfc4de4ab1 313
xlizne01 3:94bfc4de4ab1 314 if(j==3)
xlizne01 3:94bfc4de4ab1 315 {
xlizne01 3:94bfc4de4ab1 316 Rled7=RledA;
xlizne01 3:94bfc4de4ab1 317 Rled8=RledB;
xlizne01 3:94bfc4de4ab1 318 Gled7=GledA;
xlizne01 3:94bfc4de4ab1 319 Gled8=GledB;
xlizne01 3:94bfc4de4ab1 320 Bled7=BledA;
xlizne01 3:94bfc4de4ab1 321 Bled8=BledB;
xlizne01 3:94bfc4de4ab1 322 }
xlizne01 3:94bfc4de4ab1 323
xlizne01 6:5711a5b57d17 324 wait(0.000005);
xlizne01 3:94bfc4de4ab1 325 RED--;
xlizne01 3:94bfc4de4ab1 326 BLUE--;
xlizne01 3:94bfc4de4ab1 327 GREEN--;
xlizne01 6:5711a5b57d17 328
xlizne01 3:94bfc4de4ab1 329 }
xlizne01 3:94bfc4de4ab1 330 }
xlizne01 3:94bfc4de4ab1 331 }
xlizne01 3:94bfc4de4ab1 332 }
xlizne01 3:94bfc4de4ab1 333
xlizne01 3:94bfc4de4ab1 334 void barva(int j , int k)
xlizne01 3:94bfc4de4ab1 335 {
xlizne01 3:94bfc4de4ab1 336 RED=pole[j][k];
xlizne01 3:94bfc4de4ab1 337 GREEN=pole[j][k];
xlizne01 3:94bfc4de4ab1 338 BLUE=pole[j][k];
xlizne01 0:ae588eb31707 339 }
xlizne01 3:94bfc4de4ab1 340
xlizne01 2:3278e4fd8fc2 341
xlizne01 2:3278e4fd8fc2 342 void accelerometer()
xlizne01 2:3278e4fd8fc2 343 {
xlizne01 2:3278e4fd8fc2 344 float ax, ay, az;
xlizne01 4:4930b1cb20bd 345
xlizne01 2:3278e4fd8fc2 346 ax = acc.getAccX();
xlizne01 2:3278e4fd8fc2 347 ay = acc.getAccY();
xlizne01 2:3278e4fd8fc2 348 az = acc.getAccZ();
xlizne01 2:3278e4fd8fc2 349
xlizne01 2:3278e4fd8fc2 350 xAngle = atan( ax / (sqrt((ay)*(ay) + (az)*(az)))) * 60;
xlizne01 2:3278e4fd8fc2 351 yAngle = atan( ay / (sqrt((ax)*(ax) + (az)*(az)))) * 60;
xlizne01 2:3278e4fd8fc2 352
xlizne01 2:3278e4fd8fc2 353
xlizne01 4:4930b1cb20bd 354 if((abs(xAngle)+abs(yAngle))<15)
xlizne01 4:4930b1cb20bd 355 {
xlizne01 4:4930b1cb20bd 356 acc_dir = '0'; // nula
xlizne01 4:4930b1cb20bd 357 }
xlizne01 4:4930b1cb20bd 358
xlizne01 4:4930b1cb20bd 359
xlizne01 2:3278e4fd8fc2 360 if(abs(xAngle) >= abs(yAngle))
xlizne01 2:3278e4fd8fc2 361 {
xlizne01 2:3278e4fd8fc2 362 if(xAngle >= MOVE_ANGLE)
xlizne01 2:3278e4fd8fc2 363 {
xlizne01 2:3278e4fd8fc2 364 acc_dir = 'U'; // +X
xlizne01 2:3278e4fd8fc2 365 }
xlizne01 2:3278e4fd8fc2 366
xlizne01 2:3278e4fd8fc2 367 if(xAngle <= -MOVE_ANGLE)
xlizne01 2:3278e4fd8fc2 368 {
xlizne01 2:3278e4fd8fc2 369 acc_dir = 'D'; // -X
xlizne01 2:3278e4fd8fc2 370 }
xlizne01 2:3278e4fd8fc2 371 }
xlizne01 2:3278e4fd8fc2 372
xlizne01 2:3278e4fd8fc2 373 else
xlizne01 2:3278e4fd8fc2 374 {
xlizne01 2:3278e4fd8fc2 375 if(yAngle >= MOVE_ANGLE)
xlizne01 2:3278e4fd8fc2 376 {
xlizne01 2:3278e4fd8fc2 377 acc_dir = 'L'; // +Y
xlizne01 2:3278e4fd8fc2 378 }
xlizne01 2:3278e4fd8fc2 379
xlizne01 2:3278e4fd8fc2 380 if(yAngle <= -MOVE_ANGLE)
xlizne01 2:3278e4fd8fc2 381 {
xlizne01 2:3278e4fd8fc2 382 acc_dir = 'R'; // -Y
xlizne01 2:3278e4fd8fc2 383 }
xlizne01 2:3278e4fd8fc2 384 }
xlizne01 2:3278e4fd8fc2 385
xlizne01 3:94bfc4de4ab1 386 }