Sternenlabor/EggDuino
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update to Verisonn 1.2

Browse Source 
complete rework of Move-Algorithm, no position errors any more, complete
in integer math //it was the hell to debug it... ;-)
optimized timing of Ack-Answer
added command QP "Query Pen", untested, answer might be wrong 0 <--> 1?
optimized Init-String for EBBv13
optimized some variable types to enhance memory performance
changed enablePenMotor to 6 by default to match it to Spherebot
Electronics
changed default maxSpeeds from 1000 to 2000;
This commit is contained in:
cocktailyogi
2014-04-25 17:38:49 +02:00
parent 7852a828b9
commit 411a57627d
4 changed files with 137 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
EggDuino.ino
View File

@@ -6,34 +6,35 @@ Thanks to my wife and my daughter for their patience. :-)
*/ */
// implemented Eggbot-Protocol-Version 2.1.0 // implemented Eggbot-Protocol-Version v13
// EBB-Command-Referenc, I sourced from: http://www.schmalzhaus.com/EBB/EBBCommands.html // EBB-Command-Reference, I sourced from: http://www.schmalzhaus.com/EBB/EBBCommands.html
// no homing sequence, switch-on position of pen will be taken as reference point. // no homing sequence, switch-on position of pen will be taken as reference point.
// No collision-detection!! // No collision-detection!!
// Supported Servos: I do not know, I use Arduino Servo Lib with TG9e- standard servo. // Supported Servos: I do not know, I use Arduino Servo Lib with TG9e- standard servo.
// Note: Maximum-Speed in Inkscape is 1000 Steps/s. You could enter more, but then Pythonscript sends nonsense.
// 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: /* TODOs:
1 collision control via penMin/penMax 1 collision control via penMin/penMax
2 implement homing sequence via microswitch or optical device 2 implement homing sequence via microswitch or optical device
3 implement hardware-button , EGGBOT-Guys call it "PRG-Button" 3 implement hardware-button , EGGBOT-Guys call it "PRG-Button"
4 Replace Stepper Control ACCELSTEPPER --> DDS, interruptbased control to improve step-precision and smoothness.
*/ */
#include "AccelStepper.h"
#include "AccelStepper.h" // nice lib from http://www.airspayce.com/mikem/arduino/AccelStepper/ //#include "libs\AccelStepper.h" // nice lib from http://www.airspayce.com/mikem/arduino/AccelStepper/
#include <Servo.h> #include <Servo.h>
#include "SerialCommand.h" //nice lib from Stefan Rado, https://github.com/kroimon/Arduino-SerialCommand #include "SerialCommand.h" //nice lib from Stefan Rado, https://github.com/kroimon/Arduino-SerialCommand
#define initSting "EBB 2.1.0+ Protocol emulated by Eggduino-Firmware V1.1" #define initSting "EBBv13_and_above Protocol emulated by Eggduino-Firmware V1.2"
//Rotational Stepper //Rotational Stepper
#define step1 11 #define step1 11
#define dir1 10 #define dir1 10
#define enableRotMotor 9 #define enableRotMotor 9
#define rotMicrostep 8 #define rotMicrostep 8 //only 1,2,4,8,16 allowed, because of Integer-Math in this Sketch
//Pen Stepper //Pen Stepper
#define step2 8 #define step2 8
#define dir2 7 #define dir2 7
#define enablePenMotor 5 #define enablePenMotor 6
#define penMicrostep 8 #define penMicrostep 8 //only 1,2,4,8,16 allowed, because of Integer-Math in this Sketch
//Servo //Servo
#define servoPin 3 #define servoPin 3
@@ -42,19 +43,26 @@ Thanks to my wife and my daughter for their patience. :-)
AccelStepper penMotor(1, step2, dir2); AccelStepper penMotor(1, step2, dir2);
Servo penServo; Servo penServo;
SerialCommand SCmd; SerialCommand SCmd;
// Variables
// Variables... be careful, by messing around here, evrything has a reason and crossrelations...
int penMin=0; int penMin=0;
int penMax=0; int penMax=0;
int penUpPos=5; //can be overwritten from EBB-Command SC int penUpPos=5; //can be overwritten from EBB-Command SC
int penDownPos=30; //can be overwritten from EBB-Command SC int penDownPos=30; //can be overwritten from EBB-Command SC
int servoRateUp=0; //from EBB-Protocol not implemented on machine-side int servoRateUp=0; //from EBB-Protocol not implemented on machine-side
int servoRateDown=0;//from EBB-Protocol not implemented on machine-side int servoRateDown=0;//from EBB-Protocol not implemented on machine-side
float rotStepMode=16; //1/16 by default, can be changed by EBB protocol, used as correction factor uint8_t rotStepMode=16; //1/16 by default, can be changed by EBB protocol, used as correction factor
float penStepMode=16; //1/16 by default, can be changed by EBB protocol, used as correction factor uint8_t penStepMode=16; //1/16 by default, can be changed by EBB protocol, used as correction factor
long rotStepError=0;
long penStepError=0;
int penState=penUpPos; int penState=penUpPos;
uint32_t nodeCount=0; uint32_t nodeCount=0;
unsigned int layer=0; unsigned int layer=0;
boolean prgButtonState=0; int prgButtonState=0;
uint8_t rotStepCorrection = rotStepMode/rotMicrostep ; //devide EBB-Coordinates by this factor to get EGGduino-Steps
uint8_t penStepCorrection = penStepMode/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
void setup() { void setup() {
Serial.begin(9600); Serial.begin(9600);

149
Functions.ino
View File

@@ -1,6 +1,6 @@
void makeComInterface(){ void makeComInterface(){
SCmd.addCommand("v",sendVersion); // Converts two arguments to integers and echos them back SCmd.addCommand("v",sendVersion);
SCmd.addCommand("EM",enableMotors); SCmd.addCommand("EM",enableMotors);
SCmd.addCommand("SC",stepperModeConfigure); SCmd.addCommand("SC",stepperModeConfigure);
SCmd.addCommand("SP",setPen); SCmd.addCommand("SP",setPen);
@@ -14,16 +14,29 @@ void makeComInterface(){
SCmd.addCommand("QN",queryNodeCount); SCmd.addCommand("QN",queryNodeCount);
SCmd.addCommand("SL",setLayer); SCmd.addCommand("SL",setLayer);
SCmd.addCommand("QL",queryLayer); SCmd.addCommand("QL",queryLayer);
SCmd.addCommand("QP",queryPen);
SCmd.addCommand("QB",queryButton); //preparation for "PRG" Button, actually gives fake answer, SCmd.addCommand("QB",queryButton); //preparation for "PRG" Button, actually gives fake answer,
SCmd.setDefaultHandler(unrecognized); // Handler for command that isn't matched (says "What?") SCmd.setDefaultHandler(unrecognized); // Handler for command that isn't matched (says "What?")
} }
void queryPen() {
char state;
if (penState=penUpPos)
state='1';
else
state='0';
Serial.print(String(state)+"\r\n");
sendAck();
}
void queryButton() { void queryButton() {
Serial.print(String(prgButtonState) +"\r\n" + "OK\r\n"); Serial.print(String(prgButtonState) +"\r\n");
sendAck();
} }
void queryLayer() { void queryLayer() {
Serial.print(String(layer) +"\r\n" + "OK\r\n"); Serial.print(String(layer) +"\r\n");
sendAck();
} }
void setLayer() { void setLayer() {
@@ -33,14 +46,16 @@ void setLayer() {
if (arg1 != NULL) { if (arg1 != NULL) {
value = atoi(arg1); value = atoi(arg1);
layer=value; layer=value;
Serial.print("OK\r\n"); sendAck();
} }
else else
Serial.print("unknown CMD\r\n"); sendError();
} }
void queryNodeCount() { void queryNodeCount() {
Serial.print(String(nodeCount) +"\r\n" + "OK\r\n"); Serial.print(String(nodeCount) +"\r\n");
sendAck();
} }
void setNodeCount() { void setNodeCount() {
@@ -50,26 +65,26 @@ void setNodeCount() {
if (arg1 != NULL) { if (arg1 != NULL) {
value = atoi(arg1); value = atoi(arg1);
nodeCount=value; nodeCount=value;
Serial.print("OK\r\n"); sendAck();
} }
else else
Serial.print("unknown CMD\r\n"); sendError();
} }
void nodeCountIncrement() { void nodeCountIncrement() {
nodeCount=nodeCount++; nodeCount=nodeCount++;
Serial.print("OK\r\n"); sendAck();
} }
void nodeCountDecrement() { void nodeCountDecrement() {
nodeCount=nodeCount--; nodeCount=nodeCount--;
Serial.print("OK\r\n"); sendAck();
} }
void stepperMove(){ void stepperMove(){
uint16_t duration=0; //in ms uint16_t duration=0; //in ms
long penStepsEBB=0; //Pen int penStepsEBB=0; //Pen
long rotStepsEBB=0; //Rot int rotStepsEBB=0; //Rot
char *arg1; char *arg1;
char *arg2; char *arg2;
char *arg3; char *arg3;
@@ -84,32 +99,47 @@ void stepperMove(){
} }
if (arg3 != NULL) { if (arg3 != NULL) {
rotStepsEBB = atoi(arg3); rotStepsEBB = atoi(arg3);
Serial.print("OK\r\n"); //sendAck();
if ( (penStepsEBB==0) && (rotStepsEBB==0) ) if ( (penStepsEBB==0) && (rotStepsEBB==0) ) {
delay(duration); delay(duration);
sendAck();
}
if ( (penStepsEBB!=0) || (rotStepsEBB!=0) ) { if ( (penStepsEBB!=0) || (rotStepsEBB!=0) ) {
//Move-Code: //################### Move-Code Start ############################################################
//Turn on Motors, if they are off....
digitalWrite(enableRotMotor, LOW) ; digitalWrite(enableRotMotor, LOW) ;
digitalWrite(enablePenMotor, LOW) ; digitalWrite(enablePenMotor, LOW) ;
long rotSteps = round( ((float) (rotMicrostep/rotStepMode)) * rotStepsEBB ); //transform to local coordiantes //incoming EBB-Steps will be multiplied by 16, then Integer-maths is done, result will be divided by 16
long penSteps = round( ((float) (penMicrostep/penStepMode)) * penStepsEBB ); //transform to local coordiantes // This make thinks here really complicated, but floating point-math kills performance and memory, believe me... I tried...
float rotSpeed = (float) ( ((long) rotSteps * (long)1000) / (long) duration ) ; long rotSteps = ( (long)rotStepsEBB * 16 / rotStepCorrection) + (long)rotStepError; //correct incoming EBB-Steps to our microstep-Setting and multiply by 16 to avoid floatingpoint...
float penSpeed = (float) ( ((long) penSteps * (long)1000) / (long) duration ) ; long penSteps = ( (long)penStepsEBB * 16 / penStepCorrection) + (long)penStepError;
rotMotor.move(rotSteps); int rotStepsToGo = (int) (rotSteps/16); //Calc Steps to go, which are possible on our machine
rotMotor.setSpeed(rotSpeed); int penStepsToGo = (int) (penSteps/16);
penMotor.move(penSteps); 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 ) ;
if (temp_rotSpeed <0 ) //remove sign, there is no negative Speed....
temp_rotSpeed= -temp_rotSpeed;
if (temp_penSpeed <0 ) //remove sign, there is no negative Speed....
temp_penSpeed= -temp_penSpeed;
float rotSpeed= (float) temp_rotSpeed; // type cast
float penSpeed= (float) temp_penSpeed;
rotMotor.move(rotStepsToGo); // finally, let us set the target position...
rotMotor.setSpeed(rotSpeed); // and the Speed!
penMotor.move(penStepsToGo);
penMotor.setSpeed( penSpeed ); penMotor.setSpeed( penSpeed );
while ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) { while ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) {
penMotor.runSpeedToPosition(); penMotor.runSpeedToPosition(); // Moving.... moving... moving....
rotMotor.runSpeedToPosition(); rotMotor.runSpeedToPosition();
} }
//end Move-Code //################### Move-Code End ############################################################
sendAck(); //Mission completed
} }
} }
else else
Serial.print("unknown CMD\r\n"); sendError();
} }
@@ -125,18 +155,16 @@ void setPen(){
case 0: case 0:
penServo.write(penUpPos); penServo.write(penUpPos);
penState=penUpPos; penState=penUpPos;
Serial.print("OK\r\n");
break; break;
case 1: case 1:
penServo.write(penDownPos); penServo.write(penDownPos);
penState=penDownPos; penState=penDownPos;
//Serial.println("case 1"); //Serial.println("case 1");
Serial.print("OK\r\n");
break; break;
default: default:
Serial.print("unknown CMD"); sendError();
} }
} }
char *val; char *val;
@@ -145,12 +173,14 @@ void setPen(){
value = atoi(val); value = atoi(val);
// Serial.println("delayvalue"); // Serial.println("delayvalue");
delay(value); delay(value);
sendAck();
} }
if (val==NULL && arg !=NULL) if (val==NULL && arg !=NULL)
delay(500); delay(500);
sendAck();
// Serial.println("delay"); // Serial.println("delay");
if (val==NULL && arg ==NULL) if (val==NULL && arg ==NULL)
Serial.print("unknown CMD\r\n"); sendError();
} }
void togglePen(){ void togglePen(){
@@ -162,20 +192,21 @@ void togglePen(){
if (penState==penUpPos) { if (penState==penUpPos) {
penServo.write(penDownPos); penServo.write(penDownPos);
penState=penDownPos; penState=penDownPos;
Serial.print("OK\r\n");
if (arg != NULL) if (arg != NULL)
delay(value); delay(value);
else else
delay(500); delay(500);
sendAck();
} }
else { else {
penServo.write(penUpPos); penServo.write(penUpPos);
penState=penUpPos; penState=penUpPos;
Serial.print("OK\r\n");
if (arg != NULL) if (arg != NULL)
delay(value); delay(value);
else else
delay(500); delay(500);
sendAck();
} }
} }
@@ -196,40 +227,48 @@ void enableMotors(){
switch (cmd) { switch (cmd) {
case 0: digitalWrite(enableRotMotor, HIGH) ; case 0: digitalWrite(enableRotMotor, HIGH) ;
digitalWrite(enablePenMotor, HIGH) ; digitalWrite(enablePenMotor, HIGH) ;
Serial.print("OK\r\n"); sendAck();
break; break;
case 1: rotStepMode=16; case 1: rotStepMode=16;
penStepMode=16; penStepMode=16;
digitalWrite(enableRotMotor, LOW) ; digitalWrite(enableRotMotor, LOW) ;
digitalWrite(enablePenMotor, LOW) ; digitalWrite(enablePenMotor, LOW) ;
Serial.print("OK\r\n"); sendAck();
break; break;
case 2: rotStepMode=8; case 2: rotStepMode=8;
penStepMode=8; penStepMode=8;
rotStepCorrection = rotStepMode/rotMicrostep;
penStepCorrection = penStepMode/penMicrostep;
digitalWrite(enableRotMotor, LOW) ; digitalWrite(enableRotMotor, LOW) ;
digitalWrite(enablePenMotor, LOW) ; digitalWrite(enablePenMotor, LOW) ;
Serial.print("OK\r\n"); sendAck();
break; break;
case 3: rotStepMode=4; case 3: rotStepMode=4;
penStepMode=4; penStepMode=4;
rotStepCorrection = rotStepMode/rotMicrostep;
penStepCorrection = penStepMode/penMicrostep;
digitalWrite(enableRotMotor, LOW) ; digitalWrite(enableRotMotor, LOW) ;
digitalWrite(enablePenMotor, LOW) ; digitalWrite(enablePenMotor, LOW) ;
Serial.print("OK\r\n"); sendAck();
break; break;
case 4: rotStepMode=2; case 4: rotStepMode=2;
penStepMode=2; penStepMode=2;
rotStepCorrection = rotStepMode/rotMicrostep;
penStepCorrection = penStepMode/penMicrostep;
digitalWrite(enableRotMotor, LOW) ; digitalWrite(enableRotMotor, LOW) ;
digitalWrite(enablePenMotor, LOW) ; digitalWrite(enablePenMotor, LOW) ;
Serial.print("OK\r\n"); sendAck();
break; break;
case 5: rotStepMode=1; case 5: rotStepMode=1;
penStepMode=1; penStepMode=1;
rotStepCorrection = rotStepMode/rotMicrostep;
penStepCorrection = penStepMode/penMicrostep;
digitalWrite(enableRotMotor, LOW) ; digitalWrite(enableRotMotor, LOW) ;
digitalWrite(enablePenMotor, LOW) ; digitalWrite(enablePenMotor, LOW) ;
Serial.print("OK\r\n"); sendAck();
break; break;
default: default:
Serial.print("unknown CMD\r\n"); sendError();
} }
} }
//the following implementaion is a little bit cheated, because i did not know, how to implement different values for first and second argument. //the following implementaion is a little bit cheated, because i did not know, how to implement different values for first and second argument.
@@ -237,41 +276,41 @@ void enableMotors(){
switch (value) { switch (value) {
case 0: digitalWrite(enableRotMotor, HIGH) ; case 0: digitalWrite(enableRotMotor, HIGH) ;
digitalWrite(enablePenMotor, HIGH) ; digitalWrite(enablePenMotor, HIGH) ;
Serial.print("OK\r\n"); sendAck();
break; break;
case 1: rotStepMode=16; case 1: rotStepMode=16;
penStepMode=16; penStepMode=16;
digitalWrite(enableRotMotor, LOW) ; digitalWrite(enableRotMotor, LOW) ;
digitalWrite(enablePenMotor, LOW) ; digitalWrite(enablePenMotor, LOW) ;
Serial.print("OK\r\n"); sendAck();
break; break;
case 2: rotStepMode=8; case 2: rotStepMode=8;
penStepMode=8; penStepMode=8;
digitalWrite(enableRotMotor, LOW) ; digitalWrite(enableRotMotor, LOW) ;
digitalWrite(enablePenMotor, LOW) ; digitalWrite(enablePenMotor, LOW) ;
Serial.print("OK\r\n"); sendAck();
break; break;
case 3: rotStepMode=4; case 3: rotStepMode=4;
penStepMode=4; penStepMode=4;
digitalWrite(enableRotMotor, LOW) ; digitalWrite(enableRotMotor, LOW) ;
digitalWrite(enablePenMotor, LOW) ; digitalWrite(enablePenMotor, LOW) ;
Serial.print("OK\r\n"); sendAck();
break; break;
case 4: rotStepMode=2; case 4: rotStepMode=2;
penStepMode=2; penStepMode=2;
digitalWrite(enableRotMotor, LOW) ; digitalWrite(enableRotMotor, LOW) ;
digitalWrite(enablePenMotor, LOW) ; digitalWrite(enablePenMotor, LOW) ;
Serial.print("OK\r\n"); sendAck();
break; break;
case 5: rotStepMode=1; case 5: rotStepMode=1;
penStepMode=1; penStepMode=1;
digitalWrite(enableRotMotor, LOW) ; digitalWrite(enableRotMotor, LOW) ;
digitalWrite(enablePenMotor, LOW) ; digitalWrite(enablePenMotor, LOW) ;
Serial.print("OK\r\n"); sendAck();
break; break;
default: default:
Serial.print("unknown CMD\r\n"); sendError();
} }
} }
} }
@@ -291,25 +330,25 @@ void stepperModeConfigure(){
if ((arg != NULL) && (val != NULL)){ if ((arg != NULL) && (val != NULL)){
switch (cmd) { switch (cmd) {
case 4: penDownPos= (int) ((float) (value-6000)/(float) 94.18); // transformation from EBB to PWM-Stepper case 4: penDownPos= (int) ((float) (value-6000)/(float) 94.18); // transformation from EBB to PWM-Stepper
Serial.print("OK\r\n"); sendAck();
break; break;
case 5: penUpPos= (int)((float) (value-6000)/(float) 94.18); // transformation from EBB to PWM-Stepper case 5: penUpPos= (int)((float) (value-6000)/(float) 94.18); // transformation from EBB to PWM-Stepper
Serial.print("OK\r\n"); sendAck();
break; break;
case 6: //rotMin=value; ignored case 6: //rotMin=value; ignored
Serial.print("OK\r\n"); sendAck();
break; break;
case 7: //rotMax=value; ignored case 7: //rotMax=value; ignored
Serial.print("OK\r\n"); sendAck();
break; break;
case 11: servoRateUp=value; case 11: servoRateUp=value;
Serial.print("OK\r\n"); sendAck();
break; break;
case 12: servoRateDown=value; case 12: servoRateDown=value;
Serial.print("OK\r\n"); sendAck();
break; break;
default: default:
Serial.print("unknown CMD\r\n"); sendError();
} }
} }
} }
@@ -320,9 +359,9 @@ void sendVersion(){
} }
void unrecognized(const char *command){ void unrecognized(const char *command){
Serial.print("unknown CMD\r\n"); sendError();
} }
void ignore(){ void ignore(){
Serial.print("OK\r\n"); sendAck();
} }

