Revision 3:086f8c1079ff, committed 2018-05-07

Comitter:

yuliyasm

Date:

Mon May 07 11:41:10 2018 +0000

Parent:

2:d4dad64faadb

Commit message:

4

Changed in this revision

--- a/AccelStepper.cpp	Sat Feb 03 19:25:25 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,651 +0,0 @@
-// AccelStepper.cpp
-//
-// Copyright (C) 2009-2013 Mike McCauley
-// $Id: AccelStepper.cpp,v 1.19 2014/10/31 06:05:27 mikem Exp mikem $
-
-#include "AccelStepper.h"
-
-#if 0
-// Some debugging assistance
-void dump(uint8_t* p, int l)
-{
-    int i;
-
-    for (i = 0; i < l; i++)
-    {
-	Serial.print(p[i], HEX);
-	Serial.print(" ");
-    }
-    Serial.println("");
-}
-#endif
-
-void AccelStepper::moveTo(long absolute)
-{
-    if (_targetPos != absolute)
-    {
-	_targetPos = absolute;
-	computeNewSpeed();
-	// compute new n?
-    }
-}
-
-void AccelStepper::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 AccelStepper::runSpeed()
-{
-	extern Timer t;
-    // Dont do anything unless we actually have a step interval
-    if (!_stepInterval)
-	return false;
-
-    //unsigned long time = micros();
-	unsigned long time = t.read_us();
-    unsigned long nextStepTime = _lastStepTime + _stepInterval;
-    // Gymnastics to detect wrapping of either the nextStepTime and/or the current time
-    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(_currentPos);
-
-	_lastStepTime = time;
-	return true;
-    }
-    else
-    {
-	return false;
-    }
-}
-
-long AccelStepper::distanceToGo()
-{
-    return _targetPos - _currentPos;
-}
-
-long AccelStepper::targetPosition()
-{
-    return _targetPos;
-}
-
-long AccelStepper::currentPosition()
-{
-    return _currentPos;
-}
-
-// Useful during initialisations or after initial positioning
-// Sets speed to 0
-void AccelStepper::setCurrentPosition(long position)
-{
-    _targetPos = _currentPos = position;
-    _n = 0;
-    _stepInterval = 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 = 1000000.0 / _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.
-bool AccelStepper::run()
-{
-    if (runSpeed())
-	  computeNewSpeed();
-    return _speed != 0.0 || distanceToGo() != 0;
-}
-
-AccelStepper::AccelStepper(uint8_t interface, PinName pin1, PinName pin2, PinName pin3, PinName pin4, bool enable)
-{
-    _interface = interface;
-    _currentPos = 0;
-    _targetPos = 0;
-    _speed = 0.0;
-    _maxSpeed = 1.0;
-    _acceleration = 0.0;
-    _sqrt_twoa = 1.0;
-    _stepInterval = 0;
-    _minPulseWidth = 1;
-    _lastStepTime = 0;
-	// _pin[0] = pin1;
-	// _pin[1] = pin2;
-	// _pin[2] = pin3;
-	// _pin[3] = pin4;
-    _pin0 = new DigitalOut(pin1);
-    _pin1 = new DigitalOut(pin2);
-    _pin2 = new DigitalOut(pin3);
-    _pin3 = new DigitalOut(pin4);
-
-    // NEW
-    _n = 0;
-    _c0 = 0.0;
-    _cn = 0.0;
-    _cmin = 1.0;
-    _direction = DIRECTION_CCW;
-
-    int i;
-    for (i = 0; i < 4; i++)
-	_pinInverted[i] = 0;
-    if (enable)
-	enableOutputs();
-    // Some reasonable default
-    setAcceleration(1);
-}
-
-AccelStepper::AccelStepper(void (*forward)(), void (*backward)())
-{
-    _interface = 0;
-    _currentPos = 0;
-    _targetPos = 0;
-    _speed = 0.0;
-    _maxSpeed = 1.0;
-    _acceleration = 0.0;
-    _sqrt_twoa = 1.0;
-    _stepInterval = 0;
-    _minPulseWidth = 1;
-    _lastStepTime = 0;
-    _forward = forward;
-    _backward = backward;
-
-    // NEW
-    _n = 0;
-    _c0 = 0.0;
-    _cn = 0.0;
-    _cmin = 1.0;
-    _direction = DIRECTION_CCW;
-
-    int i;
-    for (i = 0; i < 4; i++)
-	_pinInverted[i] = 0;
-    // Some reasonable default
-    setAcceleration(1);
-}
-
-void AccelStepper::setMaxSpeed(float speed)
-{
-    if (_maxSpeed != speed)
-    {
-	_maxSpeed = speed;
-	_cmin = 1000000.0 / speed;
-	// Recompute _n from current speed and adjust speed if accelerating or cruising
-	if (_n > 0)
-	{
-	    _n = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16
-	    computeNewSpeed();
-	}
-    }
-}
-
-void AccelStepper::setAcceleration(float acceleration)
-{
-    if (acceleration == 0.0)
-	return;
-    if (_acceleration != acceleration)
-    {
-	// Recompute _n per Equation 17
-	_n = _n * (_acceleration / acceleration);
-	// New c0 per Equation 7, with correction per Equation 15
-	_c0 = 0.676 * sqrt(2.0 / acceleration) * 1000000.0; // Equation 15
-	_acceleration = acceleration;
-	computeNewSpeed();
-    }
-}
-
-void AccelStepper::setSpeed(float speed)
-{
-    if (speed == _speed)
-        return;
-    speed = constrain(speed, -_maxSpeed, _maxSpeed);
-    if (speed == 0.0)
-	_stepInterval = 0;
-    else
-    {
-	_stepInterval = fabs(1000000.0 / speed);
-	_direction = (speed > 0.0) ? DIRECTION_CW : DIRECTION_CCW;
-    }
-    _speed = speed;
-}
-
-float AccelStepper::speed()
-{
-    return _speed;
-}
-
-// Subclasses can override
-void AccelStepper::step(long step)
-{
-    switch (_interface)
-    {
-        case FUNCTION:
-            step0(step);
-            break;
-
-	case DRIVER:
-	    step1(step);
-	    break;
-    
-	case FULL2WIRE:
-	    step2(step);
-	    break;
-    
-	case FULL3WIRE:
-	    step3(step);
-	    break;  
-
-	case FULL4WIRE:
-	    step4(step);
-	    break;  
-
-	case HALF3WIRE:
-	    step6(step);
-	    break;  
-		
-	case HALF4WIRE:
-	    step8(step);
-	    break;  
-    }
-}
-
-// You might want to override this to implement eg serial output
-// bit 0 of the mask corresponds to _pin[0]
-// bit 1 of the mask corresponds to _pin[1]
-// ....
-void AccelStepper::setOutputPins(uint8_t mask)
-{
-//    uint8_t numpins = 2;
-//    if (_interface == FULL4WIRE || _interface == HALF4WIRE)
-//        numpins = 4;
-//    else if (_interface == FULL3WIRE || _interface == HALF3WIRE)
-//        numpins = 3;
-//    uint8_t i;
-//    for (i = 0; i < numpins; i++)
-//        digitalWrite(_pin[i], (mask & (1 << i)) ? (HIGH ^ _pinInverted[i]) : (LOW ^ _pinInverted[i]));
-	*_pin0 = (mask & (1 << 0)) ? (HIGH ^ _pinInverted[0]) : (LOW ^ _pinInverted[0]);
-	*_pin1 = (mask & (1 << 1)) ? (HIGH ^ _pinInverted[1]) : (LOW ^ _pinInverted[1]);
-    if (_interface == FULL4WIRE || _interface == HALF4WIRE) {
-		*_pin2 = (mask & (1 << 2)) ? (HIGH ^ _pinInverted[2]) : (LOW ^ _pinInverted[2]);
-		*_pin3 = (mask & (1 << 3)) ? (HIGH ^ _pinInverted[3]) : (LOW ^ _pinInverted[3]);
-	}
-    else if (_interface == FULL3WIRE || _interface == HALF3WIRE)
-		*_pin2 = (mask & (1 << 2)) ? (HIGH ^ _pinInverted[2]) : (LOW ^ _pinInverted[2]);
-}
-
-// 0 pin step function (ie for functional usage)
-void AccelStepper::step0(long step)
-{
-  if (_speed > 0)
-    _forward();
-  else
-    _backward();
-}
-
-// 1 pin step function (ie for stepper drivers)
-// This is passed the current step number (0 to 7)
-// Subclasses can override
-void AccelStepper::step1(long step)
-{
-    // _pin[0] is step, _pin[1] is direction
-    setOutputPins(_direction ? 0b10 : 0b00); // Set direction first else get rogue pulses
-    setOutputPins(_direction ? 0b11 : 0b01); // step HIGH
-    // Caution 200ns setup time 
-    // Delay the minimum allowed pulse width
-    //delayMicroseconds(_minPulseWidth);
-    wait_us(_minPulseWidth);
-    setOutputPins(_direction ? 0b10 : 0b00); // step LOW
-
-}
-
-
-// 2 pin step function
-// This is passed the current step number (0 to 7)
-// Subclasses can override
-void AccelStepper::step2(long step)
-{
-    switch (step & 0x3)
-    {
-	case 0: /* 01 */
-	    setOutputPins(0b10);
-	    break;
-
-	case 1: /* 11 */
-	    setOutputPins(0b11);
-	    break;
-
-	case 2: /* 10 */
-	    setOutputPins(0b01);
-	    break;
-
-	case 3: /* 00 */
-	    setOutputPins(0b00);
-	    break;
-    }
-}
-// 3 pin step function
-// This is passed the current step number (0 to 7)
-// Subclasses can override
-void AccelStepper::step3(long step)
-{
-    switch (step % 3)
-    {
-	case 0:    // 100
-	    setOutputPins(0b100);
-	    break;
-
-	case 1:    // 001
-	    setOutputPins(0b001);
-	    break;
-
-	case 2:    //010
-	    setOutputPins(0b010);
-	    break;
-	    
-    }
-}
-
-// 4 pin step function for half stepper
-// This is passed the current step number (0 to 7)
-// Subclasses can override
-void AccelStepper::step4(long step)
-{
-    switch (step & 0x3)
-    {
-	case 0:    // 1010
-	    setOutputPins(0b0101);
-	    break;
-
-	case 1:    // 0110
-	    setOutputPins(0b0110);
-	    break;
-
-	case 2:    //0101
-	    setOutputPins(0b1010);
-	    break;
-
-	case 3:    //1001
-	    setOutputPins(0b1001);
-	    break;
-    }
-}
-
-// 3 pin half step function
-// This is passed the current step number (0 to 7)
-// Subclasses can override
-void AccelStepper::step6(long step)
-{
-    switch (step % 6)
-    {
-	case 0:    // 100
-	    setOutputPins(0b100);
-            break;
-	    
-        case 1:    // 101
-	    setOutputPins(0b101);
-            break;
-	    
-	case 2:    // 001
-	    setOutputPins(0b001);
-            break;
-	    
-        case 3:    // 011
-	    setOutputPins(0b011);
-            break;
-	    
-	case 4:    // 010
-	    setOutputPins(0b010);
-            break;
-	    
-	case 5:    // 011
-	    setOutputPins(0b110);
-            break;
-	    
-    }
-}
-
-// 4 pin half step function
-// This is passed the current step number (0 to 7)
-// Subclasses can override
-void AccelStepper::step8(long step)
-{
-    switch (step & 0x7)
-    {
-	case 0:    // 1000
-	    setOutputPins(0b0001);
-            break;
-	    
-        case 1:    // 1010
-	    setOutputPins(0b0101);
-            break;
-	    
-	case 2:    // 0010
-	    setOutputPins(0b0100);
-            break;
-	    
-        case 3:    // 0110
-	    setOutputPins(0b0110);
-            break;
-	    
-	case 4:    // 0100
-	    setOutputPins(0b0010);
-            break;
-	    
-        case 5:    //0101
-	    setOutputPins(0b1010);
-            break;
-	    
-	case 6:    // 0001
-	    setOutputPins(0b1000);
-            break;
-	    
-        case 7:    //1001
-	    setOutputPins(0b1001);
-            break;
-    }
-}
-    
-// Prevents power consumption on the outputs
-void    AccelStepper::disableOutputs()
-{   
-    if (! _interface) return;
-
-    setOutputPins(0); // Handles inversion automatically
-	// if (_enablePin != 0xff)
-    if (_enablePin)
-        // digitalWrite(_enablePin, LOW ^ _enableInverted);
-        *_enablePin = LOW ^ _enableInverted;
-}
-
-void    AccelStepper::enableOutputs()
-{
-    if (! _interface) 
-	return;
-
-    //pinMode(_pin[0], OUTPUT);
-    //pinMode(_pin[1], OUTPUT);
-    if (_interface == FULL4WIRE || _interface == HALF4WIRE)
-    {
-        //pinMode(_pin[2], OUTPUT);
-        //pinMode(_pin[3], OUTPUT);
-    }
-    else if (_interface == FULL3WIRE || _interface == HALF3WIRE)
-    {
-        //pinMode(_pin[2], OUTPUT);
-    }
-
-    // if (_enablePin != 0xff)
-	if (_enablePin)
-    {
-        //pinMode(_enablePin, OUTPUT);
-        //digitalWrite(_enablePin, HIGH ^ _enableInverted);
-        *_enablePin = HIGH ^ _enableInverted;
-    }
-}
-
-void AccelStepper::setMinPulseWidth(unsigned int minWidth)
-{
-    _minPulseWidth = minWidth;
-}
-
-// void AccelStepper::setEnablePin(uint8_t enablePin)
-void AccelStepper::setEnablePin(PinName enablePin)
-{
-    // _enablePin = enablePin;
-	_enablePin = new DigitalOut(enablePin);
-
-    // This happens after construction, so init pin now.
-	// if (_enablePin != 0xff)
-    if (*_enablePin)
-    {
-        //pinMode(_enablePin, OUTPUT);
-        //digitalWrite(_enablePin, HIGH ^ _enableInverted);
-        *_enablePin = HIGH ^ _enableInverted;
-    }
-}
-
-void AccelStepper::setPinsInverted(bool directionInvert, bool stepInvert, bool enableInvert)
-{
-    _pinInverted[0] = stepInvert;
-    _pinInverted[1] = directionInvert;
-    _enableInverted = enableInvert;
-}
-
-void AccelStepper::setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert)
-{    
-    _pinInverted[0] = pin1Invert;
-    _pinInverted[1] = pin2Invert;
-    _pinInverted[2] = pin3Invert;
-    _pinInverted[3] = pin4Invert;
-    _enableInverted = enableInvert;
-}
-
-// Blocks until the target position is reached and stopped
-void AccelStepper::runToPosition()
-{
-    while (run())
-	;
-}
-
-bool AccelStepper::runSpeedToPosition()
-{
-    if (_targetPos == _currentPos)
-	return false;
-    if (_targetPos >_currentPos)
-	_direction = DIRECTION_CW;
-    else
-	_direction = DIRECTION_CCW;
-    return runSpeed();
-}
-
-// Blocks until the new target position is reached
-void AccelStepper::runToNewPosition(long position)
-{
-    moveTo(position);
-    runToPosition();
-}
-
-void AccelStepper::stop()
-{
-    if (_speed != 0.0)
-    {    
-	long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)) + 1; // Equation 16 (+integer rounding)
-	if (_speed > 0)
-	    move(stepsToStop);
-	else
-	    move(-stepsToStop);
-    }
-}
-

