Arduino Controlled Car Factory: Body Shop

1#include <l293d.h>
2#include <Servo.h>
3#include <Wire.h>
4
5
6
7
8/*Controller  Setup*******************************************************
9
10  Pin mapping  Arduino Mega Shield
11 ***********************************************************************/
12//  constants describing the pin mapping of the L293 chip
13// const unint8_t[3] "motor_name"  = {chip_enable, input1, input2};
14const uint8_t M1[3] = {8, 45, 43};
15const  uint8_t M2[3] = {9, 40, 38};
16const uint8_t M3[3] = {7, 41, 30};
17const uint8_t  M4[3] = {10, 36, 32};
18const uint8_t M5[3] = {6, 48, 46};
19const uint8_t M6[3]  = {11, 26, 24};
20const uint8_t M7[3] = {5, 44, 28};
21const uint8_t M8[3] = {4,  42, 34};
22
23// Pin mapping of digital I/O pins
24const uint8_t D1 = 16;
25const  uint8_t D2 = 35;
26const uint8_t D3 = 37;
27const uint8_t D4 = 39;
28const uint8_t  D5 = 23;
29const uint8_t D6 = 3;   // Interrupt 1
30const uint8_t D7 = 2;   //  Interrupt 0
31const uint8_t D8 = 14;
32const uint8_t D9 = 15;
33const uint8_t  D10 = 19; // Interrupt 4
34const uint8_t D11 = 18; // Interrupt 5
35const uint8_t  D12 = 17;
36
37// Pin mapping of analog I/O pins
38const uint8_t a1 = A1;
39const  uint8_t a2 = A4;
40const uint8_t a3 = A5;
41const uint8_t a4 = A7;
42const uint8_t  a5 = A0;
43const uint8_t a6 = A2;
44const uint8_t a7 = A3;
45const uint8_t a8  = A6;
46const uint8_t a9 = A11;
47const uint8_t a10 = A10;
48const uint8_t a11  = A9;
49const uint8_t a12 = A8;
50const uint8_t a13 = A12;
51const uint8_t a14  = A13;
52
53// Pin mapping extension board
54int exDataPin = 25;
55int exClockPin  = 33;
56int exLatchPin = 29;
57int exOEPin = 27;
58int exLEDgreen = 47;
59int  exLEDred = 49;
60
61/*Machine Setup**********************************************************
62
63  maschinenspezifische Variablen und Konstanten
64 ***********************************************************************/
65
66l293d  motorAxis1(0);
67l293d motorAxis2(0);
68l293d motorAxis3(0);
69Servo servoAxis5;
70l293d  vakuum(0);
71
72l293d laser(0);
73l293d millingAxisX(0);
74l293d millingMachine(0);
75
76//  declare limit switch variables
77const uint8_t limitSwitch1 = a10;
78const uint8_t  limitSwitch2 = D9;
79const uint8_t limitSwitch3 = D8;
80
81const uint8_t limitSwitchMillingX  = D5;
82
83// declare counter Pin variables
84const uint8_t counter1Pin = D11;
85const  uint8_t counter2Pin = D6;
86const uint8_t counter3Pin = D7;
87
88const uint8_t  counterMillingXPin = D10;
89
90// declare dimensions [Millimeter]
91const float  l1_0 = 15; // horizontaler Abstand von Drehpunkt zum Mittelpunkt der Achse 2 (X-Y)
92const  float l1_1 = 103.5; // vertikaler Abstand vom Punkt (0|0|0) zum Mittlepunkt der  Achse 2 (nur Z)
93const float l2 = 150;
94const float l3 = 142.5;
95const float  l5 = 60;
96const float l6 = 30;
97
98const float l_tool = 5;
99
100// declare  minimum / maximum range
101const float rangeAxis1[2] = { -160.0, 0};
102const float  rangeAxis2[2] = { 30.0, 110.0};
103const float rangeAxis3[2] = { 45.0, 140.0};
104const  float rangeAxis3s[2] = { 0.0, 80.0};
105const float rangeAxis5[2] = {0.0 , 180.0};
106
107const  float degree2encoder = 23.978; // Impulse / Grad (Drehscheibe)
108const float degree2impulse  = 1.256;
109
110const float pi = 3.14159;
111const float rad2deg = 180 / pi;
112const  float deg2rad = 1 / rad2deg;
113
114/*Program Setup**********************************************************
115
116  enthaelt globale programmspezifische Variablen
117 ***********************************************************************/
118
119int  i2c_befehl = 0;
120int i2c_status = 0;
121
122// Interrupt
123volatile int posA1,  posA2, posA3, posA5;
124volatile int posMX = 0;
125int directA1 = 1, directA2 =  1, directA3 = 1, directMX = 1;
126int clearPos1 = 1 , clearPos2 = 1, clearPos3 =  1, clearPosMX = 1;
127
128// Geschwindigkeitssteuerung Motoren
129uint8_t speedA1  = 140;
130uint8_t speedA2 = 160;
131uint8_t speedA3 = 130;
132
133// Geschwindigkeitssteuerung  Servo
134int servoA5ot, servoA5nt;
135// Debounce
136volatile unsigned int dbotMX  = 0;
137volatile unsigned int dbntMX = 0;
138
139// Winkel
140float q1, q2, q3s,  q3, q4, q5, q5s; // q2s: Stellwinkel fuer Achse 2
141
142// Positionsdatenbank
143float  posLib[][3] =
144{
145  { -150.0, -92.0, 144.0},
146  { -202.0, -114.0, 18.0}, //  Werkstueckaufnahme
147  { -190.0, -92.0, 55.0},
148  { -177.0, -40.0, 55.0}, //  Laser
149  { -177.0, -40.0, 55.0},
150  { -150.0, 65.0, 66.0}, // Fraese
151  {  -33.0, 229.0, 100.0},
152  { -30.0, 229.0, 80.0}, // Montage
153};
154
155/************************************************************************
156  Unterprogramme
157 ***********************************************************************/
158
159//  ISR
160
161// ISR Achse 1
162void counterA1()
163{
164  posA1 = (posA1 + directA1)  * clearPos1;
165}
166
167// ISR Achse 2
168void counterA2()
169{
170  posA2 = (posA2  + directA2) * clearPos2;
171}
172// ISR Achse 3
173void counterA3()
174{
175  posA3  = (posA3 + directA3) * clearPos3;
176}
177// ISR Milling Axis X
178void counterMX()
179{
180  dbntMX = millis();
181  if ((dbntMX - dbotMX) > 1)
182  {
183    posMX = (posMX  + directMX);
184    dbotMX = dbntMX;
185  }
186  posMX *= clearPosMX;
187}
188
189bool  pos(l293d & axis_ref, int limitSwitch, int currentPos, int & clearPos_ref, int &  directAxis_ref, int target, int motorSpeed)
190{
191  if (target == 0)
192  {
193    if (digitalRead(limitSwitch) == 0)
194    {
195      clearPos_ref = 0;
196      axis_ref.ccw(motorSpeed);
197      return false;
198    }
199    else
200    {
201      axis_ref.stop();
202      clearPos_ref = 1;
203      return true;
204    }
205  }
206  else if (target  == currentPos)
207  {
208    axis_ref.stop();
209    // remove in order to keep  measureing: directAxis_ref = 0;
210    return true;
211  }
212  else if (target  > currentPos)
213  {
214    axis_ref.cw(motorSpeed);
215    directAxis_ref = 1;
216    return false;
217  }
218  else if (target < currentPos)
219  {
220    axis_ref.ccw(motorSpeed);
221    directAxis_ref = -1;
222    return false;
223  }
224}
225
226int pos1(int target,  int motorspeed)
227{
228  int currentPos = posA1;
229  return pos(motorAxis1, limitSwitch1,  currentPos, clearPos1, directA1, target, motorspeed);
230}
231int pos2(int target,  int motorspeed)
232{
233  int currentPos = posA2;
234  return pos(motorAxis2, limitSwitch2,  currentPos, clearPos2, directA2, target, motorspeed);
235}
236int pos3(int target,  int motorspeed)
237{
238  int currentPos = posA3;
239  return pos(motorAxis3, limitSwitch3,  currentPos, clearPos3, directA3, target, motorspeed);
240}
241int posMillingX(int  target, int motorspeed)
242{
243  int currentPos = posMX;
244  return pos(millingAxisX,  limitSwitchMillingX, currentPos, clearPosMX, directMX, target, motorspeed);
245}
246
247//  Achssteuerung
248// auf Achsteuerung soll NICHT von auen zugegriffen werden, nur  ber die moveJ Befehle
249int axis1(float angle, int motorspeed)
250{
251  /* Definition  Achse 1:
252
253      Nullpunkt bei q1 = 0
254      Arbeitsbereich einseitig fr  q1 <= 0
255  */
256
257  // Pruefe ob der Winkel im gueltigen Arbeitsbereich der  Maschine liegt
258  if (angle < rangeAxis1[0] || angle > rangeAxis1[1])
259  {
260    errorDisplay(301, 1);
261    return 1;
262  }
263  else
264  {
265    int impulse  = (angle * -1) * degree2encoder;
266    return pos1(impulse, motorspeed);
267  }
268}
269
270int  axis2(float angle, int motorspeed)
271{
272  // Pruefe ob der Winkel im gueltigen  Arbeitsbereich der Maschine liegt
273  if (angle < rangeAxis2[0] || angle > rangeAxis2[1])
274  {
275    errorDisplay(301, 2);
276    return 1;
277  }
278  else
279  {
280    //  Umrechnung von Zielwinkel in Maschinenwinkel, Multiplikation mit der Encoderkonstante
281    int impulse = (-71 + (180 - angle)) * degree2encoder;
282    return pos2(impulse,  motorspeed);
283  }
284}
285
286int axis3(float angle, float  rq2, int motorspeed)
287{
288  /* Randbediengungen Achse 3
289      Stellwinkel: der vom Roboter gestellte Winkel  der Achse 3
290      q3: Winkel zwischgen l2 und l3:
291      q3 = (180-q2) - q3s
292      q3s = 180 - q2 - q3
293
294      Bedingungen
295      1) q3s: (180-q2) - q3s  > 60, damit Bewegung gueltig ist.
296      2) q3s: 0 < q3s < 80
297      Bei Fehler  1) Bewegung unterbrechen
298      Bei Fehler 2) Bewegung abbrechen
299  */
300
301  // Pruefe ob der Zielwinkel im gueltigen Arbeitsbereich der Maschine liegt (Bedingung  1 und 2)
302  q3s = 180 - rq2 - angle;
303  if (q3s < rangeAxis3s[0] || q3s > rangeAxis3s[1]  || angle < rangeAxis3[0] || angle > rangeAxis3[1])
304  {
305    errorDisplay(301,  3);
306    return 1;
307  }
308  else
309  {
310    // Umrechnung von Zielwinkel  in Maschinenwinkel, Multiplikation mit der Encoderkonstante
311    // Pruefe in  Echtzeit, ob der Winkel zwischen l2 und l3 > 80 ist (Bedinung 1)
312    // lq2 und  lq3 enthalten Winkel in Echtzeit
313    float lq2 = (109.0 - (posA2 / degree2encoder));
314    int lq3s = posA3 / degree2encoder;
315    if ((180 - lq2 - lq3s) < rangeAxis3[0]  || (180 - lq2 - lq3s) > rangeAxis3[1])
316    {
317      motorAxis3.stop();
318      errorDisplay(302, 3);
319      return 0;
320    }
321    else
322    {
323      int impulse = q3s * degree2encoder;
324      return pos3(impulse, motorspeed);
325
326    }
327  }
328}
329
330int axis5(int angle, int servospeed)
331{
332  // variable  posA5 enthaelt Winkel in Grad
333  // servospeed: Zeit in ms
334  if (angle < rangeAxis5[0]  || angle > rangeAxis5[1])
335  {
336    errorDisplay(301, 5);
337    return 1;
338  }
339  else
340  {
341    if (servospeed == 0)
342    {
343      posA5 = q5s +  140 + (posA1 / degree2encoder * -1);
344      servoAxis5.write(posA5);
345      return  1;
346    }
347    else {
348      servoA5nt = millis();
349      if ((servoA5nt  - servoA5ot) > servospeed)
350      {
351        servoA5ot = servoA5nt;
352
353        if (angle > posA5)
354        {
355          posA5++;
356          servoAxis5.write(posA5);
357        }
358        else if (angle < posA5)
359        {
360          posA5--;
361          servoAxis5.write(posA5);
362        }
363        else
364        {
365          return 1;
366        }
367      }
368      else
369      {
370        return  0;
371      }
372    }
373  }
374}
375
376void referencePoint()
377{
378  int refP  = 0;
379  while (!refP)
380  {
381    refP = pos1(0, speedA1);
382    refP &= pos2(0,  speedA2);
383    refP &= pos3(0, speedA3);
384    refP &= axis5(140, 15);
385  }
386}
387
388void  errorDisplay(int errorCode, int detail)
389{
390  /*  Fehler Codes
391
392      Achsensteuerung
393      - 301: Ungueltige Bewegungsanfrage Achse
394      - 302: Konflikt mit anderer  Achse
395
396      Systemfehler:
397      - 100: Unbekannter Fehler
398  */
399  switch (errorCode)
400  {
401    case 301:
402      Serial.print("301: Ungueltige  Bewegungsanfrage Achse ");
403      Serial.println(detail);
404      break;
405    case 302:
406      Serial.print("302: Konflikt mit anderer Achse - Achse ");
407      Serial.println(detail);
408      break;
409    default:
410      Serial.println("100:  Unbekannter Fehler  aufgetreten");
411      break;
412  }
413}
414
415/*Inverse  Kinematic******************************************************
416
417  errechnet  aus Zielpunkt mit Vektor (ggf. inklusive Werkzeug die Stell-
418  winkel des Roboters
419
420  Definition:
421  - alpha: Winkel des Werkzeugs auf Element
422  - targetPoint:  Zielpunkt des TCPs am Objekt
423  - int toolParameter: Index fr die Auswahl passender  Paramter aus einer
424    Werkzeugdatenbank
425
426  Vektor:
427  - Darstellung  als  Array
428  - Datentyp: float
429  - eventueller vierter Index enthlt die Laenge  des Vektors
430
431  Algorithmus:
432  - Zielvektor errechnen: x1 = 0
433                          x2  = 0
434                          x3 = -1
435  - Inverse Kinematik nach: https://www.youtube.com/watch?v=3s2x4QsD3uM
436    Vorbereitung: Maschinendaten laden und vect_5toTool berechnen
437
438    q1:  vect_0to5 = vect_0toPoint - vect_5toTool
439        q1 = atan (vect_0to5[1]/vect0to5[0])
440    q3: vect_2to5 = vect_0to5 - vect_0to2
441                                (cos  q1 * l1_0)
442                  = vect_0to5 - (sin qi * l1_0)
443                                (l1_1         )
444        beliebiges Dreieck l2, l3, |vect_2to5|, q3 ueber Kosinussatz:
445        q3 = acos (l3^2 + l2^2 - |vect_2to5|^2) / (2*l3*l2)
446    q2: beliebiges  Dreieck l2, l3, |vect_2to5|, q2 ueber Kosinussatz:
447        q2' = acos (|vect_2to5|^2  + l2^2 - l3^2) / (2*|vect_2to5|*l2)
448        q2'' = asin (vect_2to5[2] / |vect_2to5|)
449        q2 = q2' + q2''
450    q5: beta = q1
451        alpha = Zielwinkel (alpha)
452        q5 = 180 - alpha -beta
453
454
455 ***********************************************************************/
456void  inverseKinematic(float alpha, float targetPoint[])
457{
458  // Errechne Zielvektor
459  float unit_vect_5toTool[4];
460  float vect_5toTool[4];
461  // Automatische Ausrichtung  nach unten
462  unit_vect_5toTool[0] = 0;
463  unit_vect_5toTool[1] = 0;
464  unit_vect_5toTool[2]  = -1;
465  vect_5toTool[3] = 1; //sqrt (pow(vect_5toTool[0], 2) + pow(vect_5toTool[1],  2) + pow(vect_5toTool[2], 2));
466
467  // Verlaengere Zielvektor auf Abmessungen  des Roboters
468  for (int i = 0; i < 4; i++)
469  {
470    vect_5toTool[i] = unit_vect_5toTool[i]  * (l5 + l6 + l_tool);
471  }
472
473  // Berechne vect_0to5
474  float vect_0to5[3];
475  for (int i = 0; i < 3; i++)
476  {
477    vect_0to5[i] = targetPoint[i] - vect_5toTool[i];
478    //Serial.println(vect_0to5[i]);
479  }
480
481  // Berechne q1
482  // q1 =  atan (vect_0to5[0] / vect_0to5[1]) * rad2deg; // nur wenn vect_0to5[1] > 0
483
484  // cos-1(y/hyp_0to5)
485  q1 = acos (vect_0to5[1] / sqrt(pow(vect_0to5[0], 2)  + pow(vect_0to5[1], 2))) * rad2deg;
486  if (vect_0to5[0] < 0)
487  {
488    q1  *= -1;
489  }
490
491  // Berechne vect_2to5
492  float vect_2to5[4];
493  vect_2to5[0]  = vect_0to5[0] - (sin(q1 * deg2rad) * l1_0);
494  vect_2to5[1] = vect_0to5[1] -  (cos(q1 * deg2rad) * l1_0);
495  vect_2to5[2] = vect_0to5[2] - l1_1;
496  vect_2to5[3]  = sqrt (pow(vect_2to5[0], 2) + pow(vect_2to5[1], 2) + pow(vect_2to5[2], 2));
497
498  // Berechne q3
499  q3 = acos((pow(l3, 2) + pow(l2, 2) - pow(vect_2to5[3], 2))  / (2 * l3 * l2)) * rad2deg;
500
501  // Berechne q2
502  float q2_1, q2_2;
503  q2_1  = acos((pow(vect_2to5[3], 2) + pow(l2, 2) - pow(l3, 2)) / (2 * vect_2to5[3] * l2))  * rad2deg;
504  q2_2 = atan(vect_2to5[2] / vect_2to5[3]) * rad2deg;
505  q2 = q2_1  + q2_2;
506
507  // Berechne q5
508
509  q5s = alpha;
510  q5 = q5s + q1 + 140;
511}
512
513void  moveJ(float alpha, float targetPoint[])
514{
515  inverseKinematic(alpha, targetPoint);
516  Serial.println(q5);
517  bool i = false;
518  while (i == false)
519  {
520    i  = axis1(q1, speedA1);
521    i &= axis2(q2, speedA2);
522    i &= axis3(q3, q2,  speedA3);
523    i &= axis5(q5, 0);
524  }
525}
526
527void moveP(float alpha, float  targetPoint[])
528{
529  inverseKinematic(0.0, targetPoint);
530  bool i = false;
531  while (i == false)
532  {
533    i = axis1(q1, speedA1);
534    i &= axis5(q5,  0);
535  }
536
537  i = false;
538  while (i == false)
539  {
540    i = axis2(q2,  speedA2);
541    i &= axis3(q3, q2, speedA3);
542  }
543  delay(250);
544}
545
546/*void  moveP2P(float startingPoint[], float targetPoint[], float resolution)
547  {
548  // berechne Fahrtvektor
549  float vect_s2t[4];
550  vect_s2t[0] = targetPoint[0]  - startingPoint[0];
551  vect_s2t[1] = targetPoint[1] - startingPoint[1];
552  vect_s2t[2]  = targetPoint[2] - startingPoint[2];
553  vect_s2t[3] = sqrt(pow(vect_s2t[0], 2)  + pow(vect_s2t[1], 2) + pow(vect_s2t[2], 2));
554
555  // Fahre zum Startpunkt (sofern  noch nicht geschehen)
556  moveJ(startingPoint);
557
558  float vect_nxt[3];
559  for (float i = 0; i < (vect_s2t[3] - resolution); i += resolution)
560    {
561    vect_nxt[0] = startingPoint[0] + (i / vect_s2t[3]) * vect_s2t[0];
562    vect_nxt[1]  = startingPoint[1] + (i / vect_s2t[3]) * vect_s2t[1];
563    vect_nxt[2] = startingPoint[2]  + (i / vect_s2t[3]) * vect_s2t[2];
564    Serial.println(vect_nxt[0]);
565    Serial.println(vect_nxt[1]);
566    Serial.println(vect_nxt[2]);
567    moveJ(alpha, vect_nxt, l_tool);
568    }
569  moveJ(targetPoint);
570  }*/
571
572// Sonstige Unterprogramme
573
574void fraese()
575{
576  for (int i = 50; i < 255; i++)
577  {
578    millingMachine.on(i);
579    delay(15);
580  }
581  while (!posMillingX(34, 255)) {}
582  while (!axis5(q5 - 25, 35)) {}
583  delay(500);
584  while (!axis5(q5 + 25, 35)) {}
585  delay(500);
586  while (!axis5(q5,  35)) {}
587  while (!posMillingX(0, 255)) {}
588  for (int i = 255; i > 0; i--)
589  {
590    millingMachine.on(i);
591    delay(5);
592  }
593  millingMachine.off();
594
595}
596void  referenzfahrt(int befehl)
597{
598  i2c_befehl = 0;
599  i2c_status = 0;
600  Serial.println("Referenzfahrt:  Programm gestartet");
601  millingMachine.off();
602  laser.off();
603  vakuum.off();
604  referencePoint();
605  while (!posMillingX(0, 255)) {}
606  Serial.println("Referenzfahrt:  Programm beendet");
607  i2c_status = befehl;
608}
609
610void vorbereitung(int  befehl)
611{
612  i2c_befehl = 0;
613  Serial.println("Vorbereitung: Programm gestartet");
614
615  // Aufnahme
616  moveJ(90.0, posLib[0]);
617  moveJ(90.0, posLib[1]);
618  delay(1000);
619  vakuum.on();
620  delay(2000);
621  moveJ(90.0, posLib[2]);
622
623  // Laser
624  moveJ(0.0, posLib[3]);
625  laser.on();
626  delay(500);
627  while (!axis5(q5  - 30, 20)) {}
628  delay(500);
629  while (!axis5(q5 + 30, 20)) {}
630  delay(500);
631  laser.off();
632  while (!axis5(q5, 20)) {}
633  delay(1000);
634
635  // Fraese
636  moveJ(00.0, posLib[5]);
637  delay(500);
638  fraese();
639  delay(500);
640
641  // Referenzpunkt
642  referencePoint();
643  Serial.println("Vorbereitung: Programm  beendet");
644  i2c_status = befehl;
645}
646
647void montage(int befehl)
648{
649  i2c_befehl = 0;
650  Serial.println("Montage: Programm gestartet");
651  moveJ(0.0,  posLib[6]);
652  delay(500);
653  moveJ(-2.0, posLib[7]);
654  delay(1000);
655  vakuum.off();
656  delay(1000);
657  referencePoint();
658  Serial.println("Montage:  Programm beendet");
659  i2c_status = befehl;
660}
661/************************************************************************
662  Hauptprogramm
663 ***********************************************************************/
664
665void  setup()
666{
667  // Serial
668  Serial.begin(9600);
669
670  // I2C setup:
671  Wire.begin(0x44);             // join i2c bus with address 0x33
672  Wire.onReceive(receiveEvent);  // Reaktion auf Befehl
673  Wire.onRequest(requestEvent); // Reaktion auf Anfrage
674
675  // Motorinitialisierung
676  motorAxis1.setMotor(M8[0], M8[1], M8[2]);
677  motorAxis2.setMotor(M6[0],  M6[1], M6[2]);
678  motorAxis3.setMotor(M5[0], M5[1], M5[2]);
679  vakuum.setMotor(M4[0],  M4[1], M4[2]);
680
681  millingAxisX.setMotor(M1[0], M1[1], M1[2]);
682  millingMachine.setMotor(M2[0],  M2[1], M2[2]);
683  laser.setMotor(M7[0], M7[1], M7[2]);
684
685  servoAxis5.attach(D12);
686
687  pinMode(limitSwitch1, INPUT_PULLUP);
688  pinMode(limitSwitch2, INPUT_PULLUP);
689  pinMode(limitSwitch3, INPUT_PULLUP);
690  pinMode(limitSwitchMillingX, INPUT_PULLUP);
691
692  pinMode(counter1Pin, INPUT_PULLUP);
693  pinMode(counter2Pin, INPUT_PULLUP);
694  pinMode(counter3Pin, INPUT_PULLUP);
695  pinMode(counterMillingXPin, INPUT_PULLUP);
696
697  attachInterrupt(digitalPinToInterrupt(counter1Pin), counterA1, CHANGE);
698  attachInterrupt(digitalPinToInterrupt(counter2Pin),  counterA2, CHANGE);
699  attachInterrupt(digitalPinToInterrupt(counter3Pin), counterA3,  CHANGE);
700  attachInterrupt(digitalPinToInterrupt(counterMillingXPin), counterMX,  CHANGE);
701}
702
703void loop()
704{ 
705   /*referenzfahrt(1);
706    vorbereitung(1);
707    montage(1);*/
708  
709  switch (i2c_befehl)
710  {
711    case 20:
712      referenzfahrt(i2c_befehl);
713      break;
714    case 21:
715      vorbereitung(i2c_befehl);
716      break;
717    case 22:
718      montage(i2c_befehl);
719      break;
720    default:
721      break;
722  }
723}
724
725/* I2C
726   function that executes whenever data is received from  master
727   this function is registered as an event
728*/
729void receiveEvent(int  howMany) {
730  while (1 < Wire.available()) { // loop through all but the last
731    char c = Wire.read(); // receive byte as a character
732  }
733  int x = Wire.read();    // receive byte as an integer
734  //Serial.println(x);
735  i2c_befehl = x;
736}
737
738void  requestEvent() {
739  Wire.write(i2c_status);
740}
741