Major update
imported some improvements from user "bartebor" - fixed some timing issues - added button-support via #ifdefine - worked on penarm-shaking-bug - restructured some codesegments
This commit is contained in:
cocktailyogi
163
Functions.ino
163
Functions.ino
@@ -5,7 +5,9 @@ void makeComInterface(){
|
||||
SCmd.addCommand("SC",stepperModeConfigure);
|
||||
SCmd.addCommand("SP",setPen);
|
||||
SCmd.addCommand("SM",stepperMove);
|
||||
SCmd.addCommand("SMQB",stepperMoveQueryButton); // composite function enabling smooth movement
|
||||
#ifdef CommandSMQB
|
||||
SCmd.addCommand("SMQB",stepperMoveQueryButton); // composite function enabling smooth movement
|
||||
#endif
|
||||
SCmd.addCommand("SE",ignore);
|
||||
SCmd.addCommand("TP",togglePen);
|
||||
SCmd.addCommand("PO",ignore); //Engraver command, not implemented, gives fake answer
|
||||
@@ -32,8 +34,8 @@ void queryPen() {
|
||||
|
||||
void queryButton() {
|
||||
Serial.print(String(prgButtonState) +"\r\n");
|
||||
prgButtonState = 0;
|
||||
sendAck();
|
||||
prgButtonState = 0;
|
||||
}
|
||||
|
||||
void queryLayer() {
|
||||
@@ -87,109 +89,60 @@ void stepperMove() {
|
||||
uint16_t duration=0; //in ms
|
||||
int penStepsEBB=0; //Pen
|
||||
int rotStepsEBB=0; //Rot
|
||||
while ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) {
|
||||
penMotor.runSpeedToPosition(); // Moving.... moving... moving....
|
||||
rotMotor.runSpeedToPosition();
|
||||
}
|
||||
|
||||
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();
|
||||
//sendAck();
|
||||
return;
|
||||
}
|
||||
|
||||
// sending ACK before actual move to allow buffering
|
||||
queryButton();
|
||||
doMove(duration, penStepsEBB, rotStepsEBB);
|
||||
//sendAck();
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
return true;
|
||||
}
|
||||
#ifdef CommandSMQB
|
||||
void stepperMoveQueryButton() {
|
||||
uint16_t duration=0; //in ms
|
||||
int penStepsEBB=0; //Pen
|
||||
int rotStepsEBB=0; //Rot
|
||||
|
||||
return false;
|
||||
}
|
||||
if (!parseSMArgs(&duration, &penStepsEBB, &rotStepsEBB)) {
|
||||
sendError();
|
||||
return;
|
||||
}
|
||||
|
||||
void doMove(uint16_t duration, int penStepsEBB, int rotStepsEBB) {
|
||||
motorsOn();
|
||||
if ( (penStepsEBB==0) && (rotStepsEBB==0) ) {
|
||||
delay(duration);
|
||||
queryButton();
|
||||
return;
|
||||
}
|
||||
|
||||
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;
|
||||
// sending ACK before actual move to allow buffering
|
||||
queryButton();
|
||||
doMove(duration, penStepsEBB, rotStepsEBB);
|
||||
}
|
||||
#endif
|
||||
|
||||
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;
|
||||
char *arg;
|
||||
while ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) {
|
||||
penMotor.runSpeedToPosition(); // Moving.... moving... moving....
|
||||
rotMotor.runSpeedToPosition();
|
||||
}
|
||||
arg = SCmd.next();
|
||||
if (arg != NULL) {
|
||||
cmd = atoi(arg);
|
||||
@@ -202,7 +155,6 @@ void setPen(){
|
||||
case 1:
|
||||
penServo.write(penDownPos);
|
||||
penState=penDownPos;
|
||||
//Serial.println("case 1");
|
||||
break;
|
||||
|
||||
default:
|
||||
@@ -212,14 +164,14 @@ void setPen(){
|
||||
char *val;
|
||||
val = SCmd.next();
|
||||
if (val != NULL) {
|
||||
value = atoi(val);
|
||||
// Serial.println("delayvalue");
|
||||
delay(value);
|
||||
sendAck();
|
||||
}
|
||||
if (val==NULL && arg !=NULL)
|
||||
delay(500);
|
||||
sendAck();
|
||||
value = atoi(val);
|
||||
sendAck();
|
||||
delay(value);
|
||||
}
|
||||
if (val==NULL && arg !=NULL) {
|
||||
sendAck();
|
||||
delay(500);
|
||||
}
|
||||
// Serial.println("delay");
|
||||
if (val==NULL && arg ==NULL)
|
||||
sendError();
|
||||
@@ -228,16 +180,19 @@ void setPen(){
|
||||
void togglePen(){
|
||||
int value;
|
||||
char *arg;
|
||||
while ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) {
|
||||
penMotor.runSpeedToPosition(); // Moving.... moving... moving....
|
||||
rotMotor.runSpeedToPosition();
|
||||
}
|
||||
arg = SCmd.next();
|
||||
if (arg != NULL)
|
||||
value = atoi(arg);
|
||||
else
|
||||
value = 500;
|
||||
|
||||
doTogglePen();
|
||||
|
||||
delay(value);
|
||||
sendAck();
|
||||
doTogglePen();
|
||||
sendAck();
|
||||
delay(value);
|
||||
}
|
||||
|
||||
void doTogglePen() {
|
||||
@@ -289,26 +244,6 @@ void enableMotors(){
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
Reference in New Issue
Block a user