--- a/AccelStepper.h	Sat Feb 03 19:25:25 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,680 +0,0 @@
-// AccelStepper.h
-//
-/// \mainpage AccelStepper library for MBED
-///
-/// This is the MBED AccelStepper library.
-/// It provides an object-oriented interface for 2, 3 or 4 pin stepper motors.
-/// Based on the Arduino AccelStepper library by Airspayce.com
-/// Translated for MBED by Jaap Vermaas <jaap@tuxic.nl>, 03-2015
-///
-/// The standard Arduino IDE includes the Stepper library
-/// (http://arduino.cc/en/Reference/Stepper) for stepper motors. It is
-/// perfectly adequate for simple, single motor applications.
-///
-/// AccelStepper significantly improves on the standard Arduino Stepper library in several ways:
-/// \li Supports acceleration and deceleration
-/// \li Supports multiple simultaneous steppers, with independent concurrent stepping on each stepper
-/// \li API functions never delay() or block
-/// \li Supports 2, 3 and 4 wire steppers, plus 3 and 4 wire half steppers.
-/// \li Supports alternate stepping functions to enable support of AFMotor (https://github.com/adafruit/Adafruit-Motor-Shield-library)
-/// \li Supports stepper drivers such as the Sparkfun EasyDriver (based on 3967 driver chip)
-/// \li Very slow speeds are supported
-/// \li Extensive API
-/// \li Subclass support
-///
-/// The latest version of this documentation can be downloaded from 
-/// http://www.airspayce.com/mikem/arduino/AccelStepper
-/// The version of the package that this documentation refers to can be downloaded 
-/// from http://www.airspayce.com/mikem/arduino/AccelStepper/AccelStepper-1.47.zip
-///
-/// Example Arduino programs are included to show the main modes of use.
-///
-/// You can also find online help and discussion at http://groups.google.com/group/accelstepper
-/// Please use that group for all questions and discussions on this topic. 
-/// Do not contact the author directly, unless it is to discuss commercial licensing.
-/// Before asking a question or reporting a bug, please read http://www.catb.org/esr/faqs/smart-questions.html
-///
-/// Tested on Arduino Diecimila and Mega with arduino-0018 & arduino-0021 
-/// on OpenSuSE 11.1 and avr-libc-1.6.1-1.15,
-/// cross-avr-binutils-2.19-9.1, cross-avr-gcc-4.1.3_20080612-26.5.
-/// Tested on Teensy http://www.pjrc.com/teensy including Teensy 3.1 built using Arduino IDE 1.0.5 with 
-/// teensyduino addon 1.18 and later.
-///
-/// \par Installation
-///
-/// Install in the usual way: unzip the distribution zip file to the libraries
-/// sub-folder of your sketchbook. 
-///
-/// \par Theory
-///
-/// This code uses speed calculations as described in 
-/// "Generate stepper-motor speed profiles in real time" by David Austin 
-/// http://fab.cba.mit.edu/classes/MIT/961.09/projects/i0/Stepper_Motor_Speed_Profile.pdf
-/// with the exception that AccelStepper uses steps per second rather than radians per second
-/// (because we dont know the step angle of the motor)
-/// An initial step interval is calculated for the first step, based on the desired acceleration
-/// On subsequent steps, shorter step intervals are calculated based 
-/// on the previous step until max speed is achieved.
-/// 
-/// \par Donations
-///
-/// This library is offered under a free GPL license for those who want to use it that way. 
-/// We try hard to keep it up to date, fix bugs
-/// and to provide free support. If this library has helped you save time or money, please consider donating at
-/// http://www.airspayce.com or here:
-///
-/// \htmlonly <form action="https://www.paypal.com/cgi-bin/webscr" method="post"><input type="hidden" name="cmd" value="_donations" /> <input type="hidden" name="business" value="mikem@airspayce.com" /> <input type="hidden" name="lc" value="AU" /> <input type="hidden" name="item_name" value="Airspayce" /> <input type="hidden" name="item_number" value="AccelStepper" /> <input type="hidden" name="currency_code" value="USD" /> <input type="hidden" name="bn" value="PP-DonationsBF:btn_donateCC_LG.gif:NonHosted" /> <input type="image" alt="PayPal — The safer, easier way to pay online." name="submit" src="https://www.paypalobjects.com/en_AU/i/btn/btn_donateCC_LG.gif" /> <img alt="" src="https://www.paypalobjects.com/en_AU/i/scr/pixel.gif" width="1" height="1" border="0" /></form> \endhtmlonly
-/// 
-/// \par Trademarks
-///
-/// AccelStepper is a trademark of AirSpayce Pty Ltd. The AccelStepper mark was first used on April 26 2010 for
-/// international trade, and is used only in relation to motor control hardware and software.
-/// It is not to be confused with any other similar marks covering other goods and services.
-///
-/// \par Copyright
-///
-/// This software is Copyright (C) 2010 Mike McCauley. Use is subject to license
-/// conditions. The main licensing options available are GPL V2 or Commercial:
-///
-/// \par Open Source Licensing GPL V2
-/// This is the appropriate option if you want to share the source code of your
-/// application with everyone you distribute it to, and you also want to give them
-/// the right to share who uses it. If you wish to use this software under Open
-/// Source Licensing, you must contribute all your source code to the open source
-/// community in accordance with the GPL Version 2 when your application is
-/// distributed. See http://www.gnu.org/copyleft/gpl.html
-/// 
-/// \par Commercial Licensing
-/// This is the appropriate option if you are creating proprietary applications
-/// and you are not prepared to distribute and share the source code of your
-/// application. Contact info@airspayce.com for details.
-///
-/// \par Revision History
-/// \version 1.0 Initial release
-///
-/// \version 1.1 Added speed() function to get the current speed.
-/// \version 1.2 Added runSpeedToPosition() submitted by Gunnar Arndt.
-/// \version 1.3 Added support for stepper drivers (ie with Step and Direction inputs) with _pins == 1
-/// \version 1.4 Added functional contructor to support AFMotor, contributed by Limor, with example sketches.
-/// \version 1.5 Improvements contributed by Peter Mousley: Use of microsecond steps and other speed improvements
-///              to increase max stepping speed to about 4kHz. New option for user to set the min allowed pulse width.
-///              Added checks for already running at max speed and skip further calcs if so. 
-/// \version 1.6 Fixed a problem with wrapping of microsecond stepping that could cause stepping to hang. 
-///              Reported by Sandy Noble.
-///              Removed redundant _lastRunTime member.
-/// \version 1.7 Fixed a bug where setCurrentPosition() did not always work as expected. 
-///              Reported by Peter Linhart.
-/// \version 1.8 Added support for 4 pin half-steppers, requested by Harvey Moon
-/// \version 1.9 setCurrentPosition() now also sets motor speed to 0.
-/// \version 1.10 Builds on Arduino 1.0
-/// \version 1.11 Improvments from Michael Ellison:
-///   Added optional enable line support for stepper drivers
-///   Added inversion for step/direction/enable lines for stepper drivers
-/// \version 1.12 Announce Google Group
-/// \version 1.13 Improvements to speed calculation. Cost of calculation is now less in the worst case, 
-///    and more or less constant in all cases. This should result in slightly beter high speed performance, and
-///    reduce anomalous speed glitches when other steppers are accelerating. 
-///    However, its hard to see how to replace the sqrt() required at the very first step from 0 speed.
-/// \version 1.14 Fixed a problem with compiling under arduino 0021 reported by EmbeddedMan
-/// \version 1.15 Fixed a problem with runSpeedToPosition which did not correctly handle
-///    running backwards to a smaller target position. Added examples
-/// \version 1.16 Fixed some cases in the code where abs() was used instead of fabs().
-/// \version 1.17 Added example ProportionalControl
-/// \version 1.18 Fixed a problem: If one calls the funcion runSpeed() when Speed is zero, it makes steps 
-///    without counting. reported by  Friedrich, Klappenbach.
-/// \version 1.19 Added MotorInterfaceType and symbolic names for the number of pins to use
-///               for the motor interface. Updated examples to suit.
-///               Replaced individual pin assignment variables _pin1, _pin2 etc with array _pin[4].
-///               _pins member changed to _interface.
-///               Added _pinInverted array to simplify pin inversion operations.
-///               Added new function setOutputPins() which sets the motor output pins.
-///               It can be overridden in order to provide, say, serial output instead of parallel output
-///               Some refactoring and code size reduction.
-/// \version 1.20 Improved documentation and examples to show need for correctly
-///               specifying AccelStepper::FULL4WIRE and friends.
-/// \version 1.21 Fixed a problem where desiredSpeed could compute the wrong step acceleration
-///               when _speed was small but non-zero. Reported by Brian Schmalz.
-///               Precompute sqrt_twoa to improve performance and max possible stepping speed
-/// \version 1.22 Added Bounce.pde example
-///               Fixed a problem where calling moveTo(), setMaxSpeed(), setAcceleration() more 
-///               frequently than the step time, even
-///               with the same values, would interfere with speed calcs. Now a new speed is computed 
-///               only if there was a change in the set value. Reported by Brian Schmalz.
-/// \version 1.23 Rewrite of the speed algorithms in line with 
-///               http://fab.cba.mit.edu/classes/MIT/961.09/projects/i0/Stepper_Motor_Speed_Profile.pdf
-///               Now expect smoother and more linear accelerations and decelerations. The desiredSpeed()
-///               function was removed.
-/// \version 1.24  Fixed a problem introduced in 1.23: with runToPosition, which did never returned
-/// \version 1.25  Now ignore attempts to set acceleration to 0.0
-/// \version 1.26  Fixed a problem where certina combinations of speed and accelration could cause
-///                oscillation about the target position.
-/// \version 1.27  Added stop() function to stop as fast as possible with current acceleration parameters.
-///                Also added new Quickstop example showing its use.
-/// \version 1.28  Fixed another problem where certain combinations of speed and accelration could cause
-///                oscillation about the target position.
-///                Added support for 3 wire full and half steppers such as Hard Disk Drive spindle.
-///                Contributed by Yuri Ivatchkovitch.
-/// \version 1.29  Fixed a problem that could cause a DRIVER stepper to continually step
-///                with some sketches. Reported by Vadim.
-/// \version 1.30  Fixed a problem that could cause stepper to back up a few steps at the end of
-///                accelerated travel with certain speeds. Reported and patched by jolo.
-/// \version 1.31  Updated author and distribution location details to airspayce.com
-/// \version 1.32  Fixed a problem with enableOutputs() and setEnablePin on Arduino Due that
-///                prevented the enable pin changing stae correctly. Reported by Duane Bishop.
-/// \version 1.33  Fixed an error in example AFMotor_ConstantSpeed.pde did not setMaxSpeed();
-///                Fixed a problem that caused incorrect pin sequencing of FULL3WIRE and HALF3WIRE.
-///                Unfortunately this meant changing the signature for all step*() functions.
-///                Added example MotorShield, showing how to use AdaFruit Motor Shield to control
-///                a 3 phase motor such as a HDD spindle motor (and without using the AFMotor library.
-/// \version 1.34  Added setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert) 
-///                to allow inversion of 2, 3 and 4 wire stepper pins. Requested by Oleg.
-/// \version 1.35  Removed default args from setPinsInverted(bool, bool, bool, bool, bool) to prevent ambiguity with 
-///                setPinsInverted(bool, bool, bool). Reported by Mac Mac.
-/// \version 1.36  Changed enableOutputs() and disableOutputs() to be virtual so can be overridden.
-///                Added new optional argument 'enable' to constructor, which allows you toi disable the 
-///                automatic enabling of outputs at construction time. Suggested by Guido.
-/// \version 1.37  Fixed a problem with step1 that could cause a rogue step in the 
-///                wrong direction (or not,
-///                depending on the setup-time requirements of the connected hardware). 
-///                Reported by Mark Tillotson.
-/// \version 1.38  run() function incorrectly always returned true. Updated function and doc so it returns true 
-///                if the motor is still running to the target position.
-/// \version 1.39  Updated typos in keywords.txt, courtesey Jon Magill.
-/// \version 1.40  Updated documentation, including testing on Teensy 3.1
-/// \version 1.41  Fixed an error in the acceleration calculations, resulting in acceleration of haldf the intended value
-/// \version 1.42  Improved support for FULL3WIRE and HALF3WIRE output pins. These changes were in Yuri's original
-///                contribution but did not make it into production.<br>
-/// \version 1.43  Added DualMotorShield example. Shows how to use AccelStepper to control 2 x 2 phase steppers using the 
-///                Itead Studio Arduino Dual Stepper Motor Driver Shield model IM120417015.<br>
-/// \version 1.44  examples/DualMotorShield/DualMotorShield.ino examples/DualMotorShield/DualMotorShield.pde
-///                was missing from the distribution.<br>
-/// \version 1.45  Fixed a problem where if setAcceleration was not called, there was no default
-///                acceleration. Reported by Michael Newman.<br>
-/// \version 1.45  Fixed inaccuracy in acceleration rate by using Equation 15, suggested by Sebastian Gracki.<br>
-///                Performance improvements in runSpeed suggested by Jaakko Fagerlund.<br>
-/// \version 1.46  Fixed error in documentation for runToPosition().
-///                Reinstated time calculations in runSpeed() since new version is reported 
-///                not to work correctly under some circumstances. Reported by Oleg V Gavva.<br>
-
-///
-/// \author  Mike McCauley (mikem@airspayce.com) DO NOT CONTACT THE AUTHOR DIRECTLY: USE THE LISTS
-// Copyright (C) 2009-2013 Mike McCauley
-// $Id: AccelStepper.h,v 1.21 2014/10/31 06:05:30 mikem Exp mikem $
-
-#ifndef AccelStepper_h
-#define AccelStepper_h
-
-#include <stdlib.h>
-#include <mbed.h>
-#define max(a,b) (((a) > (b)) ? (a) : (b))
-#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
-#define LOW false
-#define HIGH true
-//#if ARDUINO >= 100
-//#include <Arduino.h>
-//#else
-//#include <WProgram.h>
-//#include <wiring.h>
-//#endif
-
-// These defs cause trouble on some versions of Arduino
-#undef round
-
-/////////////////////////////////////////////////////////////////////
-/// \class AccelStepper AccelStepper.h <AccelStepper.h>
-/// \brief Support for stepper motors with acceleration etc.
-///
-/// This defines a single 2 or 4 pin stepper motor, or stepper moter with fdriver chip, with optional
-/// acceleration, deceleration, absolute positioning commands etc. Multiple
-/// simultaneous steppers are supported, all moving 
-/// at different speeds and accelerations. 
-///
-/// \par Operation
-/// This module operates by computing a step time in microseconds. The step
-/// time is recomputed after each step and after speed and acceleration
-/// parameters are changed by the caller. The time of each step is recorded in
-/// microseconds. The run() function steps the motor once if a new step is due.
-/// The run() function must be called frequently until the motor is in the
-/// desired position, after which time run() will do nothing.
-///
-/// \par Positioning
-/// Positions are specified by a signed long integer. At
-/// construction time, the current position of the motor is consider to be 0. Positive
-/// positions are clockwise from the initial position; negative positions are
-/// anticlockwise. The current position can be altered for instance after
-/// initialization positioning.
-///
-/// \par Caveats
-/// This is an open loop controller: If the motor stalls or is oversped,
-/// AccelStepper will not have a correct 
-/// idea of where the motor really is (since there is no feedback of the motor's
-/// real position. We only know where we _think_ it is, relative to the
-/// initial starting point).
-///
-/// \par Performance
-/// The fastest motor speed that can be reliably supported is about 4000 steps per
-/// second at a clock frequency of 16 MHz on Arduino such as Uno etc. 
-/// Faster processors can support faster stepping speeds. 
-/// However, any speed less than that
-/// down to very slow speeds (much less than one per second) are also supported,
-/// provided the run() function is called frequently enough to step the motor
-/// whenever required for the speed set.
-/// Calling setAcceleration() is expensive,
-/// since it requires a square root to be calculated.
-class AccelStepper
-{
-public:
-    /// \brief Symbolic names for number of pins.
-    /// Use this in the pins argument the AccelStepper constructor to 
-    /// provide a symbolic name for the number of pins
-    /// to use.
-    typedef enum
-    {
-	FUNCTION  = 0, ///< Use the functional interface, implementing your own driver functions (internal use only)
-	DRIVER    = 1, ///< Stepper Driver, 2 driver pins required
-	FULL2WIRE = 2, ///< 2 wire stepper, 2 motor pins required
-	FULL3WIRE = 3, ///< 3 wire stepper, such as HDD spindle, 3 motor pins required
-        FULL4WIRE = 4, ///< 4 wire full stepper, 4 motor pins required
-	HALF3WIRE = 6, ///< 3 wire half stepper, such as HDD spindle, 3 motor pins required
-	HALF4WIRE = 8  ///< 4 wire half stepper, 4 motor pins required
-    } MotorInterfaceType;
-
-    /// Constructor. You can have multiple simultaneous steppers, all moving
-    /// at different speeds and accelerations, provided you call their run()
-    /// functions at frequent enough intervals. Current Position is set to 0, target
-    /// position is set to 0. MaxSpeed and Acceleration default to 1.0.
-    /// The motor pins will be initialised to OUTPUT mode during the
-    /// constructor by a call to enableOutputs().
-    /// \param[in] interface Number of pins to interface to. 1, 2, 4 or 8 are
-    /// supported, but it is preferred to use the \ref MotorInterfaceType symbolic names. 
-    /// AccelStepper::DRIVER (1) means a stepper driver (with Step and Direction pins).
-    /// If an enable line is also needed, call setEnablePin() after construction.
-    /// You may also invert the pins using setPinsInverted().
-    /// AccelStepper::FULL2WIRE (2) means a 2 wire stepper (2 pins required). 
-    /// AccelStepper::FULL3WIRE (3) means a 3 wire stepper, such as HDD spindle (3 pins required). 
-    /// AccelStepper::FULL4WIRE (4) means a 4 wire stepper (4 pins required). 
-    /// AccelStepper::HALF3WIRE (6) means a 3 wire half stepper, such as HDD spindle (3 pins required)
-    /// AccelStepper::HALF4WIRE (8) means a 4 wire half stepper (4 pins required)
-    /// Defaults to AccelStepper::FULL4WIRE (4) pins.
-    /// \param[in] pin1 Arduino digital pin number for motor pin 1. Defaults
-    /// to pin 2. For a AccelStepper::DRIVER (interface==1), 
-    /// this is the Step input to the driver. Low to high transition means to step)
-    /// \param[in] pin2 Arduino digital pin number for motor pin 2. Defaults
-    /// to pin 3. For a AccelStepper::DRIVER (interface==1), 
-    /// this is the Direction input the driver. High means forward.
-    /// \param[in] pin3 Arduino digital pin number for motor pin 3. Defaults
-    /// to pin 4.
-    /// \param[in] pin4 Arduino digital pin number for motor pin 4. Defaults
-    /// to pin 5.
-    /// \param[in] enable If this is true (the default), enableOutputs() will be called to enable
-    /// the output pins at construction time.
-    // AccelStepper(uint8_t interface = AccelStepper::FULL4WIRE, uint8_t pin1 = 2, uint8_t pin2 = 3, uint8_t pin3 = 4, uint8_t pin4 = 5, bool enable = true);
-	AccelStepper(uint8_t interface, PinName pin1 = LED1, PinName pin2 = LED2, PinName pin3 = LED3, PinName pin4 = LED4, bool enable = true);
-
-    /// Alternate Constructor which will call your own functions for forward and backward steps. 
-    /// You can have multiple simultaneous steppers, all moving
-    /// at different speeds and accelerations, provided you call their run()
-    /// functions at frequent enough intervals. Current Position is set to 0, target
-    /// position is set to 0. MaxSpeed and Acceleration default to 1.0.
-    /// Any motor initialization should happen before hand, no pins are used or initialized.
-    /// \param[in] forward void-returning procedure that will make a forward step
-    /// \param[in] backward void-returning procedure that will make a backward step
-    AccelStepper(void (*forward)(), void (*backward)());
-    
-    /// Set the target position. The run() function will try to move the motor (at most one step per call)
-    /// from the current position to the target position set by the most
-    /// recent call to this function. Caution: moveTo() also recalculates the speed for the next step. 
-    /// If you are trying to use constant speed movements, you should call setSpeed() after calling moveTo().
-    /// \param[in] absolute The desired absolute position. Negative is
-    /// anticlockwise from the 0 position.
-    void    moveTo(long absolute); 
-
-    /// Set the target position relative to the current position
-    /// \param[in] relative The desired position relative to the current position. Negative is
-    /// anticlockwise from the current position.
-    void    move(long relative);
-
-    /// Poll the motor and step it if a step is due, implementing
-    /// accelerations and decelerations to acheive the target position. You must call this as
-    /// frequently as possible, but at least once per minimum step time interval,
-    /// preferably in your main loop. Note that each call to run() will make at most one step, and then only when a step is due,
-    /// based on the current speed and the time since the last step.
-    /// \return true if the motor is still running to the target position.
-    bool run();
-
-    /// Poll the motor and step it if a step is due, implementing a constant
-    /// speed as set by the most recent call to setSpeed(). You must call this as
-    /// frequently as possible, but at least once per step interval,
-    /// \return true if the motor was stepped.
-    bool runSpeed();
-
-    /// Sets the maximum permitted speed. The run() function will accelerate
-    /// up to the speed set by this function.
-    /// Caution: the maximum speed achievable depends on your processor and clock speed.
-    /// \param[in] speed The desired maximum speed in steps per second. Must
-    /// be > 0. Caution: Speeds that exceed the maximum speed supported by the processor may
-    /// Result in non-linear accelerations and decelerations.
-    void    setMaxSpeed(float speed);
-
-    /// Sets the acceleration/deceleration rate.
-    /// \param[in] acceleration The desired acceleration in steps per second
-    /// per second. Must be > 0.0. This is an expensive call since it requires a square 
-    /// root to be calculated. Dont call more ofthen than needed
-    void    setAcceleration(float acceleration);
-
-    /// Sets the desired constant speed for use with runSpeed().
-    /// \param[in] speed The desired constant speed in steps per
-    /// second. Positive is clockwise. Speeds of more than 1000 steps per
-    /// second are unreliable. Very slow speeds may be set (eg 0.00027777 for
-    /// once per hour, approximately. Speed accuracy depends on the Arduino
-    /// crystal. Jitter depends on how frequently you call the runSpeed() function.
-    void    setSpeed(float speed);
-
-    /// The most recently set speed
-    /// \return the most recent speed in steps per second
-    float   speed();
-
-    /// The distance from the current position to the target position.
-    /// \return the distance from the current position to the target position
-    /// in steps. Positive is clockwise from the current position.
-    long    distanceToGo();
-
-    /// The most recently set target position.
-    /// \return the target position
-    /// in steps. Positive is clockwise from the 0 position.
-    long    targetPosition();
-
-    /// The currently motor position.
-    /// \return the current motor position
-    /// in steps. Positive is clockwise from the 0 position.
-    long    currentPosition();  
-
-    /// Resets the current position of the motor, so that wherever the motor
-    /// happens to be right now is considered to be the new 0 position. Useful
-    /// for setting a zero position on a stepper after an initial hardware
-    /// positioning move.
-    /// Has the side effect of setting the current motor speed to 0.
-    /// \param[in] position The position in steps of wherever the motor
-    /// happens to be right now.
-    void    setCurrentPosition(long position);  
-    
-    /// Moves the motor (with acceleration/deceleration) 
-    /// to the target position and blocks until it is at
-    /// position. Dont use this in event loops, since it blocks.
-    void    runToPosition();
-
-    /// Runs at the currently selected speed until the target position is reached
-    /// Does not implement accelerations.
-    /// \return true if it stepped
-    bool runSpeedToPosition();
-
-    /// Moves the motor (with acceleration/deceleration)
-    /// to the new target position and blocks until it is at
-    /// position. Dont use this in event loops, since it blocks.
-    /// \param[in] position The new target position.
-    void    runToNewPosition(long position);
-
-    /// Sets a new target position that causes the stepper
-    /// to stop as quickly as possible, using the current speed and acceleration parameters.
-    void stop();
-
-    /// Disable motor pin outputs by setting them all LOW
-    /// Depending on the design of your electronics this may turn off
-    /// the power to the motor coils, saving power.
-    /// This is useful to support Arduino low power modes: disable the outputs
-    /// during sleep and then reenable with enableOutputs() before stepping
-    /// again.
-    virtual void    disableOutputs();
-
-    /// Enable motor pin outputs by setting the motor pins to OUTPUT
-    /// mode. Called automatically by the constructor.
-    virtual void    enableOutputs();
-
-    /// Sets the minimum pulse width allowed by the stepper driver. The minimum practical pulse width is 
-    /// approximately 20 microseconds. Times less than 20 microseconds
-    /// will usually result in 20 microseconds or so.
-    /// \param[in] minWidth The minimum pulse width in microseconds. 
-    void    setMinPulseWidth(unsigned int minWidth);
-
-    /// Sets the enable pin number for stepper drivers.
-    /// 0xFF indicates unused (default).
-    /// Otherwise, if a pin is set, the pin will be turned on when 
-    /// enableOutputs() is called and switched off when disableOutputs() 
-    /// is called.
-    /// \param[in] enablePin Arduino digital pin number for motor enable
-    /// \sa setPinsInverted
-    void    setEnablePin(PinName enablePin);
-
-    /// Sets the inversion for stepper driver pins
-    /// \param[in] directionInvert True for inverted direction pin, false for non-inverted
-    /// \param[in] stepInvert      True for inverted step pin, false for non-inverted
-    /// \param[in] enableInvert    True for inverted enable pin, false (default) for non-inverted
-    void    setPinsInverted(bool directionInvert = false, bool stepInvert = false, bool enableInvert = false);
-
-    /// Sets the inversion for 2, 3 and 4 wire stepper pins
-    /// \param[in] pin1Invert True for inverted pin1, false for non-inverted
-    /// \param[in] pin2Invert True for inverted pin2, false for non-inverted
-    /// \param[in] pin3Invert True for inverted pin3, false for non-inverted
-    /// \param[in] pin4Invert True for inverted pin4, false for non-inverted
-    /// \param[in] enableInvert    True for inverted enable pin, false (default) for non-inverted
-    void    setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert);
-
-protected:
-	DigitalOut *_pin0;
-	DigitalOut *_pin1;
-	DigitalOut *_pin2;
-	DigitalOut *_pin3;
-
-    /// \brief Direction indicator
-    /// Symbolic names for the direction the motor is turning
-    typedef enum
-    {
-	DIRECTION_CCW = 0,  ///< Clockwise
-        DIRECTION_CW  = 1   ///< Counter-Clockwise
-    } Direction;
-
-    /// Forces the library to compute a new instantaneous speed and set that as
-    /// the current speed. It is called by
-    /// the library:
-    /// \li  after each step
-    /// \li  after change to maxSpeed through setMaxSpeed()
-    /// \li  after change to acceleration through setAcceleration()
-    /// \li  after change to target position (relative or absolute) through
-    /// move() or moveTo()
-    void           computeNewSpeed();
-
-    /// Low level function to set the motor output pins
-    /// bit 0 of the mask corresponds to _pin[0]
-    /// bit 1 of the mask corresponds to _pin[1]
-    /// You can override this to impment, for example serial chip output insted of using the
-    /// output pins directly
-    virtual void   setOutputPins(uint8_t mask);
-
-    /// Called to execute a step. Only called when a new step is
-    /// required. Subclasses may override to implement new stepping
-    /// interfaces. The default calls step1(), step2(), step4() or step8() depending on the
-    /// number of pins defined for the stepper.
-    /// \param[in] step The current step phase number (0 to 7)
-    virtual void   step(long step);
-
-    /// Called to execute a step using stepper functions (pins = 0) Only called when a new step is
-    /// required. Calls _forward() or _backward() to perform the step
-    /// \param[in] step The current step phase number (0 to 7)
-    virtual void   step0(long step);
-
-    /// Called to execute a step on a stepper driver (ie where pins == 1). Only called when a new step is
-    /// required. Subclasses may override to implement new stepping
-    /// interfaces. The default sets or clears the outputs of Step pin1 to step, 
-    /// and sets the output of _pin2 to the desired direction. The Step pin (_pin1) is pulsed for 1 microsecond
-    /// which is the minimum STEP pulse width for the 3967 driver.
-    /// \param[in] step The current step phase number (0 to 7)
-    virtual void   step1(long step);
-
-    /// Called to execute a step on a 2 pin motor. Only called when a new step is
-    /// required. Subclasses may override to implement new stepping
-    /// interfaces. The default sets or clears the outputs of pin1 and pin2
-    /// \param[in] step The current step phase number (0 to 7)
-    virtual void   step2(long step);
-
-    /// Called to execute a step on a 3 pin motor, such as HDD spindle. Only called when a new step is
-    /// required. Subclasses may override to implement new stepping
-    /// interfaces. The default sets or clears the outputs of pin1, pin2,
-    /// pin3
-    /// \param[in] step The current step phase number (0 to 7)
-    virtual void   step3(long step);
-
-    /// Called to execute a step on a 4 pin motor. Only called when a new step is
-    /// required. Subclasses may override to implement new stepping
-    /// interfaces. The default sets or clears the outputs of pin1, pin2,
-    /// pin3, pin4.
-    /// \param[in] step The current step phase number (0 to 7)
-    virtual void   step4(long step);
-
-    /// Called to execute a step on a 3 pin motor, such as HDD spindle. Only called when a new step is
-    /// required. Subclasses may override to implement new stepping
-    /// interfaces. The default sets or clears the outputs of pin1, pin2,
-    /// pin3
-    /// \param[in] step The current step phase number (0 to 7)
-    virtual void   step6(long step);
-
-    /// Called to execute a step on a 4 pin half-steper motor. Only called when a new step is
-    /// required. Subclasses may override to implement new stepping
-    /// interfaces. The default sets or clears the outputs of pin1, pin2,
-    /// pin3, pin4.
-    /// \param[in] step The current step phase number (0 to 7)
-    virtual void   step8(long step);
-
-private:
-    /// Number of pins on the stepper motor. Permits 2 or 4. 2 pins is a
-    /// bipolar, and 4 pins is a unipolar.
-    uint8_t        _interface;          // 0, 1, 2, 4, 8, See MotorInterfaceType
-
-    /// Arduino pin number assignments for the 2 or 4 pins required to interface to the
-    /// stepper motor or driver
-    uint8_t        _pin[4];
-
-    /// Whether the _pins is inverted or not
-    uint8_t        _pinInverted[4];
-
-    /// The current absolution position in steps.
-    long           _currentPos;    // Steps
-
-    /// The target position in steps. The AccelStepper library will move the
-    /// motor from the _currentPos to the _targetPos, taking into account the
-    /// max speed, acceleration and deceleration
-    long           _targetPos;     // Steps
-
-    /// The current motos speed in steps per second
-    /// Positive is clockwise
-    float          _speed;         // Steps per second
-
-    /// The maximum permitted speed in steps per second. Must be > 0.
-    float          _maxSpeed;
-
-    /// The acceleration to use to accelerate or decelerate the motor in steps
-    /// per second per second. Must be > 0
-    float          _acceleration;
-    float          _sqrt_twoa; // Precomputed sqrt(2*_acceleration)
-
-    /// The current interval between steps in microseconds.
-    /// 0 means the motor is currently stopped with _speed == 0
-    unsigned long  _stepInterval;
-
-    /// The last step time in microseconds
-    unsigned long  _lastStepTime;
-
-    /// The minimum allowed pulse width in microseconds
-    unsigned int   _minPulseWidth;
-
-    /// Is the direction pin inverted?
-    ///bool           _dirInverted; /// Moved to _pinInverted[1]
-
-    /// Is the step pin inverted?
-    ///bool           _stepInverted; /// Moved to _pinInverted[0]
-
-    /// Is the enable pin inverted?
-    bool           _enableInverted;
-
-    /// Enable pin for stepper driver, or 0xFF if unused.
-    //uint8_t        _enablePin;
-	DigitalOut *_enablePin;
-
-    /// The pointer to a forward-step procedure
-    void (*_forward)();
-
-    /// The pointer to a backward-step procedure
-    void (*_backward)();
-
-    /// The step counter for speed calculations
-    long _n;
-
-    /// Initial step size in microseconds
-    float _c0;
-
-    /// Last step size in microseconds
-    float _cn;
-
-    /// Min step size in microseconds based on maxSpeed
-    float _cmin; // at max speed
-
-    /// Current direction motor is spinning in
-    bool _direction; // 1 == CW
-
-};
-
-/// @example Random.pde
-/// Make a single stepper perform random changes in speed, position and acceleration
-
-/// @example Overshoot.pde
-///  Check overshoot handling
-/// which sets a new target position and then waits until the stepper has 
-/// achieved it. This is used for testing the handling of overshoots
-
-/// @example MultiStepper.pde
-/// Shows how to multiple simultaneous steppers
-/// Runs one stepper forwards and backwards, accelerating and decelerating
-/// at the limits. Runs other steppers at the same time
-
-/// @example ConstantSpeed.pde
-/// Shows how to run AccelStepper in the simplest,
-/// fixed speed mode with no accelerations
-
-/// @example Blocking.pde 
-/// Shows how to use the blocking call runToNewPosition
-/// Which sets a new target position and then waits until the stepper has 
-/// achieved it.
-
-/// @example AFMotor_MultiStepper.pde
-/// Control both Stepper motors at the same time with different speeds
-/// and accelerations. 
-
-/// @example AFMotor_ConstantSpeed.pde
-/// Shows how to run AccelStepper in the simplest,
-/// fixed speed mode with no accelerations
-
-/// @example ProportionalControl.pde
-/// Make a single stepper follow the analog value read from a pot or whatever
-/// The stepper will move at a constant speed to each newly set posiiton, 
-/// depending on the value of the pot.
-
-/// @example Bounce.pde
-/// Make a single stepper bounce from one limit to another, observing
-/// accelrations at each end of travel
-
-/// @example Quickstop.pde
-/// Check stop handling.
-/// Calls stop() while the stepper is travelling at full speed, causing
-/// the stepper to stop as quickly as possible, within the constraints of the
-/// current acceleration.
-
-/// @example MotorShield.pde
-/// Shows how to use AccelStepper to control a 3-phase motor, such as a HDD spindle motor
-/// using the Adafruit Motor Shield http://www.ladyada.net/make/mshield/index.html.
-
-/// @example DualMotorShield.pde
-/// Shows how to use AccelStepper to control 2 x 2 phase steppers using the 
-/// Itead Studio Arduino Dual Stepper Motor Driver Shield
-/// model IM120417015
-
-#endif 
-

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AccelStepper.lib	Mon May 07 11:41:10 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/yuliyasm/code/AccelStepper/#deef8c36d17a

