From 0d7fe3e922344862260f60fc593d9a370994a475 Mon Sep 17 00:00:00 2001 From: Bartosz Borkowski Date: Thu, 30 Apr 2015 23:43:14 +0200 Subject: [PATCH] Adding few improvements: - motors toggle button: allows for manual adjustments and silences motors - pen toggle button: pen up and down positions are stored in EEPROM - SMQB command: allows faster and smoother operation with custom version of eggbot extension (bartebor/eggbot_extensions) - PRG button fix --- EggDuino.ino | 61 +++++++++++- Functions.ino | 220 ++++++++++++++++++++++++++----------------- Helper_Functions.ino | 31 +++++- 3 files changed, 217 insertions(+), 95 deletions(-) diff --git a/EggDuino.ino b/EggDuino.ino index 08b5630..e14a218 100644 --- a/EggDuino.ino +++ b/EggDuino.ino @@ -17,12 +17,12 @@ Thanks to my wife and my daughter for their patience. :-) /* TODOs: 1 collision control via penMin/penMax 2 implement homing sequence via microswitch or optical device -3 implement hardware-button , EGGBOT-Guys call it "PRG-Button" */ #include "AccelStepper.h" //#include "libs\AccelStepper.h" // nice lib from http://www.airspayce.com/mikem/arduino/AccelStepper/ #include #include "SerialCommand.h" //nice lib from Stefan Rado, https://github.com/kroimon/Arduino-SerialCommand +#include #define initSting "EBBv13_and_above Protocol emulated by Eggduino-Firmware V1.4" //Rotational Stepper @@ -37,7 +37,17 @@ Thanks to my wife and my daughter for their patience. :-) #define penMicrostep 16 //MicrostepMode, only 1,2,4,8,16 allowed, because of Integer-Math in this Sketch //Servo #define servoPin 3 - #define PrgButton 2 //optional Pushbutton between Pin and Ground to use certain functions with Eggbot-Inkscape extension +// PRG button + #define prgButton 2 +// pen up/down button + #define penToggleButton 12 +// motors enable button + #define motorsButton 4 + + #define penUpPosEEAddress ((uint16_t *)0) + #define penDownPosEEAddress ((uint16_t *)2) + + #define debounceDelay 50 //make Objects AccelStepper rotMotor(1, step1, dir1); @@ -62,6 +72,45 @@ Thanks to my wife and my daughter for their patience. :-) 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 + int motorsEnabled = 0; + + typedef void (*ActionCb)(void); + + class Button { + public: + Button(byte p, ActionCb a): debounce(0), state(1), lastState(1), action(a), pin(p) {}; + + void check() { + byte b = digitalRead(pin); + long t = millis(); + + if (b != lastState) { + debounce = t; + } + + if ((t - debounce) > debounceDelay) { + if (b != state) { + state = b; + + if (!state) { + (*action)(); + } + } + } + + lastState = b; + } + + private: + long debounce; + byte state:1; + byte lastState:1; + byte pin; + ActionCb action; + }; + + Button penToggle(penToggleButton, doTogglePen); + Button motorsToggle(motorsButton, toggleMotors); void setup() { Serial.begin(9600); @@ -70,7 +119,9 @@ void setup() { } void loop() { - SCmd.readSerial(); - if( 0 == digitalRead(PrgButton)) - prgButtonState == 1; + SCmd.readSerial(); + penToggle.check(); + motorsToggle.check(); + if( 0 == digitalRead(prgButton)) + prgButtonState = 1; } diff --git a/Functions.ino b/Functions.ino index d76c669..16817af 100644 --- a/Functions.ino +++ b/Functions.ino @@ -5,6 +5,7 @@ void makeComInterface(){ SCmd.addCommand("SC",stepperModeConfigure); SCmd.addCommand("SP",setPen); SCmd.addCommand("SM",stepperMove); + SCmd.addCommand("SMQB",stepperMoveQueryButton); // composite function enabling smooth movement SCmd.addCommand("SE",ignore); SCmd.addCommand("TP",togglePen); SCmd.addCommand("PO",ignore); //Engraver command, not implemented, gives fake answer @@ -38,7 +39,7 @@ void queryButton() { void queryLayer() { Serial.print(String(layer) +"\r\n"); sendAck(); - } +} void setLayer() { uint32_t value=0; @@ -82,76 +83,109 @@ void nodeCountDecrement() { sendAck(); } -void stepperMove(){ +void stepperMove() { uint16_t duration=0; //in ms int penStepsEBB=0; //Pen int rotStepsEBB=0; //Rot + + if (!parseSMArgs(&duration, &penStepsEBB, &rotStepsEBB)) { + sendError(); + return; + } + + if ( (penStepsEBB==0) && (rotStepsEBB==0) ) { + delay(duration); + sendAck(); + return; + } + + doMove(duration, penStepsEBB, rotStepsEBB); + sendAck(); +} + +void stepperMoveQueryButton() { + uint16_t duration=0; //in ms + int penStepsEBB=0; //Pen + int rotStepsEBB=0; //Rot + + if (!parseSMArgs(&duration, &penStepsEBB, &rotStepsEBB)) { + sendError(); + return; + } + + if ( (penStepsEBB==0) && (rotStepsEBB==0) ) { + delay(duration); + queryButton(); + return; + } + + // sending ACK before actual move to allow buffering + queryButton(); + doMove(duration, penStepsEBB, rotStepsEBB); +} + +bool parseSMArgs(uint16_t *duration, int *penStepsEBB, int *rotStepsEBB) { char *arg1; char *arg2; char *arg3; arg1 = SCmd.next(); if (arg1 != NULL) { - duration = atoi(arg1); - arg2 = SCmd.next(); - } - if (arg2 != NULL) { - penStepsEBB = atoi(arg2); - arg3 = SCmd.next(); - } - if (arg3 != NULL) { - rotStepsEBB = atoi(arg3); - //sendAck(); + *duration = atoi(arg1); + arg2 = SCmd.next(); + } + if (arg2 != NULL) { + *penStepsEBB = atoi(arg2); + arg3 = SCmd.next(); + } + if (arg3 != NULL) { + *rotStepsEBB = atoi(arg3); - if ( (penStepsEBB==0) && (rotStepsEBB==0) ) { - delay(duration); - sendAck(); - } - if ( (penStepsEBB!=0) || (rotStepsEBB!=0) ) { -//################### Move-Code Start ############################################################ - //Turn on Motors, if they are off.... - digitalWrite(enableRotMotor, LOW) ; - digitalWrite(enablePenMotor, LOW) ; - if( (1 == rotStepCorrection) && (1 == penStepCorrection) ){ // if coordinatessystems are identical - //set Coordinates and Speed - rotMotor.move(rotStepsEBB); - rotMotor.setSpeed( abs( (float)rotStepsEBB * (float)1000 / (float)duration ) ); - penMotor.move(penStepsEBB); - penMotor.setSpeed( abs( (float)penStepsEBB * (float)1000 / (float)duration ) ); - } - 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... - long penSteps = ( (long)penStepsEBB * 16 / penStepCorrection) + (long)penStepError; - int rotStepsToGo = (int) (rotSteps/16); //Calc Steps to go, which are possible on our machine - int penStepsToGo = (int) (penSteps/16); - rotStepError = (long)rotSteps - ((long) rotStepsToGo * (long)16); // calc Position-Error, if there is one - penStepError = (long)penSteps - ((long) penStepsToGo * (long)16); - long temp_rotSpeed = ((long)rotStepsToGo * (long)1000 / (long)duration ); // calc Speed in Integer Math - long temp_penSpeed = ((long)penStepsToGo * (long)1000 / (long)duration ) ; - float rotSpeed= (float) abs(temp_rotSpeed); // type cast - float penSpeed= (float) abs(temp_penSpeed); - //set Coordinates and Speed - rotMotor.move(rotStepsToGo); // finally, let us set the target position... - rotMotor.setSpeed(rotSpeed); // and the Speed! - penMotor.move(penStepsToGo); - penMotor.setSpeed( penSpeed ); - } - //Start Move - while ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) { - penMotor.runSpeedToPosition(); // Moving.... moving... moving.... - rotMotor.runSpeedToPosition(); - } -//################### Move-Code End ############################################################ - sendAck(); //report Mission completed - } - } - else - sendError(); - + return true; + } + + return false; +} + +void doMove(uint16_t duration, int penStepsEBB, int rotStepsEBB) { + motorsOn(); + + if( (1 == rotStepCorrection) && (1 == penStepCorrection) ){ // if coordinatessystems are identical + //set Coordinates and Speed + rotMotor.move(rotStepsEBB); + rotMotor.setSpeed( abs( (float)rotStepsEBB * (float)1000 / (float)duration ) ); + penMotor.move(penStepsEBB); + penMotor.setSpeed( abs( (float)penStepsEBB * (float)1000 / (float)duration ) ); + } 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... + long penSteps = ( (long)penStepsEBB * 16 / penStepCorrection) + (long)penStepError; + + int rotStepsToGo = (int) (rotSteps/16); //Calc Steps to go, which are possible on our machine + int penStepsToGo = (int) (penSteps/16); + + rotStepError = (long)rotSteps - ((long) rotStepsToGo * (long)16); // calc Position-Error, if there is one + penStepError = (long)penSteps - ((long) penStepsToGo * (long)16); + + long temp_rotSpeed = ((long)rotStepsToGo * (long)1000 / (long)duration ); // calc Speed in Integer Math + long temp_penSpeed = ((long)penStepsToGo * (long)1000 / (long)duration ) ; + + float rotSpeed= (float) abs(temp_rotSpeed); // type cast + float penSpeed= (float) abs(temp_penSpeed); + + //set Coordinates and Speed + rotMotor.move(rotStepsToGo); // finally, let us set the target position... + rotMotor.setSpeed(rotSpeed); // and the Speed! + penMotor.move(penStepsToGo); + penMotor.setSpeed( penSpeed ); + } + + while ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) { + penMotor.runSpeedToPosition(); // Moving.... moving... moving.... + rotMotor.runSpeedToPosition(); + } } - void setPen(){ int cmd; int value; @@ -197,28 +231,25 @@ void togglePen(){ arg = SCmd.next(); if (arg != NULL) value = atoi(arg); + else + value = 500; + + doTogglePen(); + + delay(value); + sendAck(); +} + +void doTogglePen() { if (penState==penUpPos) { penServo.write(penDownPos); penState=penDownPos; - if (arg != NULL) - delay(value); - else - delay(500); - sendAck(); - } - - else { + } else { penServo.write(penUpPos); penState=penUpPos; - if (arg != NULL) - delay(value); - else - delay(500); - sendAck(); } -} - - +} + void enableMotors(){ int cmd; int value; @@ -233,12 +264,10 @@ void enableMotors(){ //values parsed if ((arg != NULL) && (val == NULL)){ switch (cmd) { - case 0: digitalWrite(enableRotMotor, HIGH) ; - digitalWrite(enablePenMotor, HIGH) ; + case 0: motorsOff(); sendAck(); break; - case 1: digitalWrite(enableRotMotor, LOW) ; - digitalWrite(enablePenMotor, LOW) ; + case 1: motorsOn(); sendAck(); break; default: @@ -248,21 +277,38 @@ void enableMotors(){ //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: digitalWrite(enableRotMotor, HIGH) ; - digitalWrite(enablePenMotor, HIGH) ; + case 0: motorsOff(); sendAck(); break; - case 1: digitalWrite(enableRotMotor, LOW) ; - digitalWrite(enablePenMotor, LOW) ; + case 1: motorsOn(); sendAck(); break; - default: sendError(); } } } - + +void motorsOff() { + digitalWrite(enableRotMotor, HIGH); + digitalWrite(enablePenMotor, HIGH); + motorsEnabled = 0; +} + +void motorsOn() { + digitalWrite(enableRotMotor, LOW) ; + digitalWrite(enablePenMotor, LOW) ; + motorsEnabled = 1; +} + +void toggleMotors() { + if (motorsEnabled) { + motorsOff(); + } else { + motorsOn(); + } +} + void stepperModeConfigure(){ int cmd; int value; @@ -277,9 +323,11 @@ void stepperModeConfigure(){ 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 diff --git a/Helper_Functions.ino b/Helper_Functions.ino index c9dca75..a8f45b4 100644 --- a/Helper_Functions.ino +++ b/Helper_Functions.ino @@ -1,16 +1,38 @@ void initHardware(){ + // enable eeprom wait in avr/eeprom.h functions + SPMCSR &= ~SELFPRGEN; + + loadPenPosFromEE(); + pinMode(enableRotMotor, OUTPUT); pinMode(enablePenMotor, OUTPUT); + pinMode(prgButton, INPUT_PULLUP); + pinMode(penToggleButton, INPUT_PULLUP); + pinMode(motorsButton, INPUT_PULLUP); + rotMotor.setMaxSpeed(2000.0); rotMotor.setAcceleration(10000.0); penMotor.setMaxSpeed(2000.0); penMotor.setAcceleration(10000.0); - digitalWrite(enableRotMotor, HIGH) ; - digitalWrite(enablePenMotor, HIGH) ; + motorsOff(); penServo.attach(servoPin); - penServo.write(penUpPos); + penServo.write(penState); } - + +void inline loadPenPosFromEE() { + penUpPos = eeprom_read_word(penUpPosEEAddress); + penDownPos = eeprom_read_word(penDownPosEEAddress); + penState = penUpPos; +} + +void inline storePenUpPosInEE() { + eeprom_update_word(penUpPosEEAddress, penUpPos); +} + +void inline storePenDownPosInEE() { + eeprom_update_word(penDownPosEEAddress, penDownPos); +} + void inline sendAck(){ Serial.print("OK\r\n"); } @@ -18,3 +40,4 @@ void inline sendAck(){ void inline sendError(){ Serial.print("unknown CMD\r\n"); } +