Slightly altered version of Arduino OneStep
This library is specific to the ST L6474 stepper driver.
Diff: OneStep.cpp
- Revision:
- 0:92e1b5622620
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/OneStep.cpp Wed Sep 02 15:43:40 2015 +0000 @@ -0,0 +1,458 @@ +/** + * @file OneStep.cpp + * + * @author Jon Buckman + * + * @section LICENSE + * + * Copyright (c) 2014 Jon Buckman + * + * Copyright (C) 2009-2013 Mike McCauley + * $Id: AccelStepper.cpp,v 1.19 2014/10/31 06:05:27 mikem Exp mikem $ + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * @section DESCRIPTION + * + * OneStep stepper motor accelerate and manipulate. + * + * Datasheet: + * + * + */ + +/** + * Includes + */ +#include "OneStep.h" + +/** + * Methods + */ +OneStep::OneStep(PinName mosi, + PinName miso, + PinName sck, + PinName csn, + PinName step, + PinName dir, + PinName reset, + PinName flag) : spi_(mosi, miso, sck), nCS_(csn), step_(step), dir_(dir), reset_(reset), flag_(flag) // step, direction, reset, flag +{ + //5MHz, see page 13 of datasheet for max clock. May need to be reduces if using long wires + spi_.frequency(5000000); + spi_.format(8,3); + + nCS_ = 1; + + wait_us(500); + + _currentPos = 0; + _targetPos = 0; + _speed = 0.0; + _maxSpeed = 1.0; + _acceleration = 0.0; + _sqrt_twoa = 1.0; + _stepInterval = 0; + _minPulseWidth = 1; + _lastStepTime = 0; + + // NEW + _n = 0; + _c0 = 0.0; + _cn = 0.0; + _cmin = 1.0; + _direction = dir_ = DIRECTION_CCW; + reset_ = 0; + t_.start(); +} + +// No operation. +void OneStep::nop(void) { + wait_us(1); + nCS_ = 0; + spi_.write(0x00); + nCS_ = 1; +} + +// Enable driver. +void OneStep::enable(void) { + wait_us(1); + nCS_ = 0; + spi_.write(ENBL); + nCS_ = 1; +} + +// Disable driver. +void OneStep::disable(void) { + wait_us(1); + nCS_ = 0; + spi_.write(DSBL); + nCS_ = 1; +} + +// Set parameter value; +void OneStep::set_param(char parameter, int length, int value) { + char val[3]; + switch (length) { + case 1: + val[0] = (value & 0x0000ffUL) ; + break; + case 2: + val[0] = (value & 0x00ff00UL) >> 8; + val[1] = (value & 0x0000ffUL) ; + break; + case 3: + val[0] = (value & 0xff0000UL) >> 16; + val[1] = (value & 0x00ff00UL) >> 8; + val[2] = (value & 0x0000ffUL) ; + break; + } + wait_us(1); + nCS_ = 0; + spi_.write(parameter); + nCS_ = 1; + for (int i = 0; i < length; i++) { + wait_us(1); + nCS_ = 0; + spi_.write(val[i]); + nCS_ = 1; + } +} + +// Get parameter value. +int OneStep::get_param(char parameter, int length) { + char output = 0x20 | parameter; + char buf[3] = {0, 0, 0}; + wait_us(1); + nCS_ = 0; + spi_.write(output); + nCS_ = 1; + for (int i = 0; i < length; i++) { + wait_us(1); + nCS_ = 0; + buf[i] = spi_.write(0x00); + nCS_ = 1; + } + switch (length) { + case 1: + return buf[0]; + case 2: + return buf[0] << 8 | buf[1]; + case 3: + return buf[0] << 16 | buf[1] << 8 | buf[2]; + } + return 0; +} + +// Get status. +int OneStep::get_status() { + int ret_bytes[2]; + wait_us(1); + nCS_ = 0; + spi_.write(0xD0); //write request for status return + nCS_ = 1; + wait_us(1); + nCS_ = 0; + ret_bytes[0] = spi_.write(0x00); //read first byte + nCS_ = 1; + wait_us(1); + nCS_ = 0; + ret_bytes[1] = spi_.write(0x00); //read second byte + nCS_ = 1; + return ret_bytes[0] << 8 | ret_bytes[1]; //return i16 response +} + +void OneStep::moveTo(long absolute) +{ + if (_targetPos != absolute) + { + _targetPos = absolute; + computeNewSpeed(); + } +} + +void OneStep::move(long relative) +{ + moveTo(_currentPos + relative); +} + +// Implements steps according to the current step interval. +// You must call this at least once per step +// returns true if a step occurred. +bool OneStep::runSpeed() +{ + // Dont do anything unless we actually have a step interval + if (!_stepInterval) + return false; + + unsigned long time = t_.read_us(); + // Gymnastics to detect wrapping of either the nextStepTime and/or the current time + unsigned long nextStepTime = _lastStepTime + _stepInterval; + if ( ((nextStepTime >= _lastStepTime) && ((time >= nextStepTime) || (time < _lastStepTime))) + || ((nextStepTime < _lastStepTime) && ((time >= nextStepTime) && (time < _lastStepTime)))) + + { + if (_direction == DIRECTION_CW) + { + // Clockwise + _currentPos += 1; + } + else + { + // Anticlockwise + _currentPos -= 1; + } + step(); + + _lastStepTime = time; + return true; + } + else + { + return false; + } +} + +long OneStep::distanceToGo() +{ + return _targetPos - _currentPos; +} + +long OneStep::targetPosition() +{ + return _targetPos; +} + +long OneStep::currentPosition() +{ + return _currentPos; +} + +// Useful during initialisations or after initial positioning +// Sets speed to 0 +void OneStep::setCurrentPosition(long position) +{ + _targetPos = _currentPos = position; + _n = 0; + _stepInterval = 0; +} + +void OneStep::computeNewSpeed() +{ + long distanceTo = distanceToGo(); // +ve is clockwise from curent location + + long stepsToStop = (long)((_speed * _speed) / (2.0f * _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 = dir_ = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW; + } + else + { + // Subsequent step. Works for accel (n is +_ve) and decel (n is -ve). + _cn = _cn - ((2.0f * _cn) / ((4.0f * _n) + 1)); // Equation 13 + _cn = max(_cn, _cmin); + } + _n++; + _stepInterval = _cn; + _speed = 1000000.0f / _cn; + if (_direction == DIRECTION_CCW) + _speed = -_speed; +} + +// 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. +bool OneStep::run() +{ + if (runSpeed()) + computeNewSpeed(); + return _speed != 0.0f || distanceToGo() != 0; +} + +// Step once. +void OneStep::step() +{ + step_ = 1; + wait_us(5); + step_ = 0; +} + +// Set the max speed. +void OneStep::setMaxSpeed(float speed) +{ + if (_maxSpeed != speed) + { + _maxSpeed = speed; + _cmin = 1000000.0f / speed; + // Recompute _n from current speed and adjust speed if accelerating or cruising + if (_n > 0) + { + _n = (long)((_speed * _speed) / (2.0f * _acceleration)); // Equation 16 + computeNewSpeed(); + } + } +} + +// Set the acceleration. +void OneStep::setAcceleration(float acceleration) +{ + if (acceleration == 0.0f) + return; + if (_acceleration != acceleration) + { + // Recompute _n per Equation 17 + _n = _n * (_acceleration / acceleration); + // New c0 per Equation 7 + //_c0 = sqrt(2.0 / acceleration) * 1000000.0; // Accelerates at half the expected rate. Why? + _c0 = sqrt(1.0f/acceleration) * 1000000.0f; + _acceleration = acceleration; + computeNewSpeed(); + } +} + +// Set the speed. +void OneStep::setSpeed(float speed) +{ + if (speed == _speed) + return; + speed = constrain(speed, -_maxSpeed, _maxSpeed); + if (speed == 0.0f) + _stepInterval = 0; + else + { + _stepInterval = fabs(1000000.0f / speed); + _direction = dir_ = (speed > 0.0f) ? DIRECTION_CW : DIRECTION_CCW; + } + _speed = speed; +} + +// Get current speed. +float OneStep::speed() +{ + return _speed; +} + +// Set the minimum pulse width. +void OneStep::setMinPulseWidth(unsigned int minWidth) +{ + _minPulseWidth = minWidth; +} + +// System reset enable. +void OneStep::enableReset() +{ + reset_ = 0; +} + +// System reset disable. +void OneStep::disableReset() +{ + reset_ = 1; +} + +// Blocks until the target position is reached and stopped. +void OneStep::runToPosition() +{ + while (run()) + ; +} + +// Run at speed to a position. +bool OneStep::runSpeedToPosition() +{ + if (_targetPos == _currentPos) + return false; + if (_targetPos >_currentPos) + _direction = dir_ = DIRECTION_CW; + else + _direction = dir_ = DIRECTION_CCW; + return runSpeed(); +} + +// Blocks until the new target position is reached. +void OneStep::runToNewPosition(long position) +{ + moveTo(position); + runToPosition(); +} + +void OneStep::stop() +{ + if (_speed != 0.0f) + { + long stepsToStop = (long)((_speed * _speed) / (2.0f * _acceleration)) + 1; // Equation 16 (+integer rounding) + if (_speed > 0) + move(stepsToStop); + else + move(-stepsToStop); + } +} + +// Get the alarm flag state. +int OneStep::getAlarm() +{ + return(flag_); +}