spacecruft
/
farmbot-arduino-firmware
1
0
Fork 0
Code Issues Pull Requests Releases Activity

added motor and calibration status

pull/35/head
TimEvWw 2016-02-06 16:23:24 -01:00
parent 0250dccb66
commit df0eb18777
5 changed files with 148 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/Config.h
View File

@ -22,12 +22,13 @@ const int LOGGING = 0;
//const bool AXIS_HOME_UP_Y = false;
//const bool AXIS_HOME_UP_Z = true;
const String COMM_REPORT_CMD_START = "R01";
const String COMM_REPORT_CMD_DONE = "R02";
const String COMM_REPORT_CMD_ERROR = "R03";
const String COMM_REPORT_CMD_BUSY = "R04";
const String COMM_REPORT_CMD_STATUS = "R05";
const String COMM_REPORT_COMMENT = "R99";
const String COMM_REPORT_CMD_START = "R01";
const String COMM_REPORT_CMD_DONE = "R02";
const String COMM_REPORT_CMD_ERROR = "R03";
const String COMM_REPORT_CMD_BUSY = "R04";
const String COMM_REPORT_CMD_STATUS = "R05";
const String COMM_REPORT_CALIB_STATUS = "R06";
const String COMM_REPORT_COMMENT = "R99";
const int COMM_REPORT_MOVE_STATUS_IDLE = 0;
const int COMM_REPORT_MOVE_STATUS_START_MOTOR = 1;

143
src/StepperControl.cpp
View File

