Components and supplies
1
Arduino Micro
1
Arduino Mega 2560
Apps and platforms
1
Visual Studio 2015
1
Arduino IDE
Project description
Code
Montage.ino
arduino
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
Downloadable files
Arduino Mega Shield
Arduino Mega Shield
Arduino Mega Shield
Arduino Mega Shield
Comments
Only logged in users can leave comments