Motors moving
This commit is contained in:
Holger Weber
190
src/Helper_Functions.cpp
Normal file
190
src/Helper_Functions.cpp
Normal file
@@ -0,0 +1,190 @@
|
||||
#include "EggDuino.h"
|
||||
|
||||
void initHardware()
|
||||
{
|
||||
if (!initConfigStore())
|
||||
{
|
||||
penUpPos = 5;
|
||||
penDownPos = 20;
|
||||
}
|
||||
penState = penUpPos;
|
||||
|
||||
g_stepEngine.init();
|
||||
g_pStepperRotate = g_stepEngine.stepperConnectToPin(step1);
|
||||
if (g_pStepperRotate)
|
||||
{
|
||||
// rotMotor.setMaxSpeed(2000.0);
|
||||
// rotMotor.setAcceleration(10000.0);
|
||||
|
||||
g_pStepperRotate->setDirectionPin(dir1);
|
||||
g_pStepperRotate->setEnablePin(enableRotMotor);
|
||||
g_pStepperRotate->setAcceleration(10000);
|
||||
g_pStepperRotate->setAutoEnable(false);
|
||||
}
|
||||
|
||||
// Stepper pen init
|
||||
g_pStepperPen = g_stepEngine.stepperConnectToPin(step2);
|
||||
if (g_pStepperPen)
|
||||
{
|
||||
// penMotor.setMaxSpeed(2000.0);
|
||||
// penMotor.setAcceleration(10000.0);
|
||||
g_pStepperPen->setDirectionPin(dir2);
|
||||
g_pStepperPen->setEnablePin(enablePenMotor);
|
||||
g_pStepperPen->setAcceleration(10000);
|
||||
g_pStepperPen->setAutoEnable(false);
|
||||
}
|
||||
|
||||
motorsOff();
|
||||
penServo.attach(servoPin);
|
||||
penServo.write(penState);
|
||||
}
|
||||
|
||||
void storePenUpPosInEE()
|
||||
{
|
||||
saveConfigToFile();
|
||||
}
|
||||
|
||||
void storePenDownPosInEE()
|
||||
{
|
||||
saveConfigToFile();
|
||||
}
|
||||
|
||||
void sendAck()
|
||||
{
|
||||
Serial.print("OK\r\n");
|
||||
}
|
||||
|
||||
void sendError()
|
||||
{
|
||||
Serial.print("unknown CMD\r\n");
|
||||
}
|
||||
|
||||
void motorsOff()
|
||||
{
|
||||
// digitalWrite(enableRotMotor, HIGH);
|
||||
// digitalWrite(enablePenMotor, HIGH);
|
||||
g_pStepperPen->disableOutputs();
|
||||
g_pStepperRotate->disableOutputs();
|
||||
motorsEnabled = 0;
|
||||
}
|
||||
|
||||
void motorsOn()
|
||||
{
|
||||
// digitalWrite(enableRotMotor, LOW) ;
|
||||
// digitalWrite(enablePenMotor, LOW) ;
|
||||
g_pStepperPen->enableOutputs();
|
||||
g_pStepperRotate->enableOutputs();
|
||||
motorsEnabled = 1;
|
||||
}
|
||||
|
||||
void toggleMotors()
|
||||
{
|
||||
if (motorsEnabled)
|
||||
{
|
||||
motorsOff();
|
||||
}
|
||||
else
|
||||
{
|
||||
motorsOn();
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void prepareMove(uint16_t duration, int penStepsEBB, int rotStepsEBB)
|
||||
{
|
||||
if (!motorsEnabled)
|
||||
{
|
||||
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));
|
||||
g_pStepperRotate->move(rotStepsEBB);
|
||||
g_pStepperRotate->setSpeedInTicks(abs((float)rotStepsEBB * (float)1000 / (float)duration));
|
||||
|
||||
// penMotor.move(penStepsEBB);
|
||||
// penMotor.setSpeed(abs((float)penStepsEBB * (float)1000 / (float)duration));
|
||||
g_pStepperPen->move(penStepsEBB);
|
||||
g_pStepperRotate->setSpeedInTicks(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!
|
||||
g_pStepperRotate->move(rotStepsToGo);
|
||||
g_pStepperRotate->setSpeedInTicks(rotSpeed);
|
||||
|
||||
// penMotor.move(penStepsToGo);
|
||||
// penMotor.setSpeed(penSpeed);
|
||||
g_pStepperPen->move(penStepsToGo);
|
||||
g_pStepperRotate->setSpeedInTicks(penSpeed);
|
||||
}
|
||||
}
|
||||
|
||||
void moveOneStep()
|
||||
{
|
||||
// if (penMotor.distanceToGo() || rotMotor.distanceToGo())
|
||||
// {
|
||||
// penMotor.runSpeedToPosition(); // Moving.... moving... moving....
|
||||
// rotMotor.runSpeedToPosition();
|
||||
// }
|
||||
}
|
||||
|
||||
void moveToDestination()
|
||||
{
|
||||
// while (penMotor.distanceToGo() || rotMotor.distanceToGo())
|
||||
// {
|
||||
// penMotor.runSpeedToPosition(); // Moving.... moving... moving....
|
||||
// rotMotor.runSpeedToPosition();
|
||||
// }
|
||||
}
|
||||
|
||||
void setprgButtonState()
|
||||
{
|
||||
prgButtonState = 1;
|
||||
}
|
||||
@@ -1,132 +0,0 @@
|
||||
#include "EggDuino.h"
|
||||
|
||||
void initHardware(){
|
||||
if (!initConfigStore()) {
|
||||
penUpPos = 5;
|
||||
penDownPos = 20;
|
||||
}
|
||||
penState = penUpPos;
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
void storePenUpPosInEE() {
|
||||
saveConfigToFile();
|
||||
}
|
||||
|
||||
void storePenDownPosInEE() {
|
||||
saveConfigToFile();
|
||||
}
|
||||
|
||||
void sendAck(){
|
||||
Serial.print("OK\r\n");
|
||||
}
|
||||
|
||||
void sendError(){
|
||||
Serial.print("unknown CMD\r\n");
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void prepareMove(uint16_t duration, int penStepsEBB, int rotStepsEBB) {
|
||||
if (!motorsEnabled) {
|
||||
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 );
|
||||
}
|
||||
}
|
||||
|
||||
void moveOneStep() {
|
||||
if ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) {
|
||||
penMotor.runSpeedToPosition(); // Moving.... moving... moving....
|
||||
rotMotor.runSpeedToPosition();
|
||||
}
|
||||
}
|
||||
|
||||
void moveToDestination() {
|
||||
while ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) {
|
||||
penMotor.runSpeedToPosition(); // Moving.... moving... moving....
|
||||
rotMotor.runSpeedToPosition();
|
||||
}
|
||||
}
|
||||
|
||||
void setprgButtonState(){
|
||||
prgButtonState = 1;
|
||||
}
|
||||
129
src/main.cpp
129
src/main.cpp
@@ -29,51 +29,114 @@
|
||||
|
||||
//-----------------------------------------------------------------------------------------------------------
|
||||
|
||||
//make Objects
|
||||
// AccelStepper rotMotor(AccelStepper::DRIVER, step1, dir1);
|
||||
// AccelStepper penMotor(AccelStepper::DRIVER, step2, dir2);
|
||||
// Servo penServo;
|
||||
// SerialCommand SCmd;
|
||||
//create Buttons
|
||||
FastAccelStepperEngine g_stepEngine = FastAccelStepperEngine();
|
||||
FastAccelStepper *g_pStepperRotate = NULL;
|
||||
FastAccelStepper *g_pStepperPen = NULL;
|
||||
|
||||
// make Objects
|
||||
// FastAccelStepper rotMotor(FastAccelStepper::DRIVER, step1, dir1);
|
||||
// FastAccelStepper penMotor(FastAccelStepper::DRIVER, step2, dir2);
|
||||
Servo penServo;
|
||||
SerialCommand SCmd;
|
||||
|
||||
// create Buttons
|
||||
#ifdef prgButton
|
||||
Button prgButtonToggle(prgButton, setprgButtonState);
|
||||
Button prgButtonToggle(prgButton, setprgButtonState);
|
||||
#endif
|
||||
#ifdef penToggleButton
|
||||
Button penToggle(penToggleButton, doTogglePen);
|
||||
Button penToggle(penToggleButton, doTogglePen);
|
||||
#endif
|
||||
#ifdef motorsButton
|
||||
Button motorsToggle(motorsButton, toggleMotors);
|
||||
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
|
||||
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(115200);
|
||||
Serial.print("Starting...");
|
||||
//makeComInterface();
|
||||
//initHardware();
|
||||
// Stepper Test
|
||||
// #define dirPinStepper 16
|
||||
// #define enablePinStepper 12
|
||||
// #define stepPinStepper 26
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(9600);
|
||||
Serial.println("Starting...");
|
||||
makeComInterface();
|
||||
initHardware();
|
||||
startWebInterface();
|
||||
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// moveOneStep();
|
||||
// SCmd.readSerial();
|
||||
uint8_t g_uiState = 0;
|
||||
unsigned long g_uiLastTim = millis();
|
||||
|
||||
void loop()
|
||||
{
|
||||
|
||||
#ifdef TEST
|
||||
unsigned long uiNow = millis();
|
||||
motorsOn();
|
||||
|
||||
if (uiNow - g_uiLastTim > 500)
|
||||
{
|
||||
g_uiLastTim = uiNow;
|
||||
switch (g_uiState)
|
||||
{
|
||||
case 0:
|
||||
Serial.println(g_uiState);
|
||||
g_pStepperRotate->setSpeedInUs(10); // the parameter is us/step !!!
|
||||
g_pStepperRotate->setAcceleration(10000);
|
||||
g_pStepperRotate->move(1000);
|
||||
g_uiState++;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
Serial.println(g_uiState);
|
||||
g_pStepperRotate->setSpeedInUs(10); // the parameter is us/step !!!
|
||||
g_pStepperRotate->setAcceleration(10000);
|
||||
g_pStepperRotate->move(-1000);
|
||||
g_uiState++;
|
||||
break;
|
||||
|
||||
case 2:
|
||||
Serial.println(g_uiState);
|
||||
g_pStepperPen->setSpeedInUs(10); // the parameter is us/step !!!
|
||||
g_pStepperPen->setAcceleration(10000);
|
||||
g_pStepperPen->move(1000);
|
||||
g_uiState++;
|
||||
break;
|
||||
|
||||
case 3:
|
||||
Serial.println(g_uiState);
|
||||
g_pStepperPen->setSpeedInUs(10); // the parameter is us/step !!!
|
||||
g_pStepperPen->setAcceleration(10000);
|
||||
g_pStepperPen->move(-1000);
|
||||
g_uiState = 0;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
//moveOneStep();
|
||||
SCmd.readSerial();
|
||||
handleWebInterface();
|
||||
|
||||
#ifdef penToggleButton
|
||||
|
||||
Reference in New Issue
Block a user