float MeStepper::desiredSpeed()
{
long distanceTo = distanceToGo();
float requiredSpeed;
if (distanceTo == 0)
return 0.0;
else if (distanceTo > 0)
requiredSpeed = sqrt(2.0 * distanceTo * _acceleration);
else
requiredSpeed = -sqrt(2.0 * -distanceTo * _acceleration);
if (requiredSpeed > _speed)
{
if (_speed == 0)
requiredSpeed = sqrt(2.0 * _acceleration);
else
requiredSpeed = _speed + abs(_acceleration / _speed);
if (requiredSpeed > _maxSpeed)
requiredSpeed = _maxSpeed;
}
else if (requiredSpeed < _speed)
{
if (_speed == 0)
requiredSpeed = -sqrt(2.0 * _acceleration);
else
requiredSpeed = _speed - abs(_acceleration / _speed);
if (requiredSpeed < -_maxSpeed)
requiredSpeed = -_maxSpeed;
}
// Serial.println(requiredSpeed);
return requiredSpeed;
}
void changeLength(float tA, float tB)
{
float currSpeedA = speed(&motorA);
float currSpeedB = speed(&motorB);
setSpeed(&motorA,0.0);
setSpeed(&motorB,0.0);
moveTo(&motorA,tA);
moveTo(&motorB,tB);
if (!usingAcceleration)
{
// The moveTo() function changes the speed in order to do a proper
// acceleration. This counteracts it. Ha.
if (speed(&motorA) < 0)
currSpeedA = -currSpeedA;
if (speed(&motorB) < 0)
currSpeedB = -currSpeedB;
setSpeed(&motorA,currSpeedA);
setSpeed(&motorB,currSpeedB);
}
while (distanceToGo(&motorA) != 0 || distanceToGo(&motorB) != 0)
{
//impl_runBackgroundProcesses();
if (usingAcceleration)
{
run(&motorA);
run(&motorB);
}
else
{
//Serial.print("Run speed..");
//Serial.println(motorA.speed());
runSpeedToPosition(&motorA);
runSpeedToPosition(&motorB);
}
}
}
// Work out and return a new speed.
// Subclasses can override if they want
// Implement acceleration, deceleration and max speed
// Negative speed is anticlockwise
// This is called:
// after each step
// after user changes:
// maxSpeed
// acceleration
// target position (relative or absolute)
float AccelStepper::desiredSpeed()
{
float requiredSpeed;
long distanceTo = distanceToGo();
// Max possible speed that can still decelerate in the available distance
// Use speed squared to avoid using sqrt
if (distanceTo == 0)
return 0.0f; // We're there
else if (distanceTo > 0) // Clockwise
requiredSpeed = (2.0f * distanceTo * _acceleration);
else // Anticlockwise
requiredSpeed = -(2.0f * -distanceTo * _acceleration);
float sqrSpeed = (_speed * _speed) * ((_speed > 0.0f) ? 1.0f : -1.0f);
if (requiredSpeed > sqrSpeed)
{
if (_speed == _maxSpeed) // Reduces processor load by avoiding extra calculations below
{
// Maintain max speed
requiredSpeed = _maxSpeed;
}
else
{
// Need to accelerate in clockwise direction
if (_speed == 0.0f)
requiredSpeed = sqrt(2.0f * _acceleration);
else
requiredSpeed = _speed + abs(_acceleration / _speed);
if (requiredSpeed > _maxSpeed)
requiredSpeed = _maxSpeed;
}
}
else if (requiredSpeed < sqrSpeed)
{
if (_speed == -_maxSpeed) // Reduces processor load by avoiding extra calculations below
{
// Maintain max speed
requiredSpeed = -_maxSpeed;
}
else
{
// Need to accelerate in clockwise direction
if (_speed == 0.0f)
requiredSpeed = -sqrt(2.0f * _acceleration);
else
requiredSpeed = _speed - abs(_acceleration / _speed);
if (requiredSpeed < -_maxSpeed)
requiredSpeed = -_maxSpeed;
}
}
else // if (requiredSpeed == sqrSpeed)
requiredSpeed = _speed;
// Serial.println(requiredSpeed);
return requiredSpeed;
}
void moveAxis(AccelStepper *m, int dist)
{
move(m,dist);
while (distanceToGo(m) != 0)
{
//impl_runBackgroundProcesses();
run(m);
}
//lastOperationTime = millis();
}
/**
* \par Function
* run
* \par Description
* Stepper's status----run or not.
* \param[in]
* None
* \par Output
* None
* \par Return
* Return the status.
* \par Others
* None
*/
boolean MeStepper::run()
{
if((_speed == 0.0) || (distanceToGo() == 0))
{
return false;
}
if (runSpeed())
{
computeNewSpeed();
return true;
}
}
void AsyncDriver::debug(){
volatile long distanceTo = distanceToGo();
volatile long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16
Serial.println("-");
Serial.println(_n);
Serial.println(_speed);
Serial.println(_acceleration);
Serial.println(_cn);
Serial.println(_c0);
Serial.println(stepInterval);
Serial.println(distanceTo);
Serial.println(stepsToStop);
Serial.println("-");
}
/**
* \par Function
* update
* \par Description
* The Stepper loop function, used to move the stepper.
* \param[in]
* None
* \par Output
* None
* \par Return
* None
* \par Others
* None
*/
void MeStepperOnBoard::update(void)
{
if(_mode == 0)
{
runSpeed();
}
else
{
long dist = distanceToGo();
if(dist==0)
{
if(_moving)
{
_moving = false;
_callback(_slot, _extId);
}
}
runSpeedToPosition();
}
}
// Work out and return a new speed.
// Subclasses can override if they want
// Implement acceleration, deceleration and max speed
// Negative speed is anticlockwise
// This is called:
// after each step
// after user changes:
// maxSpeed
// acceleration
// target position (relative or absolute)
float AccelStepper::desiredSpeed()
{
float requiredSpeed;
long distanceTo = distanceToGo(); // +ve is clockwise from curent location
if (distanceTo == 0)
return 0.0f; // We're there
// sqrSpeed is the signed square of _speed.
float sqrSpeed = sq(_speed);
if (_speed < 0.0)
sqrSpeed = -sqrSpeed;
float twoa = 2.0 * _acceleration;
// Ensure we dont get massive acceleration when _speed is very small
// _sqrt_twoa is precomputed when _acceleration is set
float delta_speed = fabs(_acceleration / _speed);
if (delta_speed > _sqrt_twoa)
delta_speed = _sqrt_twoa;
// if v^^2/2as is the the left of target, we will arrive at 0 speed too far -ve, need to accelerate clockwise
if ((sqrSpeed / twoa) < distanceTo)
{
// Accelerate clockwise
// Need to accelerate in clockwise direction
requiredSpeed = _speed + delta_speed;
if (requiredSpeed > _maxSpeed)
requiredSpeed = _maxSpeed;
}
else
{
// Decelerate clockwise, accelerate anticlockwise
// Need to accelerate in clockwise direction
requiredSpeed = _speed - delta_speed;
if (requiredSpeed < -_maxSpeed)
requiredSpeed = -_maxSpeed;
}
// Serial.println(requiredSpeed);
return requiredSpeed;
}
// Work out and return a new speed.
// Subclasses can override if they want
// Implement acceleration, deceleration and max speed
// Negative speed is anticlockwise
// This is called:
// after each step
// after user changes:
// maxSpeed
// acceleration
// target position (relative or absolute)
float AccelStepper::desiredSpeed()
{
long distanceTo = distanceToGo();
// Max possible speed that can still decelerate in the available distance
float requiredSpeed;
if (distanceTo == 0)
return 0.0; // Were there
else if (distanceTo > 0) // Clockwise
requiredSpeed = sqrt(2.0 * distanceTo * _acceleration);
else // Anticlockwise
requiredSpeed = -sqrt(2.0 * -distanceTo * _acceleration);
if (requiredSpeed > _speed)
{
// Need to accelerate in clockwise direction
if (_speed == 0)
requiredSpeed = sqrt(2.0 * _acceleration);
else
requiredSpeed = _speed + abs(_acceleration / _speed);
if (requiredSpeed > _maxSpeed)
requiredSpeed = _maxSpeed;
}
else if (requiredSpeed < _speed)
{
// Need to accelerate in anticlockwise direction
if (_speed == 0)
requiredSpeed = -sqrt(2.0 * _acceleration);
else
requiredSpeed = _speed - abs(_acceleration / _speed);
if (requiredSpeed < -_maxSpeed)
requiredSpeed = -_maxSpeed;
}
// Serial.println(requiredSpeed);
return requiredSpeed;
}
// Run the motor to implement speed and acceleration in order to proceed to the target position
// You must call this at least once per step, preferably in your main loop
// If the motor is in the desired position, the cost is very small
// returns true if the motor is still running to the target position.
uint8_t run(Stepper_t* motor)
{
if (runSpeed(motor)) computeNewSpeed(motor);
return motor->_speed != 0.0 || distanceToGo(motor) != 0;
}
void computeNewSpeed(Stepper_t* motor)
{
long distanceTo = distanceToGo(motor); // +ve is clockwise from curent location
long stepsToStop = (long)((motor->_speed * motor->_speed) / (2.0 * motor->_acceleration)); // Equation 16
if (distanceTo == 0 && stepsToStop <= 1)
{
// We are at the target and its time to stop
motor->_stepInterval = 0;
motor->_speed = 0.0;
motor->_n = 0;
return;
}
if (distanceTo > 0)
{
// We are anticlockwise from the target
// Need to go clockwise from here, maybe decelerate now
if (motor->_n > 0)
{
// Currently accelerating, need to decel now? Or maybe going the wrong way?
if ((stepsToStop >= distanceTo) || motor->_direction == DIRECTION_CCW)
motor->_n = -stepsToStop; // Start deceleration
}
else if (motor->_n < 0)
{
// Currently decelerating, need to accel again?
if ((stepsToStop < distanceTo) && motor->_direction == DIRECTION_CW)
motor->_n = -motor->_n; // Start accceleration
}
}
else if (distanceTo < 0)
{
// We are clockwise from the target
// Need to go anticlockwise from here, maybe decelerate
if (motor->_n > 0)
{
// Currently accelerating, need to decel now? Or maybe going the wrong way?
if ((stepsToStop >= -distanceTo) || motor->_direction == DIRECTION_CW)
motor->_n = -stepsToStop; // Start deceleration
}
else if (motor->_n < 0)
{
// Currently decelerating, need to accel again?
if ((stepsToStop < -distanceTo) && motor->_direction == DIRECTION_CCW)
motor->_n = -motor->_n; // Start accceleration
}
}
// Need to accelerate or decelerate
if (motor->_n == 0)
{
// First step from stopped
motor->_cn = motor->_c0;
motor->_direction = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW;
}
else
{
// Subsequent step. Works for accel (n is +_ve) and decel (n is -ve).
motor->_cn = motor->_cn - ((2.0 * motor->_cn) / ((4.0 * motor->_n) + 1)); // Equation 13
motor->_cn = (motor->_cn > motor->_cmin) ? motor->_cn : motor->_cmin; //max(motor->_cn, motor->_cmin);
}
motor->_n++;
motor->_stepInterval = motor->_cn;
motor->_speed = 1000000.0 / motor->_cn;
if (motor->_direction == DIRECTION_CCW)
motor->_speed = -motor->_speed;
#ifdef DEBUG_MODE
printf("%f\n", motor->_speed);
printf("%f\n", motor->_acceleration);
printf("%f\n", motor->_cn);
printf("%f\n", motor->_c0);
printf("%ld\n", motor->_n);
printf("%lu\n", motor->_stepInterval);
// Serial.println(distanceTo);
// Serial.println(stepsToStop);
#endif
}
/**
* \par Function
* computeNewSpeed
* \par Description
* Compute New Speed of Stepper.
* \param[in]
* None
* \par Output
* None
* \par Return
* None
* \par Others
* None
*/
void MeStepperOnBoard::computeNewSpeed(void)
{
long distanceTo = distanceToGo();
long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration));
if (distanceTo == 0 && stepsToStop <= 1)
{
// We are at the target and its time to stop
_stepInterval = 0;
_speed = 0.0;
_n = 0;
return;
}
if (distanceTo > 0)
{
// We are anticlockwise from the target
// Need to go clockwise from here, maybe decelerate now
if (_n > 0)
{
// Currently accelerating, need to decel now? Or maybe going the wrong way?
if ((stepsToStop >= distanceTo) || _dir == DIRECTION_CCW)
{
_n = -stepsToStop; // Start deceleration
}
}
else if (_n < 0)
{
// Currently decelerating, need to accel again?
if ((stepsToStop < distanceTo) && _dir == DIRECTION_CW)
{
_n = -_n; // Start accceleration
}
}
}
else if (distanceTo < 0)
{
// We are clockwise from the target
// Need to go anticlockwise from here, maybe decelerate
if (_n > 0)
{
// Currently accelerating, need to decel now? Or maybe going the wrong way?
if ((stepsToStop >= -distanceTo) || _dir == DIRECTION_CW)
{
_n = -stepsToStop; // Start deceleration
}
}
else if (_n < 0)
{
// Currently decelerating, need to accel again?
if ((stepsToStop < -distanceTo) && _dir == DIRECTION_CCW)
{
_n = -_n; // Start accceleration
}
}
}
// Need to accelerate or decelerate
if (_n == 0)
{
// First step from stopped
_cn = _c0;
_dir = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW;
}
else
{
// Subsequent step. Works for accel (n is +_ve) and decel (n is -ve).
_cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13
_cn = max(_cn, _cmin);
}
_n++;
_stepInterval = _cn;
_speed = 1000000.0 / _cn;
if (_dir == DIRECTION_CCW)
{
_speed = -_speed;
}
}
// Blocks until the target position is reached and stopped
void AccelStepper::runToPosition()
{
while (_speed != 0 || distanceToGo() != 0)
run();
}
// Run the motor to implement speed and acceleration in order to proceed to the target position
// You must call this at least once per step, preferably in your main loop
// If the motor is in the desired position, the cost is very small
// returns true if the motor is still running to the target position.
boolean AccelStepper::run()
{
if (runSpeed())
computeNewSpeed();
return ((_speed != 0.0) || (distanceToGo() != 0));
}
void AccelStepper::computeNewSpeed()
{
long distanceTo = distanceToGo(); // +ve is clockwise from curent location
long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16
if (distanceTo == 0 && stepsToStop <= 1)
{
// We are at the target and its time to stop
_stepInterval = 0;
_speed = 0.0;
_n = 0;
return;
}
if (distanceTo > 0)
{
// We are anticlockwise from the target
// Need to go clockwise from here, maybe decelerate now
if (_n > 0)
{
// Currently accelerating, need to decel now? Or maybe going the wrong way?
if ((stepsToStop >= distanceTo) || _direction == DIRECTION_CCW)
_n = -stepsToStop; // Start deceleration
}
else if (_n < 0)
{
// Currently decelerating, need to accel again?
if ((stepsToStop < distanceTo) && _direction == DIRECTION_CW)
_n = -_n; // Start accceleration
}
}
else if (distanceTo < 0)
{
// We are clockwise from the target
// Need to go anticlockwise from here, maybe decelerate
if (_n > 0)
{
// Currently accelerating, need to decel now? Or maybe going the wrong way?
if ((stepsToStop >= -distanceTo) || _direction == DIRECTION_CW)
_n = -stepsToStop; // Start deceleration
}
else if (_n < 0)
{
// Currently decelerating, need to accel again?
if ((stepsToStop < -distanceTo) && _direction == DIRECTION_CCW)
_n = -_n; // Start accceleration
}
}
// Need to accelerate or decelerate
if (_n == 0)
{
// First step from stopped
_cn = _c0;
_direction = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW;
}
else
{
// Subsequent step. Works for accel (n is +_ve) and decel (n is -ve).
_cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13
_cn = max(_cn, _cmin);
}
_n++;
_stepInterval = _cn;
_speed = TIMER_RESOLUTION / _cn;
if (_direction == DIRECTION_CCW)
_speed = -_speed;
#if 0
Serial.println(_speed);
Serial.println(_acceleration);
Serial.println(_cn);
Serial.println(_c0);
Serial.println(_n);
Serial.println(_stepInterval);
Serial.println(distanceTo);
Serial.println(stepsToStop);
Serial.println("-----");
#endif
}
// Run the motor to implement speed and acceleration in order to proceed to the target position
// You must call this at least once per step, preferably in your main loop
// If the motor is in the desired position, the cost is very small
// returns true if the motor is still running to the target position.
boolean AccelStepper::run()
{
if (runSpeed())
computeNewSpeed();
return _speed != 0.0 || distanceToGo() != 0;
}
void AccelStepper::run()
{
float curtime = float(millis())/1000.0f;
float elapsed = curtime-mPrevTime;
//printf( "curtime<%f> prv<%f> ela<%f>\n", curtime, mPrevTime, elapsed );
mPrevTime = curtime;
float distance = distanceToGo();
float dpos = 0.0f;
if( mDirection==-1.0f )
{
if( distance>0.0 )
{ //printf( "Stepper<%s> reached target dist<%f> \n", mName.c_str(), distance );
mCurrentSpeed = 0.0f;
mCurrentPosition = mTargetPosition;
mDirection = 0.0f;
resetTimer();
}
else
{
mCurrentSpeed = -mMaxSpeed;
dpos = mCurrentSpeed*elapsed;
mCurrentPosition += dpos;
mPhysActualPosition += dpos;
if( mPhysActualPosition<0.0f )
{ mCurrentPosition-=mPhysActualPosition;
mPhysActualPosition-=mPhysActualPosition;
printf( "OUTOFBOUNDS!!!!\n");
}
}
}
else if( mDirection==1.0f)
{
if( distance<0.0 )
{ //printf( "Stepper<%s> reached target dist<%d>\n", mName.c_str(), distance );
mCurrentSpeed = 0.0f;
mCurrentPosition = mTargetPosition;
mDirection = 0.0f;
resetTimer();
}
else
{
mCurrentSpeed = +mMaxSpeed;
dpos = mCurrentSpeed*elapsed;
mCurrentPosition += dpos;
mPhysActualPosition += dpos;
if( mPhysActualPosition>mPhysMaximumPosition )
{ float overshoot = mPhysActualPosition-mPhysMaximumPosition;
static int count = 0;
count++;
printf( "OUTOFBOUNDS!!!! physAct<%f> physmax<%f>\n", mPhysActualPosition, mPhysMaximumPosition );
mCurrentPosition-=overshoot;
mPhysActualPosition-=overshoot;
assert(count<10);
}
else if( mCurrentPosition>mTargetPosition )
{ float overshoot = mCurrentPosition-mTargetPosition;
mCurrentPosition-=overshoot;
mPhysActualPosition-=overshoot;
}
}
}
else
{
//printf( "wtf\n" );
mCurrentSpeed = 0.0f;
}
mPrevDistance = distance;
if( (curtime-mPrevDumpTime) > 3.0 )
{
mPrevDumpTime = curtime;
dumpState();
}
if( (curtime-mPrevDumpTime) > 0.03 )
{
StepperVizState vstate;
vstate.mName = mName;
vstate.mSize = mPhysMaximumPosition;
vstate.mPhysicalPos = mPhysActualPosition;
vstate.mPhysicalMax = mPhysMaximumPosition;
vstate.mCurrentPos = mCurrentPosition;
vstate.mTargetPos = mTargetPosition;
vstate.mEnabled = mEnabled;
vstate.mSpindleSpeed = gSpindleSpeed;
UpdateStepperViz(vstate);
}
usleep(1000);
}