--- a/USBDevice_STM32F103.lib	Sat Feb 03 19:25:25 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/users/hudakz/code/USBDevice_STM32F103/#0a9e747bca06

--- a/main.cpp	Sat Feb 03 19:25:25 2018 +0000
+++ b/main.cpp	Mon May 07 11:41:10 2018 +0000
@@ -1,88 +1,118 @@
 #include "stm32f103c8t6.h"
 #include "mbed.h"
-#include "USBSerial.h"
 #include <string>
 #include "AccelStepper.h"
 //X20Y20Z-300\r - пример команды
 
-
-const float e = 24;     // end effector
-const float f = 75;     // base
-const float re = 300;
+const float e = 20;     // end effector
+const float f = 80;     // base
+const float re = 225;
 const float rf = 100;
 
 const float pi = 3.14;
 const float cos120 = -0.5;
 const float sin120 = sqrt(3.0) / 2;
 const float tg30 = 1 / sqrt(3.0);
-const float q = 1; //микрошаг
+const float q = 8; //микрошаг
 
 bool parseXYZ(char* m, float* A);
 int delta_calcInverse(float* A, float* B);
+void MotorsInit();
 
 PinName stp[3] = {PB_15,PA_8,PA_9};
 PinName dir[3] = {PB_12,PB_13,PB_14};
