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

Merge branch 'TimEvWw-master'

pull/30/head
TimEvWw 2015-07-15 19:06:34 -01:00
parent 263c393876 1b86fcd3f7
commit a083d3bc06
11 changed files with 1007 additions and 177 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
README.md
View File

@ -92,10 +92,10 @@ F |22 |P V |Write parameter
F |23 |P V |Update parameter (during calibration)
F |31 |P |Read status
F |32 |P V |Write status
F |41 |P V M |Set a value on an arduino pin
F |42 |P M |Read a value from an arduino pin
F |43 |P M |Set the mode of a pin in arduino
F |44 |P V W T |Set the value V on an arduino pin, wait for time T, set value W on the arduino pin
F |41 |P V M |Set a value on an arduino pin (digital/analog)
F |42 |P M |Read a value from an arduino pin (digital/analog)
F |43 |P M |Set the mode of a pin in arduino (input/output)
F |44 |P V W T M |Set the value V on an arduino pin, wait for time T, set value W on the arduino pin (d/a)
F |51 |E P V |Set a value on the tool mount with I2C
F |52 |E P |Read value from the tool mount with I2C
F |61 |P V |Set the servo on the pin (only pin 4 and 5) to the requested angle

6
src/Command.cpp
View File

@ -209,15 +209,15 @@ double Command::getZ() {
}
long Command::getA() {
return axisValue[0];
return axisSpeedValue[0];
}
long Command::getB() {
return axisValue[1];
return axisSpeedValue[1];
}
long Command::getC() {
return axisValue[2];
return axisSpeedValue[2];
}
long Command::getP() {

19
src/Config.h
View File

@ -41,25 +41,28 @@ const long MOVEMENT_INVERT_MOTOR_X_DEFAULT = 0;
const long MOVEMENT_INVERT_MOTOR_Y_DEFAULT = 0;
const long MOVEMENT_INVERT_MOTOR_Z_DEFAULT = 0;
const long MOVEMENT_STEPS_ACC_DEC_X_DEFAULT = 200;
const long MOVEMENT_STEPS_ACC_DEC_Y_DEFAULT = 200;
const long MOVEMENT_STEPS_ACC_DEC_Z_DEFAULT = 200;
const long MOVEMENT_HOME_UP_X_DEFAULT = 0;
const long MOVEMENT_HOME_UP_Y_DEFAULT = 0;
const long MOVEMENT_HOME_UP_Z_DEFAULT = 1;
// numver of steps used for acceleration or deceleration
const long MOVEMENT_STEPS_ACC_DEC_X_DEFAULT = 200;
const long MOVEMENT_STEPS_ACC_DEC_Y_DEFAULT = 200;
const long MOVEMENT_STEPS_ACC_DEC_Z_DEFAULT = 200;
// Minimum speed in steps per second
const long MOVEMENT_MIN_SPD_X_DEFAULT = 200;
const long MOVEMENT_MIN_SPD_Y_DEFAULT = 200;
const long MOVEMENT_MIN_SPD_Z_DEFAULT = 200;
const long MOVEMENT_MAX_SPD_X_DEFAULT = 1000;
const long MOVEMENT_MAX_SPD_Y_DEFAULT = 1000;
const long MOVEMENT_MAX_SPD_Z_DEFAULT = 1000;
// Maxumum speed in steps per second
const long MOVEMENT_MAX_SPD_X_DEFAULT = 4000;
const long MOVEMENT_MAX_SPD_Y_DEFAULT = 4000;
const long MOVEMENT_MAX_SPD_Z_DEFAULT = 4000;
const long STATUS_GENERAL_DEFAULT = 0;
const String SOFTWARE_VERSION = "GENESIS V.01.03";
const String SOFTWARE_VERSION = "GENESIS V.01.04";
#endif /* CONFIG_H_ */

21
src/G00Handler.cpp
View File

@ -22,8 +22,27 @@ G00Handler::G00Handler() {
}
int G00Handler::execute(Command* command) {
// Serial.print("R99");
// Serial.print(" X ");
// Serial.print(command->getX());
// Serial.print(" Y ");
// Serial.print(command->getY());
// Serial.print(" Z ");
// Serial.print(command->getZ());
// Serial.print(" A ");
// Serial.print(command->getA());
// Serial.print(" B ");
// Serial.print(command->getB());
// Serial.print(" C ");
// Serial.print(command->getC());
// Serial.print("\n");
StepperControl::getInstance()->moveAbsoluteConstant(
command->getX(), command->getY(), command->getZ(),
command->getX(), command->getY(), command->getZ(),
//0,0,0,
command->getA(), command->getB(), command->getC(),
false, false, false);
if (LOGGING) {

1
src/ServoControl.cpp
View File

@ -1,5 +1,6 @@
#include "ServoControl.h"
#include "Servo.h"
#include "TimerOne.h"
/*
Servo pin layout

632
src/StepperControl.cpp
View File

@ -1,4 +1,5 @@
#include "StepperControl.h"
#include "TimerOne.h"
static StepperControl* instance;
@ -10,7 +11,55 @@ StepperControl * StepperControl::getInstance() {
}
;
const int MOVEMENT_INTERRUPT_SPEED = 100; // Interrupt cycle in micro seconds
int test = 0;
long sourcePoint[3] = {0,0,0};
long currentPoint[3] = {0,0,0};
long destinationPoint[3] = {0,0,0};
unsigned long movementLength[3] = {0,0,0};
unsigned long maxLenth = 0;
unsigned long moveTicks[3] = {0,0,0};
unsigned long stepOnTick[3] = {0,0,0};
unsigned long stepOffTick[3] = {0,0,0};
unsigned long axisSpeed[3] = {0,0,0};
double lengthRatio[3] = {0,0,0};
long speedMax[3] = {0,0,0};
long speedMin[3] = {0,0,0};
long stepsAcc[3] = {0,0,0};
long timeOut[3] = {0,0,0};
bool homeIsUp[3] = {false,false,false};
bool motorInv[3] = {false,false,false};
bool endStInv[3] = {false,false,false};
bool homeAxis[3] = {false,false,false};
bool axisActive[3] = {false,false,false};
bool movementUp[3] = {false,false,false};
bool movementToHome[3] = {false,false,false};
bool home = false;
// ** test code
// The interrupt will blink the LED, and keep
// track of how many times it has blinked.
int ledState = LOW;
//volatile unsigned long blinkCount = 0; // use volatile for shared variables
void blinkLed() {
if (ledState == LOW) {
ledState = HIGH;
//Serial.print("R99 led on");
} else {
ledState = LOW;
//Serial.print("R99 led off");
}
digitalWrite(LED_PIN, ledState);
}
StepperControl::StepperControl() {
}
unsigned long getMaxLength(unsigned long lengths[3]) {
@ -121,6 +170,21 @@ void step(int axis, long &currentPoint, unsigned long steps,
}
}
void resetStep(int axis) {
switch (axis) {
case 0:
digitalWrite(X_STEP_PIN, LOW);
break;
case 1:
digitalWrite(Y_STEP_PIN, LOW);
break;
case 2:
digitalWrite(Z_STEP_PIN, LOW);
break;
}
}
bool pointReached(long currentPoint[3],
long destinationPoint[3]) {
for (int i = 0; i < 3; i++) {
@ -166,6 +230,9 @@ unsigned int calculateSpeed(unsigned int &currentStepsPerSecond,
}
*/
unsigned int lastCalcLog = 0;
unsigned int calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec) {
int newSpeed = 0;
@ -210,42 +277,45 @@ unsigned int calculateSpeed(long sourcePosition, long currentPosition, long dest
}
}
if (LOGGING) {
if (millis() % 200 == 0 && currentPosition != destinationPosition) {
/*
if (millis() - lastCalcLog > 1000) {
Serial.print("R99");
lastCalcLog = millis();
// Serial.print(" a ");
// Serial.print(endPos);
// Serial.print(" b ");
// Serial.print((endPos - stepsAccDec));
// Serial.print(" c ");
// Serial.print(curPos < (endPos - stepsAccDec));
Serial.print("R99");
// Serial.print(" a ");
// Serial.print(endPos);
// Serial.print(" b ");
// Serial.print((endPos - stepsAccDec));
// Serial.print(" c ");
// Serial.print(curPos < (endPos - stepsAccDec));
Serial.print(" sta ");
Serial.print(staPos);
Serial.print(" cur ");
Serial.print(curPos);
Serial.print(" end ");
Serial.print(endPos);
Serial.print(" half ");
Serial.print(halfway);
Serial.print(" len ");
Serial.print(stepsAccDec);
// Serial.print(" min ");
// Serial.print(minSpeed);
// Serial.print(" max ");
// Serial.print(maxSpeed);
Serial.print(" spd ");
Serial.print(" sta ");
Serial.print(staPos);
Serial.print(" cur ");
Serial.print(curPos);
Serial.print(" end ");
Serial.print(endPos);
Serial.print(" half ");
Serial.print(halfway);
Serial.print(" len ");
Serial.print(stepsAccDec);
Serial.print(" min ");
Serial.print(minSpeed);
Serial.print(" max ");
Serial.print(maxSpeed);
Serial.print(" spd ");
Serial.print(" ");
Serial.print(newSpeed);
Serial.print(" ");
Serial.print(newSpeed);
Serial.print("\n");
}
Serial.print("\n");
}
*/
// Return the calculated speed, in steps per second
return newSpeed;
}
@ -335,6 +405,242 @@ void doseWaterByTime(long time) {
digitalWrite(HEATER_1_PIN, LOW);
}
void checkAxisDirection(int i) {
if (homeAxis[i]){
// When home is active, the direction is fixed
movementUp[i] = homeIsUp[i];
movementToHome[i] = true;
} else {
// For normal movement, move in direction of destination
movementUp[i] = ( currentPoint[i] < destinationPoint[i] );
movementToHome[i] = (abs(currentPoint[i]) >= abs(destinationPoint[i]));
}
if (currentPoint[i] == 0) {
// Go slow when theoretical end point reached but there is no end stop siganl
axisSpeed[i] = speedMin[i];
}
}
void stepAxis(int i, bool state) {
if (homeAxis[i] && currentPoint[i] == 0) {
// Keep moving toward end stop even when position is zero
// but end stop is not yet active
if (homeIsUp[i]) {
currentPoint[i] = -1;
} else {
currentPoint[i] = 1;
}
}
// set a step on the motors
step(i, currentPoint[i], 1, destinationPoint[i]);
blinkLed();
//stepMade = true;
//lastStepMillis[i] = currentMillis;
}
void checkAxis(int i) {
//moveTicks[i]++;
checkAxisDirection(i);
/*
Serial.print("R99 check axis ");
Serial.print(i);
Serial.print(" axis active ");
Serial.print(axisActive[i]);
Serial.print(" current point ");
Serial.print(currentPoint[i]);
Serial.print(" destination point ");
Serial.print(destinationPoint[i]);
Serial.print(" home stop reached ");
Serial.print(endStopAxisReached(i, false));
Serial.print(" axis stop reached ");
Serial.print(endStopAxisReached(i, true));
Serial.print(" home axis ");
Serial.print(homeAxis[i]);
Serial.print(" movement to home ");
Serial.print(movementToHome[i]);
Serial.print("\n");
*/
//if ((!pointReached(currentPoint, destinationPoint) || home) && axisActive[i]) {
if (((currentPoint[i] != destinationPoint[i]) || homeAxis[i]) && axisActive[i]) {
//Serial.print("R99 point not reached yet\n");
// home or destination not reached, keep moving
/*
Serial.print("R99 calc axis speed");
Serial.print(" speed max ");
Serial.print(speedMax[i]);
Serial.print("\n");
*/
// Get the axis speed, in steps per second
axisSpeed[i] = calculateSpeed( sourcePoint[i],currentPoint[i],destinationPoint[i],
speedMin[i], speedMax[i], stepsAcc[i]);
//Serial.print("R99 axis speed ");
//Serial.print(axisSpeed[i]);
//Serial.print("\n");
/*
Serial.print("R99 check axis b ");
Serial.print(i);
Serial.print(" home part true ");
Serial.print(homeAxis[i] && !endStopAxisReached(i, false));
Serial.print(" !homeAxis[i] ");
Serial.print(!homeAxis[i]);
Serial.print(" !endStopAxisReached(i, !movementToHome[i]) ");
Serial.print(!endStopAxisReached(i, !movementToHome[i]));
Serial.print("\n");
*/
// If end stop reached, don't move anymore
if ((homeAxis[i] && !endStopAxisReached(i, false)) || (!homeAxis[i] && !endStopAxisReached(i, !movementToHome[i]))) {
// Set the moments when the step is set to true and false
if (axisSpeed[i] > 0)
{
/*
Serial.print("R99 calculated steps ");
Serial.print(" ** additional ticks steps ");
Serial.print(1000.0 * 1000.0 / MOVEMENT_INTERRUPT_SPEED / axisSpeed[i]);
Serial.print("\n");
*/
// Take the requested speed (steps / second) and divide by the interrupt speed (interrupts per seconde)
// This gives the number of interrupts (called ticks here) before the pulse needs to be set for the next step
stepOnTick[i] = moveTicks[i] + (1000.0 * 1000.0 / MOVEMENT_INTERRUPT_SPEED / axisSpeed[i] / 2);
stepOffTick[i] = moveTicks[i] + (1000.0 * 1000.0 / MOVEMENT_INTERRUPT_SPEED / axisSpeed[i] );
//stepOnTick[i] = moveTicks[i] + (axisSpeed[i] * 1000 / MOVEMENT_INTERRUPT_SPEED / 2);
//stepOffTick[i] = moveTicks[i] + (axisSpeed[i] * 1000 / MOVEMENT_INTERRUPT_SPEED );
/*
Serial.print("R99 calculated steps ");
Serial.print(" interrupt speed ");
Serial.print(MOVEMENT_INTERRUPT_SPEED);
Serial.print(" axisSpeed ");
Serial.print(axisSpeed[i]);
Serial.print(" moveTicks ");
Serial.print(moveTicks[i]);
Serial.print(" stepOnTick ");
Serial.print(stepOnTick[i]);
Serial.print(" stepOffTick ");
Serial.print(stepOffTick[i]);
Serial.print("\n");
*/
}
}
else {
axisActive[i] = false;
}
} else {
// Destination or home reached. Deactivate the axis.
axisActive[i] = false;
}
// If end stop for home is active, set the position to zero
if (endStopAxisReached(i, false)) {
currentPoint[i] = 0;
}
}
void checkTicksAxis(int i) {
moveTicks[i]++;
//Serial.print("R99 checkTicksAxis ");
//Serial.print(" destination point ");
//Serial.print(destinationPoint[0]);
//Serial.print(" moveTicks ");
//Serial.print(moveTicks[i]);
//Serial.print(" stepOnTick ");
//Serial.print(stepOnTick[i]);
//Serial.print(" stepOffTick ");
//Serial.print(stepOffTick[i]);
//Serial.print("\n");
if (axisActive[i]) {
if (moveTicks[i] >= stepOffTick[i]) {
/*
Serial.print("R99 reset step ");
Serial.print(" moveTicks ");
Serial.print(moveTicks[i]);
Serial.print("\n");
*/
// Negative flank for the steps
resetStep(i);
checkAxis(i);
}
else {
if (moveTicks[i] == stepOnTick[i]) {
/*
Serial.print("R99 set step ");
Serial.print(" moveTicks ");
Serial.print(moveTicks[i]);
Serial.print("\n");
*/
// Positive flank for the steps
stepAxis(i, true);
}
}
}
}
// Handle movement by checking each axis
void StepperControl::handleMovementInterrupt(void){
//Serial.print("R99 interrupt\n");
//Serial.print("R99 interrupt data ");
//Serial.print(" destination point ");
//Serial.print(destinationPoint[0]);
//Serial.print(" ");
//Serial.print("test ");
//Serial.print(test);
//Serial.print("\n");
checkTicksAxis(0);
checkTicksAxis(1);
checkTicksAxis(2);
// blinkLed();
}
bool interruptBusy = false;
void handleMovementInterruptTest(void) {
if (interruptBusy == false) {
interruptBusy = true;
StepperControl::getInstance()->handleMovementInterrupt();
//blinkLed();
interruptBusy = false;
}
}
// Start the interrupt used for moviing
// Interrupt management code library written by Paul Stoffregen
void StepperControl::initInterrupt() {
//Timer1.attachInterrupt(StepperControl::getInstance()->handleMovementInterrupt);
Timer1.attachInterrupt(handleMovementInterruptTest);
Timer1.initialize(MOVEMENT_INTERRUPT_SPEED);
Timer1.stop();
}
/**
* xDest - destination X in steps
* yDest - destination Y in steps
@ -346,70 +652,92 @@ int StepperControl::moveAbsoluteConstant( long xDest, long yDest, long zDest,
unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd,
bool xHome, bool yHome, bool zHome) {
long sourcePoint[3] = { CurrentState::getInstance()->getX(),
CurrentState::getInstance()->getY(),
CurrentState::getInstance()->getZ() };
long currentPoint[3] = { CurrentState::getInstance()->getX(),
CurrentState::getInstance()->getY(),
CurrentState::getInstance()->getZ() };
//Serial.print("R99 ");
//Serial.print(" xMaxSpd ");
//Serial.print(xMaxSpd);
//Serial.print("\n");
long destinationPoint[3] = { xDest, yDest, zDest };
sourcePoint[0] = CurrentState::getInstance()->getX();
sourcePoint[1] = CurrentState::getInstance()->getY();
sourcePoint[2] = CurrentState::getInstance()->getZ();
unsigned long movementLength[3] = { getLength(destinationPoint[0], currentPoint[0]),
getLength(destinationPoint[1], currentPoint[1]),
getLength(destinationPoint[2], currentPoint[2])};
unsigned long maxLenth = getMaxLength(movementLength);
currentPoint[0] = CurrentState::getInstance()->getX();
currentPoint[1] = CurrentState::getInstance()->getY();
currentPoint[2] = CurrentState::getInstance()->getZ();
double lengthRatio[3] = { 1.0 * movementLength[0] / maxLenth,
1.0 * movementLength[1] / maxLenth,
1.0 * movementLength[2] / maxLenth };
destinationPoint[0] = xDest;
destinationPoint[1] = yDest;
destinationPoint[2] = zDest;
bool homeIsUp[3] = { ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X),
ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y),
ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Z) };
movementLength[0] = getLength(destinationPoint[0], currentPoint[0]);
movementLength[1] = getLength(destinationPoint[1], currentPoint[1]);
movementLength[2] = getLength(destinationPoint[2], currentPoint[2]);
long speedMax[3] = { ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_X),
ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Y),
ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Z) };
maxLenth = getMaxLength(movementLength);
long speedMin[3] = { ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_X),
ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_Y),
ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_Z) };
lengthRatio[0] = 1.0 * movementLength[0] / maxLenth;
lengthRatio[1] = 1.0 * movementLength[1] / maxLenth;
lengthRatio[2] = 1.0 * movementLength[2] / maxLenth;
long stepsAcc[3] = { ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_X),
ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_Y),
ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_Z) };
homeIsUp[0] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X);
homeIsUp[1] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y);
homeIsUp[2] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Z);
bool motorInv[3] = { ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_X),
ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Y),
ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Z) };
speedMax[0] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_X);
speedMax[1] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Y);
speedMax[2] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Z);
bool endStInv[3] = { ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_X),
ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Y),
ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z) };
speedMin[0] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_X);
speedMin[1] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_Y);
speedMin[2] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_Z);
long timeOut[3] = { ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X),
ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X),
ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X) };
stepsAcc[0] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_X);
stepsAcc[1] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_Y);
stepsAcc[2] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_Z);
motorInv[0] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_X);
motorInv[1] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Y);
motorInv[2] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Z);
endStInv[0] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_X);
endStInv[1] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Y);
endStInv[2] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z);
timeOut[0] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X);
timeOut[1] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X);
timeOut[2] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X);
homeAxis[0] = xHome;
homeAxis[1] = yHome;
homeAxis[2] = zHome;
moveTicks[0] = 0;
moveTicks[1] = 0;
moveTicks[2] = 0;
home = xHome || yHome || zHome;
//Serial.print("R99 ");
//Serial.print(" after vars ");
//Serial.print(destinationPoint[0]);
//Serial.print("\n");
bool homeAxis[3] = { xHome, yHome, zHome };
bool home = xHome || yHome || zHome;
unsigned long currentMillis = 0;
unsigned long currentSteps = 0;
unsigned long lastStepMillis[3] = { 0, 0, 0 };
unsigned long timeStart = millis();
bool movementDone = false;
bool movementUp = false;
bool movementToHome = false;
bool moving = false;
bool stepMade = false;
int incomingByte = 0;
int axisSpeed = 0;
//int axisSpeed = 0;
int error = 0;
// if a speed is given in the command, use that instead of the config speed
@ -428,13 +756,13 @@ int StepperControl::moveAbsoluteConstant( long xDest, long yDest, long zDest,
// Prepare for movement
setDirections(currentPoint, destinationPoint, homeAxis, motorInv);
storeEndStops();
reportEndStops();
enableMotors(true);
setDirections(currentPoint, destinationPoint, homeAxis, motorInv);
// Limit normal movmement to the home position
for (int i = 0; i < 3; i++) {
if (!homeIsUp[i] && destinationPoint[i] < 0) {
@ -448,109 +776,92 @@ int StepperControl::moveAbsoluteConstant( long xDest, long yDest, long zDest,
// Start movement
while (!movementDone) {
axisActive[0] = true;
axisActive[1] = true;
axisActive[2] = true;
checkAxis(0);
checkAxis(1);
checkAxis(2);
Timer1.start();
// Let the interrupt handle all the movements
while (axisActive[0] || axisActive[1] || axisActive[2]) {
/**///Serial.print("R99 while loop\n");
//delay(50);
delayMicroseconds(100);
//Serial.print("R99 ");
//Serial.print(" maximim speed ");
//Serial.print(speedMax[0]);
//Serial.print("\n");
checkTicksAxis(0);
checkTicksAxis(1);
checkTicksAxis(2);
//axisActive[0] = false;
//axisActive[1] = false;
//axisActive[2] = false;
/*
Serial.print("R99");
Serial.print(" cur ");
Serial.print(currentPoint[0]);
Serial.print(" end ");
Serial.print(destinationPoint[0]);
Serial.print(" max spd ");
Serial.print(speedMax[0]);
Serial.print(" axis spd ");
Serial.print(axisSpeed[0]);
Serial.print("\n");
*/
storeEndStops();
// Check timeouts
if (axisActive[0] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) {
error = 1;
}
if (axisActive[1] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) {
error = 1;
}
if (axisActive[2] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) {
error = 1;
}
// Check if there is an emergency stop command
if (Serial.available() > 0) {
incomingByte = Serial.read();
if (incomingByte == 69 || incomingByte == 101) {
movementDone = true;
}
error = 1;
}
}
stepMade = false;
moving = false;
// Keep moving until destination reached or while moving home and end stop not reached
if ((!pointReached(currentPoint, destinationPoint) || home) && !movementDone) {
// Check each axis
for (int i = 0; i < 3; i++) {
axisSpeed = calculateSpeed( sourcePoint[i],currentPoint[i],destinationPoint[i],
speedMin[i], speedMax[i], stepsAcc[i]);
if (homeAxis[i]){
// When home is active, the direction is fixed
movementUp = homeIsUp[i];
movementToHome = true;
if (currentPoint[i] == 0) {
// Go slow when theoretical end point reached but there is no end stop siganl
axisSpeed = speedMin[i];
}
} else {
// For normal movement, move in direction of destination
movementUp = ( currentPoint[i] < destinationPoint[i] );
movementToHome = (abs(currentPoint[i]) >= abs(destinationPoint[i]));
}
if (millis() - timeStart > timeOut[i] * MOVEMENT_SPEED_BASE_TIME) {
error = 1;
} else {
//if ()
// If end stop reached, don't move anymore
if ((homeAxis[i] && !endStopAxisReached(i, false)) || (!homeAxis[i] && !endStopAxisReached(i, !movementToHome) && currentPoint[i] != destinationPoint[i] )) {
moving = true;
// Only do a step every x milliseconds (x is calculated above)
if (currentMillis - lastStepMillis[i] >= MOVEMENT_SPEED_BASE_TIME / axisSpeed) {
if (homeAxis[i] && currentPoint[i] == 0) {
// Keep moving toward end stop even when position is zero
// but end stop is not yet active
if (homeIsUp[i]) {
currentPoint[i] = -1;
} else {
currentPoint[i] = 1;
}
}
// set a step on the motors
step(i, currentPoint[i], 1, destinationPoint[i]);
stepMade = true;
lastStepMillis[i] = currentMillis;
}
}
// If end stop for home is active, set the position to zero
if (endStopAxisReached(i, false))
{
currentPoint[i] = 0;
}
}
}
} else {
movementDone = true;
}
delayMicroseconds(MOVEMENT_DELAY);
if (stepMade) {
currentSteps++;
if (error == 1) {
Serial.print("R99 error\n");
Timer1.stop();
axisActive[0] = false;
axisActive[1] = false;
axisActive[2] = false;
}
// Periodically send message still active
currentMillis++;
if (currentMillis % 10000 == 0)
if (currentMillis % 2500 == 0)
{
// Periodically send message still active
Serial.print("R04\n");
}
if (stepMade) {
digitalWrite(X_STEP_PIN, LOW);
digitalWrite(Y_STEP_PIN, LOW);
digitalWrite(Z_STEP_PIN, LOW);
}
if (!moving)
{
movementDone = true;
}
delayMicroseconds(MOVEMENT_DELAY);
}
/**/Serial.print("R99 stopped\n");
Timer1.stop();
enableMotors(false);
CurrentState::getInstance()->setX(currentPoint[0]);
@ -614,8 +925,8 @@ int StepperControl::calibrateAxis(int axis) {
int paramValueInt = 0;
bool invertEndStops = false;
int stepsCount = 0;
int incomingByte = 0;
int error = 0;
int incomingByte = 0;
int error = 0;
// Prepare for movement
@ -661,7 +972,7 @@ Serial.print("\n");
// Move until the end stop for home position is reached
if ((!endStopMin(axis) && !endStopMax(axis)) && !movementDone) {
if (millis() - timeStart > timeOut[axis] * MOVEMENT_SPEED_BASE_TIME) {
if ((millis() >= timeStart && millis() - timeStart > timeOut[axis] * 1000) || (millis() < timeStart && millis() > timeOut[axis] * 1000)) {
movementDone = true;
error = 1;
} else {
@ -845,3 +1156,4 @@ Serial.print("\n");
return error;
}

3
src/StepperControl.h
View File

@ -25,7 +25,10 @@ public:
int moveAbsoluteConstant(long xDest, long yDest, long zDest,
unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd,
bool homeX, bool homeY, bool homeZ);
void handleMovementInterrupt();
int calibrateAxis(int axis);
void initInterrupt();
private:
StepperControl();
StepperControl(StepperControl const&);

44
src/TimerOne.cpp 100644
View File

@ -0,0 +1,44 @@
/*
* Interrupt and PWM utilities for 16 bit Timer1 on ATmega168/328
* Original code by Jesse Tane for http://labs.ideo.com August 2008
* Modified March 2009 by Jérôme Despatis and Jesse Tane for ATmega328 support
* Modified June 2009 by Michael Polli and Jesse Tane to fix a bug in setPeriod() which caused the timer to stop
* Modified Oct 2009 by Dan Clemens to work with timer1 of the ATMega1280 or Arduino Mega
* Modified April 2012 by Paul Stoffregen
* Modified again, June 2014 by Paul Stoffregen
*
* This is free software. You can redistribute it and/or modify it under
* the terms of Creative Commons Attribution 3.0 United States License.
* To view a copy of this license, visit http://creativecommons.org/licenses/by/3.0/us/
* or send a letter to Creative Commons, 171 Second Street, Suite 300, San Francisco, California, 94105, USA.
*
*/
#include "TimerOne.h"
TimerOne Timer1; // preinstatiate
unsigned short TimerOne::pwmPeriod = 0;
unsigned char TimerOne::clockSelectBits = 0;
void (*TimerOne::isrCallback)() = NULL;
// interrupt service routine that wraps a user defined function supplied by attachInterrupt
#if defined(__AVR__)
ISR(TIMER1_OVF_vect)
{
Timer1.isrCallback();
}
#elif defined(__arm__) && defined(CORE_TEENSY)
void ftm1_isr(void)
{
uint32_t sc = FTM1_SC;
#ifdef KINETISL
if (sc & 0x80) FTM1_SC = sc;
#else
if (sc & 0x80) FTM1_SC = sc & 0x7F;
#endif
Timer1.isrCallback();
}
#endif

304
src/TimerOne.h 100644
View File

@ -0,0 +1,304 @@
/*
* Interrupt and PWM utilities for 16 bit Timer1 on ATmega168/328
* Original code by Jesse Tane for http://labs.ideo.com August 2008
* Modified March 2009 by Jérôme Despatis and Jesse Tane for ATmega328 support
* Modified June 2009 by Michael Polli and Jesse Tane to fix a bug in setPeriod() which caused the timer to stop
* Modified April 2012 by Paul Stoffregen - portable to other AVR chips, use inline functions
* Modified again, June 2014 by Paul Stoffregen - support Teensy 3.x & even more AVR chips
*
*
* This is free software. You can redistribute it and/or modify it under
* the terms of Creative Commons Attribution 3.0 United States License.
* To view a copy of this license, visit http://creativecommons.org/licenses/by/3.0/us/
* or send a letter to Creative Commons, 171 Second Street, Suite 300, San Francisco, California, 94105, USA.
*
*/
#ifndef TimerOne_h_
#define TimerOne_h_
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include "config/known_16bit_timers.h"
#define TIMER1_RESOLUTION 65536UL // Timer1 is 16 bit
// Placing nearly all the code in this .h file allows the functions to be
// inlined by the compiler. In the very common case with constant values
// the compiler will perform all calculations and simply write constants
// to the hardware registers (for example, setPeriod).
class TimerOne
{
#if defined(__AVR__)
public:
//****************************
// Configuration
//****************************
void initialize(unsigned long microseconds=1000000) __attribute__((always_inline)) {
TCCR1B = _BV(WGM13); // set mode as phase and frequency correct pwm, stop the timer
TCCR1A = 0; // clear control register A
setPeriod(microseconds);
}
void setPeriod(unsigned long microseconds) __attribute__((always_inline)) {
const unsigned long cycles = (F_CPU / 2000000) * microseconds;
if (cycles < TIMER1_RESOLUTION) {
clockSelectBits = _BV(CS10);
pwmPeriod = cycles;
} else
if (cycles < TIMER1_RESOLUTION * 8) {
clockSelectBits = _BV(CS11);
pwmPeriod = cycles / 8;
} else
if (cycles < TIMER1_RESOLUTION * 64) {
clockSelectBits = _BV(CS11) | _BV(CS10);
pwmPeriod = cycles / 64;
} else
if (cycles < TIMER1_RESOLUTION * 256) {
clockSelectBits = _BV(CS12);
pwmPeriod = cycles / 256;
} else
if (cycles < TIMER1_RESOLUTION * 1024) {
clockSelectBits = _BV(CS12) | _BV(CS10);
pwmPeriod = cycles / 1024;
} else {
clockSelectBits = _BV(CS12) | _BV(CS10);
pwmPeriod = TIMER1_RESOLUTION - 1;
}
ICR1 = pwmPeriod;
TCCR1B = _BV(WGM13) | clockSelectBits;
}
//****************************
// Run Control
//****************************
void start() __attribute__((always_inline)) {
TCCR1B = 0;
TCNT1 = 0; // TODO: does this cause an undesired interrupt?
resume();
}
void stop() __attribute__((always_inline)) {
TCCR1B = _BV(WGM13);
}
void restart() __attribute__((always_inline)) {
start();
}
void resume() __attribute__((always_inline)) {
TCCR1B = _BV(WGM13) | clockSelectBits;
}
//****************************
// PWM outputs
//****************************
void setPwmDuty(char pin, unsigned int duty) __attribute__((always_inline)) {
unsigned long dutyCycle = pwmPeriod;
dutyCycle *= duty;
dutyCycle >>= 10;
if (pin == TIMER1_A_PIN) OCR1A = dutyCycle;
#ifdef TIMER1_B_PIN
else if (pin == TIMER1_B_PIN) OCR1B = dutyCycle;
#endif
#ifdef TIMER1_C_PIN
else if (pin == TIMER1_C_PIN) OCR1C = dutyCycle;
#endif
}
void pwm(char pin, unsigned int duty) __attribute__((always_inline)) {
if (pin == TIMER1_A_PIN) { pinMode(TIMER1_A_PIN, OUTPUT); TCCR1A |= _BV(COM1A1); }
#ifdef TIMER1_B_PIN
else if (pin == TIMER1_B_PIN) { pinMode(TIMER1_B_PIN, OUTPUT); TCCR1A |= _BV(COM1B1); }
#endif
#ifdef TIMER1_C_PIN
else if (pin == TIMER1_C_PIN) { pinMode(TIMER1_C_PIN, OUTPUT); TCCR1A |= _BV(COM1C1); }
#endif
setPwmDuty(pin, duty);
TCCR1B = _BV(WGM13) | clockSelectBits;
}
void pwm(char pin, unsigned int duty, unsigned long microseconds) __attribute__((always_inline)) {
if (microseconds > 0) setPeriod(microseconds);
pwm(pin, duty);
}
void disablePwm(char pin) __attribute__((always_inline)) {
if (pin == TIMER1_A_PIN) TCCR1A &= ~_BV(COM1A1);
#ifdef TIMER1_B_PIN
else if (pin == TIMER1_B_PIN) TCCR1A &= ~_BV(COM1B1);
#endif
#ifdef TIMER1_C_PIN
else if (pin == TIMER1_C_PIN) TCCR1A &= ~_BV(COM1C1);
#endif
}
//****************************
// Interrupt Function
//****************************
void attachInterrupt(void (*isr)()) __attribute__((always_inline)) {
isrCallback = isr;
TIMSK1 = _BV(TOIE1);
}
void attachInterrupt(void (*isr)(), unsigned long microseconds) __attribute__((always_inline)) {
if(microseconds > 0) setPeriod(microseconds);
attachInterrupt(isr);
}
void detachInterrupt() __attribute__((always_inline)) {
TIMSK1 = 0;
}
static void (*isrCallback)();
private:
// properties
static unsigned short pwmPeriod;
static unsigned char clockSelectBits;
#elif defined(__arm__) && defined(CORE_TEENSY)
#if defined(KINETISK)
#define F_TIMER F_BUS
#elif defined(KINETISL)
#define F_TIMER (F_PLL/2)
#endif
public:
//****************************
// Configuration
//****************************
void initialize(unsigned long microseconds=1000000) __attribute__((always_inline)) {
setPeriod(microseconds);
}
void setPeriod(unsigned long microseconds) __attribute__((always_inline)) {
const unsigned long cycles = (F_TIMER / 2000000) * microseconds;
if (cycles < TIMER1_RESOLUTION) {
clockSelectBits = 0;
pwmPeriod = cycles;
} else
if (cycles < TIMER1_RESOLUTION * 2) {
clockSelectBits = 1;
pwmPeriod = cycles >> 1;
} else
if (cycles < TIMER1_RESOLUTION * 4) {
clockSelectBits = 2;
pwmPeriod = cycles >> 2;
} else
if (cycles < TIMER1_RESOLUTION * 8) {
clockSelectBits = 3;
pwmPeriod = cycles >> 3;
} else
if (cycles < TIMER1_RESOLUTION * 16) {
clockSelectBits = 4;
pwmPeriod = cycles >> 4;
} else
if (cycles < TIMER1_RESOLUTION * 32) {
clockSelectBits = 5;
pwmPeriod = cycles >> 5;
} else
if (cycles < TIMER1_RESOLUTION * 64) {
clockSelectBits = 6;
pwmPeriod = cycles >> 6;
} else
if (cycles < TIMER1_RESOLUTION * 128) {
clockSelectBits = 7;
pwmPeriod = cycles >> 7;
} else {
clockSelectBits = 7;
pwmPeriod = TIMER1_RESOLUTION - 1;
}
uint32_t sc = FTM1_SC;
FTM1_SC = 0;
FTM1_MOD = pwmPeriod;
FTM1_SC = FTM_SC_CLKS(1) | FTM_SC_CPWMS | clockSelectBits | (sc & FTM_SC_TOIE);
}
//****************************
// Run Control
//****************************
void start() __attribute__((always_inline)) {
stop();
FTM1_CNT = 0;
resume();
}
void stop() __attribute__((always_inline)) {
FTM1_SC = FTM1_SC & (FTM_SC_TOIE | FTM_SC_CPWMS | FTM_SC_PS(7));
}
void restart() __attribute__((always_inline)) {
start();
}
void resume() __attribute__((always_inline)) {
FTM1_SC = (FTM1_SC & (FTM_SC_TOIE | FTM_SC_PS(7))) | FTM_SC_CPWMS | FTM_SC_CLKS(1);
}
//****************************
// PWM outputs
//****************************
void setPwmDuty(char pin, unsigned int duty) __attribute__((always_inline)) {
unsigned long dutyCycle = pwmPeriod;
dutyCycle *= duty;
dutyCycle >>= 10;
if (pin == TIMER1_A_PIN) {
FTM1_C0V = dutyCycle;
} else if (pin == TIMER1_B_PIN) {
FTM1_C1V = dutyCycle;
}
}
void pwm(char pin, unsigned int duty) __attribute__((always_inline)) {
setPwmDuty(pin, duty);
if (pin == TIMER1_A_PIN) {
*portConfigRegister(TIMER1_A_PIN) = PORT_PCR_MUX(3) | PORT_PCR_DSE | PORT_PCR_SRE;
} else if (pin == TIMER1_B_PIN) {
*portConfigRegister(TIMER1_B_PIN) = PORT_PCR_MUX(3) | PORT_PCR_DSE | PORT_PCR_SRE;
}
}
void pwm(char pin, unsigned int duty, unsigned long microseconds) __attribute__((always_inline)) {
if (microseconds > 0) setPeriod(microseconds);
pwm(pin, duty);
}
void disablePwm(char pin) __attribute__((always_inline)) {
if (pin == TIMER1_A_PIN) {
*portConfigRegister(TIMER1_A_PIN) = 0;
} else if (pin == TIMER1_B_PIN) {
*portConfigRegister(TIMER1_B_PIN) = 0;
}
}
//****************************
// Interrupt Function
//****************************
void attachInterrupt(void (*isr)()) __attribute__((always_inline)) {
isrCallback = isr;
FTM1_SC |= FTM_SC_TOIE;
NVIC_ENABLE_IRQ(IRQ_FTM1);
}
void attachInterrupt(void (*isr)(), unsigned long microseconds) __attribute__((always_inline)) {
if(microseconds > 0) setPeriod(microseconds);
attachInterrupt(isr);
}
void detachInterrupt() __attribute__((always_inline)) {
FTM1_SC &= ~FTM_SC_TOIE;
NVIC_DISABLE_IRQ(IRQ_FTM1);
}
static void (*isrCallback)();
private:
// properties
static unsigned short pwmPeriod;
static unsigned char clockSelectBits;
#undef F_TIMER
#endif
};
extern TimerOne Timer1;
#endif

143
src/config/known_16bit_timers.h 100644
View File

@ -0,0 +1,143 @@
#ifndef known_16bit_timers_header_
#define known_16bit_timers_header_
// Wiring-S
//
#if defined(__AVR_ATmega644P__) && defined(WIRING)
#define TIMER1_A_PIN 5
#define TIMER1_B_PIN 4
#define TIMER1_ICP_PIN 6
// Teensy 2.0
//
#elif defined(__AVR_ATmega32U4__) && defined(CORE_TEENSY)
#define TIMER1_A_PIN 14
#define TIMER1_B_PIN 15
#define TIMER1_C_PIN 4
#define TIMER1_ICP_PIN 22
#define TIMER1_CLK_PIN 11
#define TIMER3_A_PIN 9
#define TIMER3_ICP_PIN 10
// Teensy++ 2.0
#elif defined(__AVR_AT90USB1286__) && defined(CORE_TEENSY)
#define TIMER1_A_PIN 25
#define TIMER1_B_PIN 26
#define TIMER1_C_PIN 27
#define TIMER1_ICP_PIN 4
#define TIMER1_CLK_PIN 6
#define TIMER3_A_PIN 16
#define TIMER3_B_PIN 15
#define TIMER3_C_PIN 14
#define TIMER3_ICP_PIN 17
#define TIMER3_CLK_PIN 13
// Teensy 3.0
//
#elif defined(__MK20DX128__)
#define TIMER1_A_PIN 3
#define TIMER1_B_PIN 4
#define TIMER1_ICP_PIN 4
// Teensy 3.1
//
#elif defined(__MK20DX256__)
#define TIMER1_A_PIN 3
#define TIMER1_B_PIN 4
#define TIMER1_ICP_PIN 4
#define TIMER3_A_PIN 32
#define TIMER3_B_PIN 25
#define TIMER3_ICP_PIN 32
// Teensy-LC
//
#elif defined(__MKL26Z64__)
#define TIMER1_A_PIN 16
#define TIMER1_B_PIN 17
#define TIMER1_ICP_PIN 17
#define TIMER3_A_PIN 3
#define TIMER3_B_PIN 4
#define TIMER3_ICP_PIN 4
// Arduino Mega
//
#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
#define TIMER1_A_PIN 11
#define TIMER1_B_PIN 12
#define TIMER1_C_PIN 13
#define TIMER3_A_PIN 5
#define TIMER3_B_PIN 2
#define TIMER3_C_PIN 3
#define TIMER4_A_PIN 6
#define TIMER4_B_PIN 7
#define TIMER4_C_PIN 8
#define TIMER4_ICP_PIN 49
#define TIMER5_A_PIN 46
#define TIMER5_B_PIN 45
#define TIMER5_C_PIN 44
#define TIMER3_ICP_PIN 48
#define TIMER3_CLK_PIN 47
// Arduino Leonardo, Yun, etc
//
#elif defined(__AVR_ATmega32U4__)
#define TIMER1_A_PIN 9
#define TIMER1_B_PIN 10
#define TIMER1_C_PIN 11
#define TIMER1_ICP_PIN 4
#define TIMER1_CLK_PIN 12
#define TIMER3_A_PIN 5
#define TIMER3_ICP_PIN 13
// Uno, Duemilanove, LilyPad, etc
//
#elif defined (__AVR_ATmega168__) || defined (__AVR_ATmega328P__)
#define TIMER1_A_PIN 9
#define TIMER1_B_PIN 10
#define TIMER1_ICP_PIN 8
#define TIMER1_CLK_PIN 5
// Sanguino
//
#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__)
#define TIMER1_A_PIN 13
#define TIMER1_B_PIN 12
#define TIMER1_ICP_PIN 14
#define TIMER1_CLK_PIN 1
// Wildfire - Wicked Devices
//
#elif defined(__AVR_ATmega1284P__) && defined(WILDFIRE_VERSION) && WILDFIRE_VERSION >= 3
#define TIMER1_A_PIN 5 // PD5
#define TIMER1_B_PIN 8 // PD4
#define TIMER1_ICP_PIN 6 // PD6
#define TIMER1_CLK_PIN 23 // PB1
#define TIMER3_A_PIN 12 // PB6
#define TIMER3_B_PIN 13 // PB7
#define TIMER3_ICP_PIN 9 // PB5
#define TIMER3_CLK_PIN 0 // PD0
#elif defined(__AVR_ATmega1284P__) && defined(WILDFIRE_VERSION) && WILDFIRE_VERSION < 3
#define TIMER1_A_PIN 5 // PD5
#define TIMER1_B_PIN 4 // PD4
#define TIMER1_ICP_PIN 6 // PD6
#define TIMER1_CLK_PIN 15 // PB1
#define TIMER3_A_PIN 12 // PB6
#define TIMER3_B_PIN 13 // PB7
#define TIMER3_ICP_PIN 11 // PB5
#define TIMER3_CLK_PIN 0 // PD0
// Mighty-1284 - Maniacbug
//
#elif defined(__AVR_ATmega1284P__)
#define TIMER1_A_PIN 12 // PD5
#define TIMER1_B_PIN 13 // PD4
#define TIMER1_ICP_PIN 14 // PD6
#define TIMER1_CLK_PIN 1 // PB1
#define TIMER3_A_PIN 6 // PB6
#define TIMER3_B_PIN 7 // PB7
#define TIMER3_ICP_PIN 5 // PB5
#define TIMER3_CLK_PIN 8 // PD0
#endif
#endif

3
src/farmbot_arduino_controller.cpp
View File

@ -2,7 +2,7 @@
#include "farmbot_arduino_controller.h"
#include "pins.h"
#include "Config.h"
#include "StepperControl.h"
#include "ServoControl.h"
static char commandEndChar = 0x0A;
@ -46,6 +46,7 @@ void setup() {
Serial.begin(115200);
ServoControl::getInstance()->attach();
StepperControl::getInstance()->initInterrupt();
lastAction = millis();