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); }