void
backward()
{
int rc;
_backward();
/* XXX: I am relying on the fact that only one (x or y) will
* change at any given time. Thus, I don't try to "undo" the
* setting of x when y fails. I just assume that x didn't
* actually change.
*/
rc = mws_set_rat_xpos(M->state, M->local_id, MY_X_LOC);
if (rc != 0)
goto undo;
rc = mws_set_rat_ypos(M->state, M->local_id, MY_Y_LOC);
if (rc != 0)
goto undo;
return;
undo:
_forward();
}
// 0 pin step function (ie for functional usage)
void AccelStepper::step0(long step)
{
if (_speed > 0)
_forward();
else
_backward();
}
// 0 pin step function (ie for functional usage)
void AccelStepper::step0(uint8_t step)
{
if (_speed > 0)
_forward();
else
_backward();
}
boolean AccelStepper::runSpeed1()
{
unsigned long time = micros();//Adjusted to microsecond based instead of millisecond
//unsigned long deltatime;
if (time > _lastStepTime + _stepInterval)
{
if (_speed > 0)
{
_forward();
// Clockwise
_currentPos += 1;
}
else if (_speed < 0)
{
// Anticlockwise
_backward();
_currentPos -= 1;
}
//deltatime = time - _lastStepTime;
_lastStepTime = time;
//Serial.println("steptime(us)");
//Serial.println(deltatime);
return true;
}
else
return false;
}
// 0 pin step function (ie for functional usage)
void AccelStepper::step0()
{
if (_speed > 0) {
_forward();
} else {
_backward();
}
}
/**
* Main loop function that will handle the current auton state. This functions
* could take a large finite amount of time to run due to the nature of the
* robot.
* @param hw is Hardware object to manipulate
*/
void auto_update(Hardware& hw){
// Run current state and update for next update call
switch(_state){
case MOVE_FWD:
_state = _forward(hw);
break;
case MOVE_BWD:
_state = _backward(hw);
break;
case MOVE_LEFT:
_state = _left(hw);
break;
case MOVE_RIGHT:
_state = _right(hw);
break;
case HEAD_LEFT:
_state = _headLeft(hw);
break;
case HEAD_CENTER:
_state = _headCenter(hw);
break;
case HEAD_RIGHT:
_state = _headRight(hw);
break;
case MEASURE_DISTANCE:
_state = _measureDistance(hw);
break;
case WHICH_WAY_SLCT:
_state = _whichWay(hw);
break;
case STUCK_FRONT:
_state = _stuckFront(hw);
break;
case STUCK_BACK:
_state = _stuckBack(hw);
break;
case DELAY_FWD_HEAD:
_state = _delayForwardToHead(hw);
break;
case DELAY_BWD_HEAD:
_state = _delayBackwardToHead(hw);
break;
case ERROR:
hw.stopMoving();
auto_reset(); // reset state-machine due to error
break;
default:
hw.stopMoving();
auto_reset(); //something broke, reset state-machine
break;
}
}
