diff --git a/EggDuino.ino b/EggDuino.ino index ac22aa3..1152949 100644 --- a/EggDuino.ino +++ b/EggDuino.ino @@ -1,10 +1,10 @@ /* Eggduino-Firmware by Joachim Cerny, 2014 -Thanks for the nice libs ACCELSTEPPER and SERIALCOMMAND, which made this project much easier. -Thanks to the Eggbot-Team for such a funny and enjoable concept! -Thanks to my wife and my daughter for their patience. :-) + Thanks for the nice libs ACCELSTEPPER and SERIALCOMMAND, which made this project much easier. + Thanks to the Eggbot-Team for such a funny and enjoable concept! + Thanks to my wife and my daughter for their patience. :-) -*/ + */ // implemented Eggbot-Protocol-Version v13 // EBB-Command-Reference, I sourced from: http://www.schmalzhaus.com/EBB/EBBCommands.html @@ -15,9 +15,9 @@ Thanks to my wife and my daughter for their patience. :-) // EBB-Coordinates are coming in for 16th-Microstepmode. The Coordinate-Transforms are done in weired integer-math. Be careful, when you diecide to modify settings. /* TODOs: -1 collision control via penMin/penMax -2 implement homing sequence via microswitch or optical device -*/ + 1 collision control via penMin/penMax + 2 implement homing sequence via microswitch or optical device + */ #include "AccelStepper.h" // nice lib from http://www.airspayce.com/mikem/arduino/AccelStepper/ #include @@ -27,18 +27,18 @@ Thanks to my wife and my daughter for their patience. :-) #define initSting "EBBv13_and_above Protocol emulated by Eggduino-Firmware V1.6" //Rotational Stepper: - #define step1 11 - #define dir1 10 - #define enableRotMotor 9 - #define rotMicrostep 16 //MicrostepMode, only 1,2,4,8,16 allowed, because of Integer-Math in this Sketch +#define step1 11 +#define dir1 10 +#define enableRotMotor 9 +#define rotMicrostep 16 //MicrostepMode, only 1,2,4,8,16 allowed, because of Integer-Math in this Sketch //Pen Stepper: - #define step2 8 - #define dir2 7 - #define enablePenMotor 6 - #define penMicrostep 16 //MicrostepMode, only 1,2,4,8,16 allowed, because of Integer-Math in this Sketch +#define step2 8 +#define dir2 7 +#define enablePenMotor 6 +#define penMicrostep 16 //MicrostepMode, only 1,2,4,8,16 allowed, because of Integer-Math in this Sketch + +#define servoPin 3 //Servo - #define servoPin 3 //Servo - // EXTRAFEATURES - UNCOMMENT TO USE THEM ------------------------------------------------------------------- // #define prgButton 2 // PRG button @@ -47,63 +47,63 @@ Thanks to my wife and my daughter for their patience. :-) //----------------------------------------------------------------------------------------------------------- - #define penUpPosEEAddress ((uint16_t *)0) - #define penDownPosEEAddress ((uint16_t *)2) +#define penUpPosEEAddress ((uint16_t *)0) +#define penDownPosEEAddress ((uint16_t *)2) //make Objects - AccelStepper rotMotor(1, step1, dir1); - AccelStepper penMotor(1, step2, dir2); - Servo penServo; - SerialCommand SCmd; - //create Buttons - #ifdef prgButton - Button prgButtonToggle(prgButton, setprgButtonState); - #endif - #ifdef penToggleButton - Button penToggle(penToggleButton, doTogglePen); - #endif - #ifdef motorsButton - Button motorsToggle(motorsButton, toggleMotors); - #endif +AccelStepper rotMotor(1, step1, dir1); +AccelStepper penMotor(1, step2, dir2); +Servo penServo; +SerialCommand SCmd; +//create Buttons +#ifdef prgButton + Button prgButtonToggle(prgButton, setprgButtonState); +#endif +#ifdef penToggleButton + Button penToggle(penToggleButton, doTogglePen); +#endif +#ifdef motorsButton + Button motorsToggle(motorsButton, toggleMotors); +#endif // Variables... be careful, by messing around here, everything has a reason and crossrelations... - int penMin=0; - int penMax=0; - int penUpPos=5; //can be overwritten from EBB-Command SC - int penDownPos=20; //can be overwritten from EBB-Command SC - int servoRateUp=0; //from EBB-Protocol not implemented on machine-side - int servoRateDown=0;//from EBB-Protocol not implemented on machine-side - long rotStepError=0; - long penStepError=0; - int penState=penUpPos; - uint32_t nodeCount=0; - unsigned int layer=0; - boolean prgButtonState=0; - uint8_t rotStepCorrection = 16/rotMicrostep ; //devide EBB-Coordinates by this factor to get EGGduino-Steps - uint8_t penStepCorrection = 16/penMicrostep ; //devide EBB-Coordinates by this factor to get EGGduino-Steps - float rotSpeed=0; - float penSpeed=0; // these are local variables for Function SteppermotorMove-Command, but for performance-reasons it will be initialized here - boolean motorsEnabled = 0; - +int penMin=0; +int penMax=0; +int penUpPos=5; //can be overwritten from EBB-Command SC +int penDownPos=20; //can be overwritten from EBB-Command SC +int servoRateUp=0; //from EBB-Protocol not implemented on machine-side +int servoRateDown=0; //from EBB-Protocol not implemented on machine-side +long rotStepError=0; +long penStepError=0; +int penState=penUpPos; +uint32_t nodeCount=0; +unsigned int layer=0; +boolean prgButtonState=0; +uint8_t rotStepCorrection = 16/rotMicrostep ; //devide EBB-Coordinates by this factor to get EGGduino-Steps +uint8_t penStepCorrection = 16/penMicrostep ; //devide EBB-Coordinates by this factor to get EGGduino-Steps +float rotSpeed=0; +float penSpeed=0; // these are local variables for Function SteppermotorMove-Command, but for performance-reasons it will be initialized here +boolean motorsEnabled = 0; + void setup() { - Serial.begin(9600); - makeComInterface(); - initHardware(); + Serial.begin(9600); + makeComInterface(); + initHardware(); } void loop() { moveOneStep(); - + SCmd.readSerial(); - - #ifdef penToggleButton - penToggle.check(); - #endif - - #ifdef motorsButton - motorsToggle.check(); - #endif - - #ifdef prgButton - prgButtonToggle.check(); - #endif + +#ifdef penToggleButton + penToggle.check(); +#endif + +#ifdef motorsButton + motorsToggle.check(); +#endif + +#ifdef prgButton + prgButtonToggle.check(); +#endif } diff --git a/Functions.ino b/Functions.ino index f20c8aa..0c107c8 100644 --- a/Functions.ino +++ b/Functions.ino @@ -1,22 +1,22 @@ void makeComInterface(){ - SCmd.addCommand("v",sendVersion); - SCmd.addCommand("EM",enableMotors); - SCmd.addCommand("SC",stepperModeConfigure); - SCmd.addCommand("SP",setPen); - SCmd.addCommand("SM",stepperMove); - SCmd.addCommand("SE",ignore); - SCmd.addCommand("TP",togglePen); - SCmd.addCommand("PO",ignore); //Engraver command, not implemented, gives fake answer - SCmd.addCommand("NI",nodeCountIncrement); - SCmd.addCommand("ND",nodeCountDecrement); - SCmd.addCommand("SN",setNodeCount); - SCmd.addCommand("QN",queryNodeCount); - SCmd.addCommand("SL",setLayer); - SCmd.addCommand("QL",queryLayer); - SCmd.addCommand("QP",queryPen); - SCmd.addCommand("QB",queryButton); //"PRG" Button, - SCmd.setDefaultHandler(unrecognized); // Handler for command that isn't matched (says "What?") + SCmd.addCommand("v",sendVersion); + SCmd.addCommand("EM",enableMotors); + SCmd.addCommand("SC",stepperModeConfigure); + SCmd.addCommand("SP",setPen); + SCmd.addCommand("SM",stepperMove); + SCmd.addCommand("SE",ignore); + SCmd.addCommand("TP",togglePen); + SCmd.addCommand("PO",ignore); //Engraver command, not implemented, gives fake answer + SCmd.addCommand("NI",nodeCountIncrement); + SCmd.addCommand("ND",nodeCountDecrement); + SCmd.addCommand("SN",setNodeCount); + SCmd.addCommand("QN",queryNodeCount); + SCmd.addCommand("SL",setLayer); + SCmd.addCommand("QL",queryLayer); + SCmd.addCommand("QP",queryPen); + SCmd.addCommand("QB",queryButton); //"PRG" Button, + SCmd.setDefaultHandler(unrecognized); // Handler for command that isn't matched (says "What?") } void queryPen() { @@ -34,29 +34,29 @@ void queryButton() { sendAck(); prgButtonState = 0; } - + void queryLayer() { - Serial.print(String(layer) +"\r\n"); - sendAck(); -} - -void setLayer() { - uint32_t value=0; - char *arg1; - arg1 = SCmd.next(); - if (arg1 != NULL) { - value = atoi(arg1); - layer=value; - sendAck(); - } - else - sendError(); - } - -void queryNodeCount() { - Serial.print(String(nodeCount) +"\r\n"); + Serial.print(String(layer) +"\r\n"); sendAck(); - +} + +void setLayer() { + uint32_t value=0; + char *arg1; + arg1 = SCmd.next(); + if (arg1 != NULL) { + value = atoi(arg1); + layer=value; + sendAck(); + } + else + sendError(); +} + +void queryNodeCount() { + Serial.print(String(nodeCount) +"\r\n"); + sendAck(); + } void setNodeCount() { @@ -83,182 +83,182 @@ void nodeCountDecrement() { } void stepperMove() { - uint16_t duration=0; //in ms - int penStepsEBB=0; //Pen - int rotStepsEBB=0; //Rot + uint16_t duration=0; //in ms + int penStepsEBB=0; //Pen + int rotStepsEBB=0; //Rot - moveToDestination(); + moveToDestination(); - if (!parseSMArgs(&duration, &penStepsEBB, &rotStepsEBB)) { - sendError(); - return; - } + if (!parseSMArgs(&duration, &penStepsEBB, &rotStepsEBB)) { + sendError(); + return; + } - sendAck(); - - if ( (penStepsEBB==0) && (rotStepsEBB==0) ) { - delay(duration); - return; - } + sendAck(); - prepareMove(duration, penStepsEBB, rotStepsEBB); + if ( (penStepsEBB==0) && (rotStepsEBB==0) ) { + delay(duration); + return; + } + + prepareMove(duration, penStepsEBB, rotStepsEBB); } void setPen(){ - int cmd; - int value; - char *arg; + int cmd; + int value; + char *arg; - moveToDestination(); + moveToDestination(); - arg = SCmd.next(); - if (arg != NULL) { - cmd = atoi(arg); - switch (cmd) { - case 0: - penServo.write(penUpPos); - penState=penUpPos; - break; - - case 1: - penServo.write(penDownPos); - penState=penDownPos; - break; - - default: - sendError(); - } - } - char *val; - val = SCmd.next(); - if (val != NULL) { + arg = SCmd.next(); + if (arg != NULL) { + cmd = atoi(arg); + switch (cmd) { + case 0: + penServo.write(penUpPos); + penState=penUpPos; + break; + + case 1: + penServo.write(penDownPos); + penState=penDownPos; + break; + + default: + sendError(); + } + } + char *val; + val = SCmd.next(); + if (val != NULL) { value = atoi(val); sendAck(); - delay(value); - } - if (val==NULL && arg !=NULL) { + delay(value); + } + if (val==NULL && arg !=NULL) { sendAck(); delay(500); - } - // Serial.println("delay"); - if (val==NULL && arg ==NULL) - sendError(); + } + // Serial.println("delay"); + if (val==NULL && arg ==NULL) + sendError(); } void togglePen(){ - int value; - char *arg; + int value; + char *arg; - moveToDestination(); + moveToDestination(); - arg = SCmd.next(); - if (arg != NULL) - value = atoi(arg); - else - value = 500; + arg = SCmd.next(); + if (arg != NULL) + value = atoi(arg); + else + value = 500; doTogglePen(); sendAck(); - delay(value); + delay(value); } void doTogglePen() { - if (penState==penUpPos) { - penServo.write(penDownPos); - penState=penDownPos; - } else { - penServo.write(penUpPos); - penState=penUpPos; - } + if (penState==penUpPos) { + penServo.write(penDownPos); + penState=penDownPos; + } else { + penServo.write(penUpPos); + penState=penUpPos; + } } void enableMotors(){ - int cmd; - int value; - char *arg; - char *val; - arg = SCmd.next(); - if (arg != NULL) - cmd = atoi(arg); - val = SCmd.next(); - if (val != NULL) - value = atoi(val); - //values parsed - if ((arg != NULL) && (val == NULL)){ - switch (cmd) { - case 0: motorsOff(); - sendAck(); - break; - case 1: motorsOn(); - sendAck(); - break; - default: - sendError(); - } -} -//the following implementaion is a little bit cheated, because i did not know, how to implement different values for first and second argument. - if ((arg != NULL) && (val != NULL)){ - switch (value) { - case 0: motorsOff(); - sendAck(); - break; - case 1: motorsOn(); - sendAck(); - break; - default: - sendError(); - } - } + int cmd; + int value; + char *arg; + char *val; + arg = SCmd.next(); + if (arg != NULL) + cmd = atoi(arg); + val = SCmd.next(); + if (val != NULL) + value = atoi(val); + //values parsed + if ((arg != NULL) && (val == NULL)){ + switch (cmd) { + case 0: motorsOff(); + sendAck(); + break; + case 1: motorsOn(); + sendAck(); + break; + default: + sendError(); + } + } + //the following implementaion is a little bit cheated, because i did not know, how to implement different values for first and second argument. + if ((arg != NULL) && (val != NULL)){ + switch (value) { + case 0: motorsOff(); + sendAck(); + break; + case 1: motorsOn(); + sendAck(); + break; + default: + sendError(); + } + } } void stepperModeConfigure(){ - int cmd; - int value; - char *arg; - arg = SCmd.next(); - if (arg != NULL) - cmd = atoi(arg); - char *val; - val = SCmd.next(); - if (val != NULL) - value = atoi(val); - if ((arg != NULL) && (val != NULL)){ - switch (cmd) { - case 4: penDownPos= (int) ((float) (value-6000)/(float) 133.3); // transformation from EBB to PWM-Servo - storePenDownPosInEE(); - sendAck(); - break; - case 5: penUpPos= (int)((float) (value-6000)/(float) 133.3); // transformation from EBB to PWM-Servo - storePenUpPosInEE(); - sendAck(); - break; - case 6: //rotMin=value; ignored - sendAck(); - break; - case 7: //rotMax=value; ignored - sendAck(); - break; - case 11: servoRateUp=value; - sendAck(); - break; - case 12: servoRateDown=value; - sendAck(); - break; - default: - sendError(); - } - } + int cmd; + int value; + char *arg; + arg = SCmd.next(); + if (arg != NULL) + cmd = atoi(arg); + char *val; + val = SCmd.next(); + if (val != NULL) + value = atoi(val); + if ((arg != NULL) && (val != NULL)){ + switch (cmd) { + case 4: penDownPos= (int) ((float) (value-6000)/(float) 133.3); // transformation from EBB to PWM-Servo + storePenDownPosInEE(); + sendAck(); + break; + case 5: penUpPos= (int)((float) (value-6000)/(float) 133.3); // transformation from EBB to PWM-Servo + storePenUpPosInEE(); + sendAck(); + break; + case 6: //rotMin=value; ignored + sendAck(); + break; + case 7: //rotMax=value; ignored + sendAck(); + break; + case 11: servoRateUp=value; + sendAck(); + break; + case 12: servoRateDown=value; + sendAck(); + break; + default: + sendError(); + } + } } void sendVersion(){ - Serial.print(initSting); - Serial.print("\r\n"); + Serial.print(initSting); + Serial.print("\r\n"); } - + void unrecognized(const char *command){ - sendError(); + sendError(); } - + void ignore(){ - sendAck(); + sendAck(); } diff --git a/Helper_Functions.ino b/Helper_Functions.ino index 7344f87..a43f8ea 100644 --- a/Helper_Functions.ino +++ b/Helper_Functions.ino @@ -1,33 +1,33 @@ void initHardware(){ - // enable eeprom wait in avr/eeprom.h functions - SPMCSR &= ~SELFPRGEN; + // enable eeprom wait in avr/eeprom.h functions + SPMCSR &= ~SELFPRGEN; - loadPenPosFromEE(); + loadPenPosFromEE(); - pinMode(enableRotMotor, OUTPUT); - pinMode(enablePenMotor, OUTPUT); + pinMode(enableRotMotor, OUTPUT); + pinMode(enablePenMotor, OUTPUT); - rotMotor.setMaxSpeed(2000.0); - rotMotor.setAcceleration(10000.0); - penMotor.setMaxSpeed(2000.0); - penMotor.setAcceleration(10000.0); - motorsOff(); - penServo.attach(servoPin); - penServo.write(penState); + rotMotor.setMaxSpeed(2000.0); + rotMotor.setAcceleration(10000.0); + penMotor.setMaxSpeed(2000.0); + penMotor.setAcceleration(10000.0); + motorsOff(); + penServo.attach(servoPin); + penServo.write(penState); } void inline loadPenPosFromEE() { - penUpPos = eeprom_read_word(penUpPosEEAddress); - penDownPos = eeprom_read_word(penDownPosEEAddress); - penState = penUpPos; + penUpPos = eeprom_read_word(penUpPosEEAddress); + penDownPos = eeprom_read_word(penDownPosEEAddress); + penState = penUpPos; } void inline storePenUpPosInEE() { - eeprom_update_word(penUpPosEEAddress, penUpPos); + eeprom_update_word(penUpPosEEAddress, penUpPos); } void inline storePenDownPosInEE() { - eeprom_update_word(penDownPosEEAddress, penDownPos); + eeprom_update_word(penDownPosEEAddress, penDownPos); } void inline sendAck(){ @@ -53,7 +53,7 @@ void motorsOn() { void toggleMotors() { if (motorsEnabled) { motorsOff(); - } else { + } else { motorsOn(); } } @@ -73,7 +73,7 @@ bool parseSMArgs(uint16_t *duration, int *penStepsEBB, int *rotStepsEBB) { } if (arg3 != NULL) { *rotStepsEBB = atoi(arg3); - + return true; } @@ -90,7 +90,7 @@ void prepareMove(uint16_t duration, int penStepsEBB, int rotStepsEBB) { rotMotor.setSpeed( abs( (float)rotStepsEBB * (float)1000 / (float)duration ) ); penMotor.move(penStepsEBB); penMotor.setSpeed( abs( (float)penStepsEBB * (float)1000 / (float)duration ) ); - } else { + } else { //incoming EBB-Steps will be multiplied by 16, then Integer-maths is done, result will be divided by 16 // This make thinks here really complicated, but floating point-math kills performance and memory, believe me... I tried... long rotSteps = ( (long)rotStepsEBB * 16 / rotStepCorrection) + (long)rotStepError; //correct incoming EBB-Steps to our microstep-Setting and multiply by 16 to avoid floatingpoint...