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