// ArduinoCNC // Styroporschneidemaschine über serielle Schnittstelle gesteuert // Version: 1.3 // Datum: 5-Jan-12 // Versionskommentar: Standardfunktion für Datenempfang eingeführt // ----------------------------------------------------------------------------------------- // Hinweis: Arduino IDE muss auf Board "Diecimila" eingestellt sein // ----------------------------------------------------------------------------------------- // Befehle, die über die Serielle Schnittstelle empfangen werden: // '5' - Links Y-Achse nach oben // '6' - Links Y-Achse nach unten // '7' - Links X-Achse nach vorne // '8' - Links X-Achse nach hinten // '1' - Rechts Y-Achse nach oben // '2' - Rechts Y-Achse nach unten // '3' - Rechts X-Achse nach vorne // '4' - Rechts X-Achse nach hinten // '0' - Stoppt die Bewegung aller Achsen // 'S' - // "" - bewegt beide Seiten um Schritte // = 'U' Aufwärts // = 'D' Abwärts // = 'F' Vorwärts // = 'B' Rückwärts // Beispiel: F00F3 // "YaspectHex>" // - Übergibt einen Vector an den Arduino. Der Arduino weiß, ob für die rechte // oder linke Seite, weil er den Vector vorher entsprechend angefragt hat. // Die drei Vorzeichen geben die Richtung an. // Die drei Werte sind unsigned Hex (4 Characters). // StepHex - Anzahl der Schritte in X-Richtung // XAspectHex - Alle wieviel Clocks wird ein X-Motorschritt gemacht. // YAspectHex - Alle wieviel Clocks wird ein Y-Motorschritt gemacht. // Beispiel: +024E+0041-003D // "#----#----#----" - Keine weiteren Vectoren mehr d.h. Arduino hört auf danach zu fragen. // // Befehle, die gesendet werden: // 'L' - Arduino fordert nächsten Vector für linke Seite an // 'R' - Arduino fordert nächsten Vector für rechte Seite an // 'A' - Rechter Endschalter in X-Richtung erreicht // 'B' - Linker Endschalter in X-Richtung erreicht // 'a' - Rechter Endschalter in Y-Richtung erreicht // 'b' - Linker Endschalter in Y-Richtung erreicht int xrDir=0, xlDir=0, yrDir=0, ylDir=0; boolean bitXR0, bitXR1, bitYR0, bitYR1; boolean bitXL0, bitXL1, bitYL0, bitYL1; int inByte='0'; int heatSetting=0; boolean cuttingInProgress = false; int aspXL, aspXR, aspYL, aspYR; // Anzahl der Clockticks, die bis zum nächsten Schritt jeweils übersprungen werden int countLeft=0, countRight=0; // Anzahl der X-Schritte auf der jeweiligen Seite des aktuellen Vektors unsigned long masterClock; unsigned long clockXL, clockXR, clockYL, clockYR; // Schrittzähler für die jeweiligen Achsen int xrSign, xlSign, yrSign, ylSign; // Richtungen (werden von requestVector erzeugt) boolean validVectorL, validVectorR; // Erkennung des Endes der Vector-Daten // ============= Initialisierung ================================== void setup() { // Output-Pins initialisieren // X-Links pinMode(8, OUTPUT); pinMode(9, OUTPUT); // Y-Links pinMode(10, OUTPUT); pinMode(11, OUTPUT); // X-Rechts pinMode(A0, OUTPUT); pinMode(A1, OUTPUT); // Y-Rechts pinMode(A2, OUTPUT); pinMode(A3, OUTPUT); // Heizung für den Draht (PWM Output) pinMode(3, OUTPUT); analogWrite(3,0); // Heizung aus //Initialisiere digitale Inputs für Endschalter pinMode(12, INPUT); // X-Rechts pinMode(13, INPUT); // Y-Rechts pinMode(A4, INPUT); // X-Links pinMode(A5, INPUT); // Y-Links // serielle Schnittstelle initalisieren Serial.begin(9600); } // ====================== Hauptschleife ========================== void loop() { if (cuttingInProgress) { // Maschine im Schneidemodus - Vektoren werden angefordert if (countLeft==0) validVectorL = requestVector('L'); if (countRight==0) validVectorR = requestVector('R'); if (validVectorL || validVectorR) { // Erst wenn beide Seiten keine validen Vectoren mehr haben, wird beendet xlDir=0; xrDir=0; ylDir=0; yrDir=0; if (masterClock>clockXL) { xlDir=xlSign; clockXL+=aspXL; // das ist die zentrale Stelle, die sicherstellt, dass alle Durchläufe // ein Schritt ausgeführt wird. Da hier die Richtung keine Rolle spielt, // wird einfach immer weitergezählt. countLeft--; // Es werden nur die Schritte in X-Richtung heruntergezählt, da sich die in Y-Richtung daraus ergeben. } if (masterClock>clockXR) { // analog für die anderen Achsen xrDir=xrSign; clockXR+=aspXR; countRight--; } if (masterClock>clockYL) { ylDir=ylSign; clockYL+=aspYL; } if (masterClock>clockYR) { yrDir=yrSign; clockYR+=aspYR; } oneStep(xrDir, yrDir, xlDir, ylDir); masterClock++; delayMicroseconds(500); } else { cuttingInProgress=false; // wenn keine gültiger Vector mehr vorhanden, in manuelle Arbeitsweise umschalten xlDir=0; xrDir=0; ylDir=0; yrDir=0; delay(1500); analogWrite(3,0); // Heizung ausschalten } } else { // Maschine im manuellen Modus - Tasteneingaben abfragen. if (Serial.available() > 0) { inByte = Serial.read(); // Byte lesen if (inByte=='1') yrDir=-1; else if (inByte=='2') yrDir=1; else if (inByte=='3') xrDir=-1; else if (inByte=='4') xrDir=1; else if (inByte=='5') ylDir=-1; else if (inByte=='6') ylDir=1; else if (inByte=='7') xlDir=-1; else if (inByte=='8') xlDir=1; else if (inByte=='0') {yrDir=0; xrDir=0; xlDir=0; ylDir=0; analogWrite(3,0);} if (inByte=='H') readSetHeat(); if ((inByte=='D')||(inByte=='U')||(inByte=='B')||(inByte=='F')) readMove(inByte); if (inByte=='S') { cuttingInProgress = true; // schalte die Maschine in den Schneidemodus validVectorL = true; validVectorR = true; countLeft=0; // stellt sicher, dass beim Umschalten in den Schneidemodus alle Zähler auf Null stehen countRight=0; // und der erste Vector angefordert wird clockXL=0; clockXR=0; clockYL=0; clockYR=0; masterClock=0; } } oneStep(xrDir, yrDir, xlDir, ylDir); if (digitalRead(12)==LOW && xrDir==1) {xrDir=0; Serial.print('A'); delay(20);} if (digitalRead(13)==LOW && yrDir==1) {yrDir=0; Serial.print('a'); delay(20);} if (digitalRead(A4)==LOW && xlDir==1) {xlDir=0; Serial.print('B'); delay(20);} if (digitalRead(A5)==LOW && ylDir==1) {ylDir=0; Serial.print('b'); delay(20);} delay(10); } } // Wert für den eingestellten Heizstrom lesen und PWM entsprechend programmieren // Format auf der seriellen Schnittstelle: Hxx xx = 2-Byte Hex-Zahl void readSetHeat() { int hexNum; heatSetting = readNumSerial(2); // 2 Byte lesen analogWrite(3,heatSetting); } // Rechte und linke Seite um Schritte verfahren // dir: U = Up, D = Down, B = Back, F = Forward void readMove(char dir) { int i, steps, hexNum, digit; steps = readNumSerial(4); for (i=0; i='A') outByte=hexChar-'A'+10; else outByte=hexChar-'0'; return(outByte); } // Führt einen Schritt auf einer oder mehreren Achsen aus // -1 rückwärts // 1 vorwärts // 0 kein Schritt void oneStep(int xr, int yr, int xl, int yl) { boolean b0xr, b0yr, b0xl, b0yl; b0xr=bitXR0; // alten Zustand zwischenspeichern b0yr=bitYR0; b0xl=bitXL0; b0yl=bitYL0; if (xr==1) {bitXR0 = !bitXR1; bitXR1 = b0xr;}; if (xr==-1) {bitXR0 = bitXR1; bitXR1 = !b0xr;}; if (xl==1) {bitXL0 = !bitXL1; bitXL1 = b0xl;}; if (xl==-1) {bitXL0 = bitXL1; bitXL1 = !b0xl;}; if (yr==1) {bitYR0 = !bitYR1; bitYR1 = b0yr;}; if (yr==-1) {bitYR0 = bitYR1; bitYR1 = !b0yr;}; if (yl==1) {bitYL0 = !bitYL1; bitYL1 = b0yl;}; if (yl==-1) {bitYL0 = bitYL1; bitYL1 = !b0yl;}; digitalWrite(8, bitXL1); digitalWrite(9, bitXL0); digitalWrite(10, bitYL0); // Hier sind die Bits auf der Platine vertauscht digitalWrite(11, bitYL1); digitalWrite(A0, bitXR1); digitalWrite(A1, bitXR0); digitalWrite(A2, bitYR1); digitalWrite(A3, bitYR0); }