1 | #include <Wire.h>
|
2 |
|
3 | //Ein/Aus-gänge
|
4 | int PinDirX = 2;//2Pin4
|
5 | int PinDirY = 3;//3Pin5
|
6 | int PinDirZ = 4;//4Pin6
|
7 | int PinStepX = 5;//5Pin11
|
8 | int PinStepY = 6;//6Pin12
|
9 | int PinStepZ = 7;//7Pin13
|
10 | int PinEndschalterX = 8;//Pin14
|
11 | int PinEndschalterY = 9;//Pin15
|
12 | int PinEndschalterZ = 10;//Pin16
|
13 | int PinEmpfangsStatus = 11;//Pin17
|
14 | int PinSyncStart = 12;//Pin18
|
15 | int PinStatusLED = 13;
|
16 |
|
17 | // Parameter für Achsen
|
18 | long XCoordinate = 0;
|
19 | long YCoordinate = 0;
|
20 | long ZCoordinate = 0;
|
21 |
|
22 | int Impulslange = 30;
|
23 | float WegProSchritt = 0.0025;
|
24 | float Beschleunigung = 250;
|
25 | float Geschwindigkeit = 250;
|
26 | boolean Motorfahren = false;
|
27 | boolean XInvert = false;
|
28 | boolean YInvert = false;
|
29 | boolean ZInvert = false;
|
30 | long XCoo = 0;
|
31 | long YCoo = 0;
|
32 | long ZCoo = 0;
|
33 |
|
34 | void setup()
|
35 | {
|
36 | Wire.begin(25);
|
37 | Wire.onReceive(receiveEvent);
|
38 | Wire.onRequest(requestEvent);
|
39 | Serial.begin(9600);
|
40 | pinMode(PinDirX, OUTPUT);
|
41 | pinMode(PinDirY, OUTPUT);
|
42 | pinMode(PinDirZ, OUTPUT);
|
43 | pinMode(PinStepX, OUTPUT);
|
44 | pinMode(PinStepY, OUTPUT);
|
45 | pinMode(PinStepZ, OUTPUT);
|
46 | pinMode(PinStatusLED, OUTPUT);
|
47 | pinMode(PinEmpfangsStatus, OUTPUT);
|
48 | pinMode(PinEndschalterZ, INPUT);
|
49 | pinMode(PinEndschalterX, INPUT);
|
50 | pinMode(PinEndschalterY, INPUT);
|
51 | pinMode(PinSyncStart, INPUT);
|
52 |
|
53 | digitalWrite(PinDirX, LOW);
|
54 | digitalWrite(PinDirY, LOW);
|
55 | digitalWrite(PinDirZ, LOW);
|
56 | digitalWrite(PinStepX, HIGH);
|
57 | digitalWrite(PinStepY, HIGH);
|
58 | digitalWrite(PinStepZ, HIGH);
|
59 | digitalWrite(PinStatusLED, LOW);
|
60 | digitalWrite(PinEmpfangsStatus, HIGH);
|
61 | }
|
62 |
|
63 | void loop()
|
64 | {
|
65 | if(Motorfahren == true)
|
66 | {
|
67 | digitalWrite(PinStatusLED, HIGH);
|
68 | int Delay = 9999;
|
69 | long Schritte[3] = {0,0,0};
|
70 | long SchritteMax = 0;
|
71 | float TimerX = 0;
|
72 | float TimerY = 0;
|
73 | float TimerZ = 0;
|
74 | int WertProSchrittX = 0;
|
75 | int WertProSchrittY = 0;
|
76 | int WertProSchrittZ = 0;
|
77 | long SchrittMaxBeschleunigung = 0;
|
78 | long SchrittMinBremsen = 0;
|
79 | float GeschwindigkeitAchse = 0;
|
80 | float Verfahrstrecke = 0;
|
81 | long Switches[3] = {0,0,0};
|
82 | float Schritt[3] = {0,0,0};
|
83 | //long ZahlerX = 0;
|
84 | //long ZahlerY = 0;
|
85 | //long ZahlerZ = 0;
|
86 | static unsigned long Micros1;
|
87 | static unsigned long Micros2;
|
88 |
|
89 | long Time1;
|
90 | long Time2;
|
91 | long TimeGes1;
|
92 |
|
93 | Schritte[0] = XCoordinate-XCoo;
|
94 | if(Schritte[0] < 0)
|
95 | {
|
96 | Schritte[0] = Schritte[0]*(-1);
|
97 | if(XInvert == true)
|
98 | {
|
99 | digitalWrite(PinDirX,LOW);
|
100 | WertProSchrittX = -1;
|
101 | }
|
102 | else
|
103 | {
|
104 | digitalWrite(PinDirX,HIGH);
|
105 | WertProSchrittX = 1;
|
106 | }
|
107 |
|
108 | }
|
109 | else
|
110 | {
|
111 | if(XInvert == true)
|
112 | {
|
113 | digitalWrite(PinDirX,HIGH);
|
114 | WertProSchrittX = 1;
|
115 | }
|
116 | else
|
117 | {
|
118 | digitalWrite(PinDirX,LOW);
|
119 | WertProSchrittX = -1;
|
120 | }
|
121 | }
|
122 |
|
123 | Schritte[1] = YCoordinate-YCoo;
|
124 | if(Schritte[1] < 0)
|
125 | {
|
126 | Schritte[1] = Schritte[1]*(-1);
|
127 | if(YInvert == true)
|
128 | {
|
129 | digitalWrite(PinDirY,LOW);
|
130 | WertProSchrittY = -1;
|
131 | }
|
132 | else
|
133 | {
|
134 | digitalWrite(PinDirY,HIGH);
|
135 | WertProSchrittY = 1;
|
136 | }
|
137 | }
|
138 | else
|
139 | {
|
140 | if(YInvert == true)
|
141 | {
|
142 | digitalWrite(PinDirY,HIGH);
|
143 | WertProSchrittY = 1;
|
144 | }
|
145 | else
|
146 | {
|
147 | digitalWrite(PinDirY,LOW);
|
148 | WertProSchrittY = -1;
|
149 | }
|
150 | }
|
151 |
|
152 | Schritte[2] = ZCoordinate-ZCoo;
|
153 | if(Schritte[2] < 0)
|
154 | {
|
155 | Schritte[2] = Schritte[2]*(-1);
|
156 | if(ZInvert == true)
|
157 | {
|
158 | digitalWrite(PinDirZ,LOW);
|
159 | WertProSchrittZ = -1;
|
160 | }
|
161 | else
|
162 | {
|
163 | digitalWrite(PinDirZ,HIGH);
|
164 | WertProSchrittZ = 1;
|
165 | }
|
166 | }
|
167 | else
|
168 | {
|
169 | if(ZInvert == true)
|
170 | {
|
171 | digitalWrite(PinDirZ,HIGH);
|
172 | WertProSchrittZ = 1;
|
173 | }
|
174 | else
|
175 | {
|
176 | digitalWrite(PinDirZ,LOW);
|
177 | WertProSchrittZ = -1;
|
178 | }
|
179 | }
|
180 | Serial.println("X: " + String(Schritte[0]) + " Y: " + String(Schritte[1])+ " Z: " + String(Schritte[2]));
|
181 |
|
182 | for(int a = 0;a < 3; a++)
|
183 | {
|
184 | if(Schritte[a] > SchritteMax)
|
185 | SchritteMax = Schritte[a];
|
186 | }
|
187 | //Serial.println("X: " + String(SchritteMax));
|
188 |
|
189 | Verfahrstrecke = sqrt(sq(Schritte[0]) + sq(Schritte[1]) + sq(Schritte[2]));
|
190 | //Serial.println("Verfahrstrecke: " + String(Verfahrstrecke));
|
191 |
|
192 | GeschwindigkeitAchse = (SchritteMax/(Verfahrstrecke/Geschwindigkeit));
|
193 | Serial.println("GeschwindigkeitAchse: " + String(GeschwindigkeitAchse));
|
194 |
|
195 | long SchritteBB = ((9999-GeschwindigkeitAchse)/Beschleunigung);
|
196 | if((SchritteBB*2) > SchritteMax)
|
197 | {
|
198 | SchrittMaxBeschleunigung = SchritteMax/2;
|
199 | SchrittMinBremsen = SchritteMax/2;
|
200 | }
|
201 | else
|
202 | {
|
203 | SchrittMaxBeschleunigung = (SchritteBB + 5);
|
204 | SchrittMinBremsen = SchritteMax - (SchritteBB + 5);
|
205 | }
|
206 |
|
207 | Serial.println("SchritteBB: " + String(SchritteBB) + " SchrittMaxBeschleunigung: " + String(SchrittMaxBeschleunigung)+ " SchrittMinBremsen: " + String(SchrittMinBremsen));
|
208 |
|
209 | if(Schritte[0] > 0)
|
210 | Schritt[0] = (SchritteMax/float(Schritte[0]));
|
211 | else
|
212 | Schritt[0] = SchritteMax + 1;
|
213 | if(Schritte[1] > 0)
|
214 | Schritt[1] = (SchritteMax/float(Schritte[1]));
|
215 | else
|
216 | Schritt[1] = SchritteMax + 1;
|
217 | if(Schritte[2] > 0)
|
218 | Schritt[2] = (SchritteMax/float(Schritte[2]));
|
219 | else
|
220 | Schritt[2] = SchritteMax + 1;
|
221 | Switches[0] = long(Schritt[0]);
|
222 | Switches[1] = long(Schritt[1]);
|
223 | Switches[2] = long(Schritt[2]);
|
224 |
|
225 | //Serial.println("SchrittX: " + String(Schritt[0]) + " Y: " + String(Schritt[1])+ " Z: " + String(Schritt[2]));
|
226 | //Serial.println("SwitchesX: " + String(Switches[0]) + " Y: " + String(Switches[1])+ " Z: " + String(Switches[2]));
|
227 |
|
228 |
|
229 |
|
230 | for(long a = 0;a < SchritteMax; a++)
|
231 | {
|
232 | //Time1 = micros();
|
233 | Micros1 = micros();
|
234 | TimerX++;
|
235 | TimerY++;
|
236 | TimerZ++;
|
237 | if(digitalRead(PinEndschalterX) == LOW || digitalRead(PinEndschalterY) == LOW || digitalRead(PinEndschalterZ) == LOW)
|
238 | {
|
239 | break;
|
240 | }
|
241 | if(TimerX >= Switches[0])
|
242 | {
|
243 | digitalWrite(PinStepX, LOW);
|
244 | delayMicroseconds(Impulslange);
|
245 | digitalWrite(PinStepX, HIGH);
|
246 | TimerX -= Schritt[0];
|
247 | XCoordinate += WertProSchrittX;
|
248 | }
|
249 | if(TimerY >= Switches[1])
|
250 | {
|
251 | digitalWrite(PinStepY, LOW);
|
252 | delayMicroseconds(Impulslange);
|
253 | digitalWrite(PinStepY, HIGH);
|
254 | TimerY -= Schritt[1];
|
255 | YCoordinate += WertProSchrittY;
|
256 | }
|
257 | if(TimerZ >= Switches[2])
|
258 | {
|
259 | digitalWrite(PinStepZ, LOW);
|
260 | delayMicroseconds(Impulslange);
|
261 | digitalWrite(PinStepZ, HIGH);
|
262 | TimerZ -= Schritt[2];
|
263 | ZCoordinate += WertProSchrittZ;
|
264 | }
|
265 |
|
266 | if(a < SchrittMaxBeschleunigung)
|
267 | {
|
268 | Delay -= int(Beschleunigung);
|
269 | }
|
270 |
|
271 | if(a > SchrittMinBremsen)
|
272 | {
|
273 | Delay += int(Beschleunigung);
|
274 | }
|
275 |
|
276 | if(Delay < GeschwindigkeitAchse)
|
277 | {
|
278 | Delay = int(GeschwindigkeitAchse);
|
279 | }
|
280 |
|
281 | for(long l=0;l<15000;l++)
|
282 | {
|
283 | Micros2 = micros();
|
284 | if (Micros2 - Micros1 > Delay)
|
285 | {
|
286 | break;
|
287 | }
|
288 | }
|
289 |
|
290 | //Time2 = micros();
|
291 | //TimeGes1 = Time2 - Time1;
|
292 | //Serial.println("Schritt: " + String(a) + " Zeit: " + String(TimeGes1));
|
293 | }
|
294 | //Serial.println("ZahlerX: " + String(ZahlerX) + " Y: " + String(ZahlerY)+ " Z: " + String(ZahlerZ));
|
295 | digitalWrite(PinStatusLED, LOW);
|
296 | Motorfahren = false;
|
297 | }
|
298 | }
|
299 |
|
300 | void receiveEvent(int HowMany)
|
301 | {
|
302 | int TimerI2C = 0;
|
303 | long WertRead = 0;
|
304 | long WertRead1 = 0;
|
305 | long WertRead2 = 0;
|
306 |
|
307 | if(0< Wire.available())
|
308 | {
|
309 | int a = Wire.read();
|
310 | if(a == 0)
|
311 | {
|
312 | int b = Wire.read();
|
313 | if(b == 0)
|
314 | {
|
315 | XCoordinate = 0;
|
316 | while(0< Wire.available())
|
317 | {
|
318 | long c = Wire.read();
|
319 | if(TimerI2C == 0)
|
320 | WertRead += c*10000/WegProSchritt;
|
321 | if(TimerI2C == 1)
|
322 | WertRead += c*100/WegProSchritt;
|
323 | if(TimerI2C == 2)
|
324 | WertRead += c/WegProSchritt;
|
325 | if(TimerI2C == 3)
|
326 | WertRead += long(c/WegProSchritt/100);
|
327 | if(TimerI2C == 4)
|
328 | WertRead += long(c/WegProSchritt/10000);
|
329 | if(TimerI2C == 5)
|
330 | {
|
331 | if(c == 0)
|
332 | XCoordinate = WertRead;
|
333 | if(c == 1)
|
334 | XCoordinate = -(WertRead);
|
335 | }
|
336 | TimerI2C++;
|
337 | }
|
338 | //Serial.println("X: " + String(XCoordinate) + " Wert: " + String(WertRead));
|
339 | /*XCoo = 123;
|
340 | YCoo = 165;
|
341 | ZCoo = -465;
|
342 | Motorfahren = true;*/
|
343 | }
|
344 | else if(b == 1)
|
345 | {
|
346 | YCoordinate = 0;
|
347 | while(0< Wire.available())
|
348 | {
|
349 | long c = Wire.read();
|
350 | if(TimerI2C == 0)
|
351 | WertRead += c*10000/WegProSchritt;
|
352 | if(TimerI2C == 1)
|
353 | WertRead += c*100/WegProSchritt;
|
354 | if(TimerI2C == 2)
|
355 | WertRead += c/WegProSchritt;
|
356 | if(TimerI2C == 3)
|
357 | WertRead += long(c/WegProSchritt/100);
|
358 | if(TimerI2C == 4)
|
359 | WertRead += long(c/WegProSchritt/10000);
|
360 | if(TimerI2C == 5)
|
361 | {
|
362 | if(c == 0)
|
363 | YCoordinate = WertRead;
|
364 | if(c == 1)
|
365 | YCoordinate = -(WertRead);
|
366 | }
|
367 | TimerI2C++;
|
368 | }
|
369 | //Serial.println("Y: " + String(YCoordinate) + " Wert: " + String(WertRead));
|
370 | }
|
371 | else if(b == 2)
|
372 | {
|
373 | ZCoordinate = 0;
|
374 | while(0< Wire.available())
|
375 | {
|
376 | long c = Wire.read();
|
377 | if(TimerI2C == 0)
|
378 | WertRead += c*10000/WegProSchritt;
|
379 | if(TimerI2C == 1)
|
380 | WertRead += c*100/WegProSchritt;
|
381 | if(TimerI2C == 2)
|
382 | WertRead += c/WegProSchritt;
|
383 | if(TimerI2C == 3)
|
384 | WertRead += long(c/WegProSchritt/100);
|
385 | if(TimerI2C == 4)
|
386 | WertRead += long(c/WegProSchritt/10000);
|
387 | if(TimerI2C == 5)
|
388 | {
|
389 | if(c == 0)
|
390 | ZCoordinate = WertRead;
|
391 | if(c == 1)
|
392 | ZCoordinate = -(WertRead);
|
393 | }
|
394 | TimerI2C++;
|
395 | }
|
396 | //Serial.println("Z: " + String(ZCoordinate) + " Wert: " + String(WertRead));
|
397 | }
|
398 | else if(b == 6)
|
399 | {
|
400 |
|
401 | while(0< Wire.available())
|
402 | {
|
403 | int c = Wire.read();
|
404 | if(TimerI2C == 0)
|
405 | Impulslange = c;
|
406 | TimerI2C++;
|
407 | }
|
408 | //Serial.println("Impulslänge: " + String(Impulslange));
|
409 | }
|
410 | else if(b == 7)
|
411 | {
|
412 | WegProSchritt = 0;
|
413 | while(0< Wire.available())
|
414 | {
|
415 | float c = Wire.read();
|
416 | if(TimerI2C == 0)
|
417 | WegProSchritt += c;
|
418 | if(TimerI2C == 1)
|
419 | WegProSchritt += c/100.0;
|
420 | if(TimerI2C == 2)
|
421 | WegProSchritt += c/10000.0;
|
422 | TimerI2C++;
|
423 | }
|
424 | //Serial.println("WegProSchritt: " + String(WegProSchritt,4));
|
425 | }
|
426 | else if(b == 8)
|
427 | {
|
428 | Beschleunigung = 0;
|
429 | while(0< Wire.available())
|
430 | {
|
431 | float c = Wire.read();
|
432 | if(TimerI2C == 0)
|
433 | Beschleunigung += c*100.0;
|
434 | if(TimerI2C == 1)
|
435 | Beschleunigung += c;
|
436 | TimerI2C++;
|
437 | }
|
438 | //Serial.println("Beschleunigung: " + String(Beschleunigung));
|
439 | }
|
440 | else if(b == 9)
|
441 | {
|
442 | while(0< Wire.available())
|
443 | {
|
444 | float c = Wire.read();
|
445 | if(TimerI2C == 0)
|
446 | if(c == 1)
|
447 | XInvert = true;
|
448 | else
|
449 | XInvert = false;
|
450 | TimerI2C++;
|
451 | }
|
452 | //Serial.println("XInvert: " + String(XInvert));
|
453 | }
|
454 | else if(b == 10)
|
455 | {
|
456 | while(0< Wire.available())
|
457 | {
|
458 | float c = Wire.read();
|
459 | if(TimerI2C == 0)
|
460 | if(c == 1)
|
461 | YInvert = true;
|
462 | else
|
463 | YInvert = false;
|
464 | TimerI2C++;
|
465 | }
|
466 | //Serial.println("YInvert: " + String(YInvert));
|
467 | }
|
468 | else if(b == 11)
|
469 | {
|
470 | while(0< Wire.available())
|
471 | {
|
472 | float c = Wire.read();
|
473 | if(TimerI2C == 0)
|
474 | if(c == 1)
|
475 | ZInvert = true;
|
476 | else
|
477 | ZInvert = false;
|
478 | TimerI2C++;
|
479 | }
|
480 | //Serial.println("ZInvert: " + String(ZInvert));
|
481 | }
|
482 |
|
483 | }
|
484 | else if(a == 1)
|
485 | {
|
486 | int b = Wire.read();
|
487 | if(b == 0)
|
488 | {
|
489 | int c = Wire.read();
|
490 | if(c == 0)
|
491 | {
|
492 | while(0< Wire.available())
|
493 | {
|
494 | float d = Wire.read();
|
495 | if(TimerI2C == 0)
|
496 | WertRead += d*10000/WegProSchritt;
|
497 | if(TimerI2C == 1)
|
498 | WertRead += d*100/WegProSchritt;
|
499 | if(TimerI2C == 2)
|
500 | WertRead += d/WegProSchritt;
|
501 | if(TimerI2C == 3)
|
502 | WertRead += long(d/WegProSchritt/100);
|
503 | if(TimerI2C == 4)
|
504 | WertRead += long(d/WegProSchritt/10000);
|
505 | if(TimerI2C == 5)
|
506 | {
|
507 | if(d == 0)
|
508 | XCoo = WertRead;
|
509 | if(d == 1)
|
510 | XCoo = -(WertRead);
|
511 | }
|
512 |
|
513 | if(TimerI2C == 6)
|
514 | WertRead1 += d*10000/WegProSchritt;
|
515 | if(TimerI2C == 7)
|
516 | WertRead1 += d*100/WegProSchritt;
|
517 | if(TimerI2C == 8)
|
518 | WertRead1 += d/WegProSchritt;
|
519 | if(TimerI2C == 9)
|
520 | WertRead1 += long(d/WegProSchritt/100);
|
521 | if(TimerI2C == 10)
|
522 | WertRead1 += long(d/WegProSchritt/10000);
|
523 | if(TimerI2C == 11)
|
524 | {
|
525 | if(d == 0)
|
526 | YCoo = WertRead1;
|
527 | if(d == 1)
|
528 | YCoo = -(WertRead1);
|
529 | }
|
530 |
|
531 | if(TimerI2C == 12)
|
532 | WertRead2 += d*10000/WegProSchritt;
|
533 | if(TimerI2C == 13)
|
534 | WertRead2 += d*100/WegProSchritt;
|
535 | if(TimerI2C == 14)
|
536 | WertRead2 += d/WegProSchritt;
|
537 | if(TimerI2C == 15)
|
538 | WertRead2 += long(d/WegProSchritt/100);
|
539 | if(TimerI2C == 16)
|
540 | WertRead2 += long(d/WegProSchritt/10000);
|
541 | if(TimerI2C == 17)
|
542 | {
|
543 | if(d == 0)
|
544 | ZCoo = WertRead2;
|
545 | if(d == 1)
|
546 | ZCoo = -(WertRead2);
|
547 | }
|
548 | TimerI2C++;
|
549 | }
|
550 | Motorfahren = true;
|
551 | //Serial.println("XCoo: " + String(XCoo) + " YCoo: " + String(YCoo) + " ZCoo: " + String(ZCoo));
|
552 | }
|
553 | else if(c == 1)
|
554 | {
|
555 |
|
556 | }
|
557 | else if(c == 94)
|
558 | {
|
559 |
|
560 | }
|
561 | }
|
562 | }
|
563 | }
|
564 | }
|
565 |
|
566 | void requestEvent()
|
567 | {
|
568 | /* String Empfangen = "";
|
569 | // if(0< Wire.available())
|
570 | // {
|
571 | // Serial.println("Available");
|
572 | int a = Wire.read();
|
573 | Empfangen += String(a);
|
574 | //}
|
575 | Serial.println("request: " + Empfangen);
|
576 | // Serial.println("Schritte X: " + String(SchritteXZehntausender) + " Y: " + String(SchritteXHunderter) + " Z: " + String(SchritteXEiner));
|
577 | byte ArraySenden[] = {2,1};
|
578 | Wire.write(ArraySenden,2);*/
|
579 | }
|