12
Helper_Functions.ino
View File

@@ -1,12 +1,20 @@
void initHardware(){ void initHardware(){
pinMode(enableRotMotor, OUTPUT); pinMode(enableRotMotor, OUTPUT);
pinMode(enablePenMotor, OUTPUT); pinMode(enablePenMotor, OUTPUT);
rotMotor.setMaxSpeed(1000.0); rotMotor.setMaxSpeed(2000.0);
rotMotor.setAcceleration(10000.0); rotMotor.setAcceleration(10000.0);
penMotor.setMaxSpeed(1000.0); penMotor.setMaxSpeed(2000.0);
penMotor.setAcceleration(10000.0); penMotor.setAcceleration(10000.0);
digitalWrite(enableRotMotor, HIGH) ; digitalWrite(enableRotMotor, HIGH) ;
digitalWrite(enablePenMotor, HIGH) ; digitalWrite(enablePenMotor, HIGH) ;
penServo.attach(servoPin); penServo.attach(servoPin);
penServo.write(penUpPos); penServo.write(penUpPos);
} }
void inline sendAck(){
Serial.print("OK\r\n");
}
void inline sendError(){
Serial.print("unknown CMD\r\n");
}

9
VersionHistory.txt
View File

@@ -1,3 +1,12 @@
23.04.2014 v1.2
complete rework of Move-Algorithm, no position errors any more, complete in integer math //it was the hell to debug it... ;-)
optimized timing of Ack-Answer
added command QP "Query Pen", untested, answer might be wrong 0 <--> 1?
optimized Init-String for EBBv13
optimized some variable types to enhance memory performance
changed enablePenMotor to 6 by default to match it to Spherebot Electronics
changed default maxSpeeds from 1000 to 2000;
22.04.2014 V1.1 22.04.2014 V1.1
bugfix Command SM - fixed rounding error in pen and rot-axis bugfix Command SM - fixed rounding error in pen and rot-axis