File content as of revision 0:92e1b5622620:

/**
 * @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_);
}