+AccelStepper axis1(1, stp[0], dir[0]);
+AccelStepper axis2(1, stp[1], dir[1]);
+AccelStepper axis3(1, stp[2], dir[2]);
+AccelStepper Motors[3] = {axis1, axis2, axis3};
+DigitalIn  zero1(PB_10);
+DigitalIn  zero2(PB_1);
+DigitalIn  zero3(PB_0);
+DigitalIn zero[3] = {zero1, zero2, zero3};
+Serial      pc(PA_2, PA_3);
+DigitalOut  myled(LED1);
+DigitalOut  enb(PA_10);
 
-AccelStepper Xaxis(1, stp[0], dir[0]);
-AccelStepper Yaxis(1, stp[1], dir[1]);
-AccelStepper Zaxis(1, stp[2], dir[2]);
-AccelStepper Motors[3] = {Xaxis, Yaxis, Zaxis};
 Timer t;
 
 int main()
 {
     confSysClock();     //Configure system clock (72MHz HSE clock, 48MHz USB clock)
-    USBSerial usbSerial(0x1f00, 0x2012, 0x0001,  false);
-    //DigitalOut  myled(LED1);
-    DigitalOut  enb(PA_10);
+    pc.baud(9600);
     t.start();
 
-    for (int i = 0; i < 3; i++) {
-        Motors[i].setMaxSpeed(1000);
-        Motors[i].setAcceleration(100);
-        Motors[i].setMinPulseWidth(10000);
-        //Motors[i].moveTo(500);
-    }
-
-    usbSerial.printf("Ready\r\n");
-
+    MotorsInit();
+    float angles[3] = {0, 0, 0};
+    float point[3] = {0, 0, 0};
+    char message[32] = "";
+    char c = '\0';
     while(1) {
 
-        enb = 1;
-        /*myled = 0;      // turn the LED on
-        wait_ms(200);   // 200 millisecond
-        myled = 1;      // turn the LED off
-        wait_ms(1000);  // 1000 millisecond
-        usbSerial.printf("Blink\r\n");*/
+        for (int i = 0; i < 3; i++)
+            Motors[i].run();
+        if (Motors[0].distanceToGo() == 0)
+            myled = 1;
 
-        if (usbSerial.available()) {
-            char message[32];
-            wait_ms(2);
-            usbSerial.scanf("%s", message);
-            strcat(message, "\r");
-            usbSerial.printf("%s", message);
+        if (pc.readable()) {
+            wait_ms(300);
+            strcpy(message, "");
+            pc.scanf("%s\r\n", message);
+            strcat(message, "\n");
+            pc.printf("%s", message);
 
-            float point[3];
             bool p = parseXYZ(message, point);
-            if(p == false)
-                return;
-            usbSerial.printf("%.2f \r", point[0]);
-            usbSerial.printf("%.2f \r", point[1]);
-            usbSerial.printf("%.2f \r", point[2]);
+            if(p == false) {
+                pc.printf("Parse fail");
+                continue;
+            }
+            pc.printf("%.2f ", point[0]);
+            pc.printf("%.2f ", point[1]);
+            pc.printf("%.2f \n", point[2]);
+            delta_calcInverse(point, angles);
 
-            float angles[3];
-            if (!delta_calcInverse(point, angles)) {
-                for (int i = 0; i < 3; i++) {
-                    usbSerial.printf("%.2f \r", angles[i]);
-                    Motors[i].moveTo(angles[i]*q/1.8);
-                }
+            for (int i = 0; i < 3; i++) {
+                pc.printf("%.2f ", angles[i]);
+                Motors[i].moveTo(int(angles[i]*q/1.8));
+                myled = 0;
+                //Motors[i].runToPosition();
+                //myled = 1;
             }
         }
-        for (int i = 0; i < 3; i++)
-            Motors[i].runToPosition();
     }
 }
 
+void MotorsInit()
+{
+    enb = 0;
+    myled = 0;
+    pc.printf("Hello\r\n");
+
+    for (int i = 0; i < 3; i++) {
+        Motors[i].setMinPulseWidth (100);
+        Motors[i].setMaxSpeed (500);
+        Motors[i].setAcceleration (100);
+        Motors[i].moveTo(-90*q/1.8);
+        Motors[i].setSpeed (50);
+    }
+
+    while(zero[0] || zero[1] || zero[2]) {
+        for (int i = 0; i < 3; i++) {
+            if(zero[i] != 0)
+                Motors[i].runSpeedToPosition();
+            if (!Motors[i].distanceToGo())
+                goto setup;
+        }
+    }
+setup:
+    for (int i = 0; i < 3; i++) {
+        Motors[i].stop();
+        Motors[i].setCurrentPosition(0);
+    }
+    myled = 1;
+    pc.printf("Ready\r\n");
+}
+
+
 bool parseXYZ(char* m, float* A)
 {
     char buff[32] = "";
@@ -90,7 +120,9 @@
     if (m[i] != 'X')
         return false;
     i++;
-    while (m[i] != 'Y' && i < 32) {
+    while (m[i] != 'Y') {
+        if (m[i] == '\n')
+            return false;
         strcat(buff, &m[i]);
         i++;
     }
@@ -98,7 +130,9 @@
 
     strcpy(buff,"");
     i++;
-    while (m[i] != 'Z' && i < 32) {
+    while (m[i] != 'Z') {
+        if (m[i] == '\n')
+            return false;
         strcat(buff, &m[i]);
         i++;
     }
@@ -106,12 +140,13 @@
 
     strcpy(buff,"");
     i++;
-    while (m[i] != '\r' && i < 32) {
+    while (m[i] != '\n') {
+        if (i > 32)
+            return false;
         strcat(buff, &m[i]);
         i++;
     }
     A[2] = atof(buff);
-
     return true;
 }
 
@@ -131,8 +166,6 @@
     return 0;
 }
 
-// inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3)
-// returned status: 0=OK, -1=non-existing position
 int delta_calcInverse(float* A, float* B)
 {
     float x0 = A[0];
@@ -142,4 +175,4 @@
     if (status == 0) status = delta_calcAngleYZ(x0 * cos120 + y0 * sin120, y0 * cos120 - x0 * sin120, z0, B[1]); // rotate coords to +120 deg
     if (status == 0) status = delta_calcAngleYZ(x0 * cos120 - y0 * sin120, y0 * cos120 + x0 * sin120, z0, B[2]); // rotate coords to -120 deg
     return status;
-}
+}
\ No newline at end of file