@ -11,6 +11,53 @@ StepperControl * StepperControl::getInstance() {
}
;
void StepperControl::reportStatus(StepperControlAxis* axis, int axisStatus) {
Serial.print(COMM_REPORT_CMD_STATUS);
Serial.print(" ");
Serial.print(axis->label);
Serial.print(axisStatus);
Serial.print("\n");
}
void StepperControl::reportCalib(StepperControlAxis* axis, int calibStatus) {
Serial.print(COMM_REPORT_CALIB_STATUS);
Serial.print(" ");
Serial.print(axis->label);
Serial.print(calibStatus);
Serial.print("\n");
}
void StepperControl::checkAxisSubStatus(StepperControlAxis* axis, int* axisSubStatus) {
int newStatus = 0;
bool statusChanged = false;
if (axis->isAccelerating()) {
newStatus = COMM_REPORT_MOVE_STATUS_ACCELERATING;
}
if (axis->isCruising()) {
newStatus = COMM_REPORT_MOVE_STATUS_CRUISING;
}
if (axis->isDecelerating()) {
newStatus = COMM_REPORT_MOVE_STATUS_DECELERATING;
}
if (axis->isCrawling()) {
newStatus = COMM_REPORT_MOVE_STATUS_CRAWLING;
}
// if the status changes, send out a status report
if (*axisSubStatus != newStatus && newStatus > 0) {
statusChanged = true;
}
*axisSubStatus = newStatus;
if (statusChanged) {
reportStatus(&axisX, *axisSubStatus);
}
}
//const int MOVEMENT_INTERRUPT_SPEED = 100; // Interrupt cycle in micro seconds
StepperControl::StepperControl() {
@ -38,6 +85,10 @@ StepperControl::StepperControl() {
axisY.loadPinNumbers(Y_STEP_PIN, Y_DIR_PIN, Y_ENABLE_PIN, Y_MIN_PIN, Y_MAX_PIN);
axisZ.loadPinNumbers(Z_STEP_PIN, Z_DIR_PIN, Z_ENABLE_PIN, Z_MIN_PIN, Z_MAX_PIN);
axisSubStep[0] = COMM_REPORT_MOVE_STATUS_IDLE;
axisSubStep[1] = COMM_REPORT_MOVE_STATUS_IDLE;
axisSubStep[2] = COMM_REPORT_MOVE_STATUS_IDLE;
loadMotorSettings();
loadEncoderSettings();
// Create the encoder controller
@ -145,6 +196,10 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest,
// Prepare for movement
axisX.movementStarted = false;
axisY.movementStarted = false;
axisZ.movementStarted = false;
storeEndStops();
reportEndStops();
@ -152,6 +207,16 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest,
axisY.setDirectionAxis();
axisZ.setDirectionAxis();
// Enable motors
axisSubStep[0] = COMM_REPORT_MOVE_STATUS_START_MOTOR;
axisSubStep[1] = COMM_REPORT_MOVE_STATUS_START_MOTOR;
axisSubStep[2] = COMM_REPORT_MOVE_STATUS_START_MOTOR;
reportStatus(&axisX, axisSubStep[0]);
reportStatus(&axisY, axisSubStep[1]);
reportStatus(&axisZ, axisSubStep[2]);
enableMotors();
// Start movement
@ -167,6 +232,10 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest,
// Let the interrupt handle all the movements
while (axisActive[0] || axisActive[1] || axisActive[2]) {
checkAxisSubStatus(&axisX, &axisSubStep[0]);
checkAxisSubStatus(&axisY, &axisSubStep[1]);
checkAxisSubStatus(&axisZ, &axisSubStep[2]);
delay(1);
//delayMicroseconds(100);
@ -332,8 +401,20 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest,
Serial.print("R99 stopped\n");
// Stop motors
axisSubStep[0] = COMM_REPORT_MOVE_STATUS_STOP_MOTOR;
axisSubStep[1] = COMM_REPORT_MOVE_STATUS_STOP_MOTOR;
axisSubStep[2] = COMM_REPORT_MOVE_STATUS_STOP_MOTOR;
reportStatus(&axisX, axisSubStep[0]);
reportStatus(&axisY, axisSubStep[1]);
reportStatus(&axisZ, axisSubStep[2]);
disableMotors();
// Report end statuses
currentPoint[0] = axisX.currentPosition();
currentPoint[1] = axisY.currentPosition();
currentPoint[2] = axisZ.currentPosition();
@ -346,6 +427,20 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest,
reportEndStops();
reportPosition();
// Report axis idle
axisSubStep[0] = COMM_REPORT_MOVE_STATUS_IDLE;
axisSubStep[1] = COMM_REPORT_MOVE_STATUS_IDLE;
axisSubStep[2] = COMM_REPORT_MOVE_STATUS_IDLE;
reportStatus(&axisX, axisSubStep[0]);
reportStatus(&axisY, axisSubStep[1]);
reportStatus(&axisZ, axisSubStep[2]);
disableMotors();
// Return error
return error;
}
@ -379,6 +474,7 @@ int StepperControl::calibrateAxis(int axis) {
long * lastPosition;
float * encoderStepDecay;
bool * encoderEnabled;
int * axisStatus;
// Prepare for movement
@ -401,6 +497,7 @@ int StepperControl::calibrateAxis(int axis) {
lastPosition = &motorLastPosition[0];
encoderStepDecay = &motorConsMissedStepsDecay[0];
encoderEnabled = &motorConsEncoderEnabled[0];
axisStatus = &axisSubStep[0];
break;
case 1:
calibAxis = &axisY;
@ -413,6 +510,7 @@ int StepperControl::calibrateAxis(int axis) {
lastPosition = &motorLastPosition[1];
encoderStepDecay = &motorConsMissedStepsDecay[1];
encoderEnabled = &motorConsEncoderEnabled[1];
axisStatus = &axisSubStep[1];
break;
case 2:
calibAxis = &axisZ;
@ -425,6 +523,7 @@ int StepperControl::calibrateAxis(int axis) {
lastPosition = &motorLastPosition[2];
encoderStepDecay = &motorConsMissedStepsDecay[2];
encoderEnabled = &motorConsEncoderEnabled[2];
axisStatus = &axisSubStep[2];
break;
default:
Serial.print("R99 Calibration error: invalid axis selected\n");
@ -445,8 +544,10 @@ int StepperControl::calibrateAxis(int axis) {
Serial.print(" move to start for calibration");
Serial.print("\n");
// Move towards home
*axisStatus = COMM_REPORT_MOVE_STATUS_START_MOTOR;
reportStatus(&axisX, axisSubStep[0]);
// Move towards home
calibAxis->enableMotor();
calibAxis->setDirectionHome();
calibAxis->setCurrentPosition(calibEncoder->currentPosition());
@ -459,6 +560,11 @@ int StepperControl::calibrateAxis(int axis) {
motorConsMissedSteps[1] = 0;
motorConsMissedSteps[2] = 0;
*axisStatus = COMM_REPORT_MOVE_STATUS_CRAWLING;
reportStatus(&axisX, axisSubStep[0]);
reportCalib(&axisX, COMM_REPORT_CALIBRATE_STATUS_TO_HOME);
while (!movementDone && error == 0) {
//checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]);
@ -524,6 +630,8 @@ int StepperControl::calibrateAxis(int axis) {
}
}
reportCalib(&axisX, COMM_REPORT_CALIBRATE_STATUS_TO_END);
Serial.print("R99");
Serial.print(" axis ");
Serial.print(calibAxis->label);
@ -664,6 +772,9 @@ int StepperControl::calibrateAxis(int axis) {
Serial.print("\n");
}
*axisStatus = COMM_REPORT_MOVE_STATUS_STOP_MOTOR;
reportStatus(&axisX, axisSubStep[0]);
calibAxis->disableMotor();
storeEndStops();
@ -684,32 +795,16 @@ int StepperControl::calibrateAxis(int axis) {
reportPosition();
*axisStatus = COMM_REPORT_MOVE_STATUS_IDLE;
reportStatus(&axisX, axisSubStep[0]);
reportCalib(&axisX, COMM_REPORT_CALIBRATE_STATUS_IDLE);
return error;
}
void checkAxisSubStatus(StepperControlAxis* axis, int* axisSubStatus) {
int newStatus = 0;
if (axis->isAccelerating()) {
newStatus = COMM_REPORT_MOVE_STATUS_ACCELERATING;
}
if (axis->isCruising()) {
newStatus = COMM_REPORT_MOVE_STATUS_CRUISING;
}
if (axis->isDecelerating()) {
newStatus = COMM_REPORT_MOVE_STATUS_DECELERATING;
}
if (axis->isCrawling()) {
newStatus = COMM_REPORT_MOVE_STATUS_CRAWLING;
}
}
// Handle movement by checking each axis
void StepperControl::handleMovementInterrupt(void){
@ -897,6 +992,7 @@ unsigned long StepperControl::getMaxLength(unsigned long lengths[3]) {
}
void StepperControl::enableMotors() {
axisX.enableMotor();
axisY.enableMotor();
axisZ.enableMotor();
@ -932,4 +1028,3 @@ void StepperControl::reportPosition(){
void StepperControl::storeEndStops() {
CurrentState::getInstance()->storeEndStops();
}

2
src/StepperControl.h
View File

@ -62,6 +62,8 @@ private:
void reportPosition();
void storeEndStops();
void reportEndStops();
void reportStatus(StepperControlAxis* axis, int axisSubStatus);
void reportCalib(StepperControlAxis* axis, int calibStatus);
unsigned long getMaxLength(unsigned long lengths[3]);
bool endStopsReached();

21
src/StepperControlAxis.cpp
View File

@ -23,6 +23,7 @@ StepperControlAxis::StepperControlAxis() {
movementCruising = false;
movementCrawling = false;
movementMotorActive = false;
movementMoving = false;
}
void StepperControlAxis::test() {
@ -51,6 +52,7 @@ unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long curren
movementDecelerating = false;
movementCruising = false;
movementCrawling = false;
movementMoving = false;
if (abs(sourcePosition) < abs(destinationPosition)) {
staPos = abs(sourcePosition);
@ -64,30 +66,35 @@ unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long curren
// Set the minimum speed if the position would be out of bounds
if (curPos < staPos || curPos > endPos) {
newSpeed = minSpeed;
newSpeed = minSpeed;
movementCrawling = true;
movementMoving = true;
} else {
if (curPos >= staPos && curPos <= halfway) {
// accelerating
if (curPos > (staPos + stepsAccDec)) {
// now beyond the accelleration point to go full speed
newSpeed = maxSpeed + 1;
newSpeed = maxSpeed + 1;
movementCruising = true;
movementMoving = true;
} else {
// speeding up, increase speed linear within the first period
newSpeed = (1.0 * (curPos - staPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed;
newSpeed = (1.0 * (curPos - staPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed;
movementAccelerating = true;
movementMoving = true;
}
} else {
// decelerating
if (curPos < (endPos - stepsAccDec)) {
// still before the deceleration point so keep going at full speed
newSpeed = maxSpeed + 2;
newSpeed = maxSpeed + 2;
movementCruising = true;
movementMoving = true;
} else {
// speeding up, increase speed linear within the first period
newSpeed = (1.0 * (endPos - curPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed;
newSpeed = (1.0 * (endPos - curPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed;
movementDecelerating = true;
movementMoving = true;
}
}
}
@ -445,6 +452,10 @@ bool StepperControlAxis::isDecelerating() {
return movementDecelerating;
}
bool StepperControlAxis::isCruising() {
return movementCruising;
}
bool StepperControlAxis::isCrawling() {
return movementCrawling;
}

4
src/StepperControlAxis.h
View File

@ -40,6 +40,7 @@ public:
bool isCruising();
bool isCrawling();
bool isMotorActive();
bool isMoving();
bool endStopMin();
bool endStopMax();
@ -72,6 +73,7 @@ public:
void test();
char label;
bool movementStarted;
private:
@ -108,6 +110,7 @@ private:
unsigned long stepOnTick;
unsigned long stepOffTick;
unsigned long axisSpeed;
bool axisActive;
bool movementUp;
bool movementToHome;
@ -117,6 +120,7 @@ private:
bool movementCruising;
bool movementCrawling;
bool movementMotorActive;
bool movementMoving;
void step(long &currentPoint, unsigned long steps, long destinationPoint);
bool pointReached(long currentPoint, long destinationPoint);