449 lines
13 KiB
C++
449 lines
13 KiB
C++
/**
|
|
FirmataStepper is a simple non-blocking stepper motor library
|
|
for 2 and 4 wire bipolar and unipolar stepper motor drive circuits
|
|
as well as EasyDriver (http://schmalzhaus.com/EasyDriver/) and
|
|
other step + direction drive circuits.
|
|
|
|
FirmataStepper (0.2) by Jeff Hoefs
|
|
|
|
EasyDriver support based on modifications by Chris Coleman
|
|
|
|
Acceleration / Deceleration algorithms and code based on:
|
|
app note: http://www.atmel.com/dyn/resources/prod_documents/doc8017.pdf
|
|
source code: http://www.atmel.com/dyn/resources/prod_documents/AVR446.zip
|
|
|
|
stepMotor function based on Stepper.cpp Stepper library for
|
|
Wiring/Arduino created by Tom Igoe, Sebastian Gassner
|
|
David Mellis and Noah Shibley.
|
|
|
|
Relevant notes from Stepper.cpp:
|
|
|
|
When wiring multiple stepper motors to a microcontroller,
|
|
you quickly run out of output pins, with each motor requiring 4 connections.
|
|
|
|
By making use of the fact that at any time two of the four motor
|
|
coils are the inverse of the other two, the number of
|
|
control connections can be reduced from 4 to 2.
|
|
|
|
A slightly modified circuit around a Darlington transistor array or an L293 H-bridge
|
|
connects to only 2 microcontroler pins, inverts the signals received,
|
|
and delivers the 4 (2 plus 2 inverted ones) output signals required
|
|
for driving a stepper motor.
|
|
|
|
The sequence of control signals for 4 control wires is as follows:
|
|
|
|
Step C0 C1 C2 C3
|
|
1 1 0 1 0
|
|
2 0 1 1 0
|
|
3 0 1 0 1
|
|
4 1 0 0 1
|
|
|
|
The sequence of controls signals for 2 control wires is as follows
|
|
(columns C1 and C2 from above):
|
|
|
|
Step C0 C1
|
|
1 0 1
|
|
2 1 1
|
|
3 1 0
|
|
4 0 0
|
|
|
|
The circuits can be found at
|
|
http://www.arduino.cc/en/Tutorial/Stepper
|
|
*/
|
|
|
|
#include "FirmataStepper.h"
|
|
|
|
/**
|
|
* Constructor.
|
|
*
|
|
* Configure a stepper for an EasyDriver or other step + direction interface or
|
|
* configure a bipolar or unipolar stepper motor for 2 wire drive mode.
|
|
* Configure a bipolar or unipolar stepper for 4 wire drive mode.
|
|
* @param interface Lower 3 bits:
|
|
* The interface type: FirmataStepper::DRIVER,
|
|
* FirmataStepper::TWO_WIRE or FirmataStepper::FOUR_WIRE
|
|
* Upper 4 bits: Any bits set = use 2 microsecond delay
|
|
* @param steps_per_rev The number of steps to make 1 revolution.
|
|
* @param first_pin The direction pin (EasyDriver) or the pin attached to the
|
|
* 1st motor coil (2 wire drive mode)
|
|
* @param second_pin The step pin (EasyDriver) or the pin attached to the 2nd
|
|
* motor coil (2 wire drive mode)
|
|
* @param motor_pin_3 The pin attached to the 3rd motor coil
|
|
* @param motor_pin_4 The pin attached to the 4th motor coil
|
|
*/
|
|
FirmataStepper::FirmataStepper(byte interface,
|
|
int steps_per_rev,
|
|
byte pin1,
|
|
byte pin2,
|
|
byte pin3,
|
|
byte pin4)
|
|
{
|
|
this->step_number = 0; // which step the motor is on
|
|
this->direction = 0; // motor direction
|
|
this->last_step_time = 0; // time stamp in ms of the last step taken
|
|
this->steps_per_rev = steps_per_rev; // total number of steps for this motor
|
|
this->running = false;
|
|
this->interface = interface & 0x0F; // default to Easy Stepper (or other step + direction driver)
|
|
|
|
// could update this in future to support additional delays if necessary
|
|
if (((interface & 0xF0) >> 4) > 0)
|
|
{
|
|
// high current driver
|
|
this->stepDelay = 2; // microseconds
|
|
}
|
|
else
|
|
{
|
|
this->stepDelay = 1; // microseconds
|
|
}
|
|
|
|
this->motor_pin_1 = pin1;
|
|
this->motor_pin_2 = pin2;
|
|
this->dir_pin = pin1;
|
|
this->step_pin = pin2;
|
|
|
|
// setup the pins on the microcontroller:
|
|
pinMode(this->motor_pin_1, OUTPUT);
|
|
pinMode(this->motor_pin_2, OUTPUT);
|
|
|
|
if (this->interface == FirmataStepper::FOUR_WIRE)
|
|
{
|
|
this->motor_pin_3 = pin3;
|
|
this->motor_pin_4 = pin4;
|
|
pinMode(this->motor_pin_3, OUTPUT);
|
|
pinMode(this->motor_pin_4, OUTPUT);
|
|
}
|
|
|
|
|
|
this->alpha = PI_2 / this->steps_per_rev;
|
|
this->at_x100 = (long)(this->alpha * T1_FREQ * 100);
|
|
this->ax20000 = (long)(this->alpha * 20000);
|
|
this->alpha_x2 = this->alpha * 2;
|
|
|
|
}
|
|
|
|
/**
|
|
* Move the stepper a given number of steps at the specified
|
|
* speed (rad/sec), acceleration (rad/sec^2) and deceleration (rad/sec^2).
|
|
*
|
|
* @param steps_to_move The number ofsteps to move the motor
|
|
* @param speed Max speed in 0.01*rad/sec
|
|
* @param accel [optional] Acceleration in 0.01*rad/sec^2
|
|
* @param decel [optional] Deceleration in 0.01*rad/sec^2
|
|
*/
|
|
void FirmataStepper::setStepsToMove(long steps_to_move, int speed, int accel, int decel)
|
|
{
|
|
unsigned long maxStepLimit;
|
|
unsigned long accelerationLimit;
|
|
|
|
this->step_number = 0;
|
|
this->lastAccelDelay = 0;
|
|
this->stepCount = 0;
|
|
this->rest = 0;
|
|
|
|
if (steps_to_move < 0)
|
|
{
|
|
this->direction = FirmataStepper::CCW;
|
|
steps_to_move = -steps_to_move;
|
|
}
|
|
else
|
|
{
|
|
this->direction = FirmataStepper::CW;
|
|
}
|
|
|
|
this->steps_to_move = steps_to_move;
|
|
|
|
// set max speed limit, by calc min_delay
|
|
// min_delay = (alpha / tt)/w
|
|
this->min_delay = this->at_x100 / speed;
|
|
|
|
// if acceleration or deceleration are not defined
|
|
// start in RUN state and do no decelerate
|
|
if (accel == 0 || decel == 0)
|
|
{
|
|
this->step_delay = this->min_delay;
|
|
|
|
this->decel_start = steps_to_move;
|
|
this->run_state = FirmataStepper::RUN;
|
|
this->accel_count = 0;
|
|
this->running = true;
|
|
|
|
return;
|
|
}
|
|
|
|
// if only moving 1 step
|
|
if (steps_to_move == 1)
|
|
{
|
|
|
|
// move one step
|
|
this->accel_count = -1;
|
|
this->run_state = FirmataStepper::DECEL;
|
|
|
|
this->step_delay = this->min_delay;
|
|
this->running = true;
|
|
}
|
|
else if (steps_to_move != 0)
|
|
{
|
|
// set initial step delay
|
|
// step_delay = 1/tt * sqrt(2*alpha/accel)
|
|
// step_delay = ( tfreq*0.676/100 )*100 * sqrt( (2*alpha*10000000000) / (accel*100) )/10000
|
|
this->step_delay = (long)((T1_FREQ_148 * sqrt(alpha_x2 / accel)) * 1000);
|
|
|
|
// find out after how many steps does the speed hit the max speed limit.
|
|
// maxSpeedLimit = speed^2 / (2*alpha*accel)
|
|
maxStepLimit = (long)speed * speed / (long)(((long)this->ax20000 * accel) / 100);
|
|
|
|
// if we hit max spped limit before 0.5 step it will round to 0.
|
|
// but in practice we need to move at least 1 step to get any speed at all.
|
|
if (maxStepLimit == 0)
|
|
{
|
|
maxStepLimit = 1;
|
|
}
|
|
|
|
// find out after how many steps we must start deceleration.
|
|
// n1 = (n1+n2)decel / (accel + decel)
|
|
accelerationLimit = (long)((steps_to_move * decel) / (accel + decel));
|
|
|
|
// we must accelerate at least 1 step before we can start deceleration
|
|
if (accelerationLimit == 0)
|
|
{
|
|
accelerationLimit = 1;
|
|
}
|
|
|
|
// use the limit we hit first to calc decel
|
|
if (accelerationLimit <= maxStepLimit)
|
|
{
|
|
this->decel_val = accelerationLimit - steps_to_move;
|
|
}
|
|
else
|
|
{
|
|
this->decel_val = -(long)(maxStepLimit * accel) / decel;
|
|
}
|
|
|
|
// we must decelerate at least 1 step to stop
|
|
if (this->decel_val == 0)
|
|
{
|
|
this->decel_val = -1;
|
|
}
|
|
|
|
// find step to start deceleration
|
|
this->decel_start = steps_to_move + this->decel_val;
|
|
|
|
// if the max speed is so low that we don't need to go via acceleration state.
|
|
if (this->step_delay <= this->min_delay)
|
|
{
|
|
this->step_delay = this->min_delay;
|
|
this->run_state = FirmataStepper::RUN;
|
|
}
|
|
else
|
|
{
|
|
this->run_state = FirmataStepper::ACCEL;
|
|
}
|
|
|
|
// reset counter
|
|
this->accel_count = 0;
|
|
this->running = true;
|
|
}
|
|
}
|
|
|
|
|
|
bool FirmataStepper::update()
|
|
{
|
|
bool done = false;
|
|
unsigned long newStepDelay = this->min_delay;
|
|
|
|
unsigned long curTimeVal = micros();
|
|
unsigned long timeDiff = curTimeVal - this->last_step_time;
|
|
|
|
if (this->running == true && timeDiff >= this->step_delay)
|
|
{
|
|
|
|
this->last_step_time = curTimeVal;
|
|
|
|
switch (this->run_state)
|
|
{
|
|
case FirmataStepper::STOP:
|
|
this->stepCount = 0;
|
|
this->rest = 0;
|
|
if (this->running)
|
|
{
|
|
done = true;
|
|
}
|
|
this->running = false;
|
|
break;
|
|
|
|
case FirmataStepper::ACCEL:
|
|
updateStepPosition();
|
|
this->stepCount++;
|
|
this->accel_count++;
|
|
newStepDelay = this->step_delay - (((2 * (long)this->step_delay) + this->rest) / (4 * this->accel_count + 1));
|
|
this->rest = ((2 * (long)this->step_delay) + this->rest) % (4 * this->accel_count + 1);
|
|
|
|
// check if we should start deceleration
|
|
if (this->stepCount >= this->decel_start)
|
|
{
|
|
this->accel_count = this->decel_val;
|
|
this->run_state = FirmataStepper::DECEL;
|
|
this->rest = 0;
|
|
}
|
|
// check if we hit max speed
|
|
else if (newStepDelay <= this->min_delay)
|
|
{
|
|
this->lastAccelDelay = newStepDelay;
|
|
newStepDelay = this->min_delay;
|
|
this->rest = 0;
|
|
this->run_state = FirmataStepper::RUN;
|
|
}
|
|
break;
|
|
|
|
case FirmataStepper::RUN:
|
|
updateStepPosition();
|
|
this->stepCount++;
|
|
|
|
// if no accel or decel was specified, go directly to STOP state
|
|
if (stepCount >= this->steps_to_move)
|
|
{
|
|
this->run_state = FirmataStepper::STOP;
|
|
}
|
|
// check if we should start deceleration
|
|
else if (this->stepCount >= this->decel_start)
|
|
{
|
|
this->accel_count = this->decel_val;
|
|
// start deceleration with same delay that accel ended with
|
|
newStepDelay = this->lastAccelDelay;
|
|
this->run_state = FirmataStepper::DECEL;
|
|
}
|
|
break;
|
|
|
|
case FirmataStepper::DECEL:
|
|
updateStepPosition();
|
|
this->stepCount++;
|
|
this->accel_count++;
|
|
|
|
newStepDelay = this->step_delay - (((2 * (long)this->step_delay) + this->rest) / (4 * this->accel_count + 1));
|
|
this->rest = ((2 * (long)this->step_delay) + this->rest) % (4 * this->accel_count + 1);
|
|
|
|
if (newStepDelay < 0) newStepDelay = -newStepDelay;
|
|
// check if we ar at the last step
|
|
if (this->accel_count >= 0)
|
|
{
|
|
this->run_state = FirmataStepper::STOP;
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
this->step_delay = newStepDelay;
|
|
|
|
}
|
|
|
|
return done;
|
|
|
|
}
|
|
|
|
/**
|
|
* Update the step position.
|
|
* @private
|
|
*/
|
|
void FirmataStepper::updateStepPosition()
|
|
{
|
|
// increment or decrement the step number,
|
|
// depending on direction:
|
|
if (this->direction == FirmataStepper::CW)
|
|
{
|
|
this->step_number++;
|
|
if (this->step_number >= this->steps_per_rev)
|
|
{
|
|
this->step_number = 0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (this->step_number <= 0)
|
|
{
|
|
this->step_number = this->steps_per_rev;
|
|
}
|
|
this->step_number--;
|
|
}
|
|
|
|
// step the motor to step number 0, 1, 2, or 3:
|
|
stepMotor(this->step_number % 4, this->direction);
|
|
}
|
|
|
|
/**
|
|
* Moves the motor forward or backwards.
|
|
* @param step_num For 2 or 4 wire configurations, this is the current step in
|
|
* the 2 or 4 step sequence.
|
|
* @param direction The direction of rotation
|
|
*/
|
|
void FirmataStepper::stepMotor(byte step_num, byte direction)
|
|
{
|
|
if (this->interface == FirmataStepper::DRIVER)
|
|
{
|
|
digitalWrite(dir_pin, direction);
|
|
delayMicroseconds(this->stepDelay);
|
|
digitalWrite(step_pin, LOW);
|
|
delayMicroseconds(this->stepDelay);
|
|
digitalWrite(step_pin, HIGH);
|
|
}
|
|
else if (this->interface == FirmataStepper::TWO_WIRE)
|
|
{
|
|
switch (step_num)
|
|
{
|
|
case 0: /* 01 */
|
|
digitalWrite(motor_pin_1, LOW);
|
|
digitalWrite(motor_pin_2, HIGH);
|
|
break;
|
|
case 1: /* 11 */
|
|
digitalWrite(motor_pin_1, HIGH);
|
|
digitalWrite(motor_pin_2, HIGH);
|
|
break;
|
|
case 2: /* 10 */
|
|
digitalWrite(motor_pin_1, HIGH);
|
|
digitalWrite(motor_pin_2, LOW);
|
|
break;
|
|
case 3: /* 00 */
|
|
digitalWrite(motor_pin_1, LOW);
|
|
digitalWrite(motor_pin_2, LOW);
|
|
break;
|
|
}
|
|
}
|
|
else if (this->interface == FirmataStepper::FOUR_WIRE)
|
|
{
|
|
switch (step_num)
|
|
{
|
|
case 0: // 1010
|
|
digitalWrite(motor_pin_1, HIGH);
|
|
digitalWrite(motor_pin_2, LOW);
|
|
digitalWrite(motor_pin_3, HIGH);
|
|
digitalWrite(motor_pin_4, LOW);
|
|
break;
|
|
case 1: // 0110
|
|
digitalWrite(motor_pin_1, LOW);
|
|
digitalWrite(motor_pin_2, HIGH);
|
|
digitalWrite(motor_pin_3, HIGH);
|
|
digitalWrite(motor_pin_4, LOW);
|
|
break;
|
|
case 2: //0101
|
|
digitalWrite(motor_pin_1, LOW);
|
|
digitalWrite(motor_pin_2, HIGH);
|
|
digitalWrite(motor_pin_3, LOW);
|
|
digitalWrite(motor_pin_4, HIGH);
|
|
break;
|
|
case 3: //1001
|
|
digitalWrite(motor_pin_1, HIGH);
|
|
digitalWrite(motor_pin_2, LOW);
|
|
digitalWrite(motor_pin_3, LOW);
|
|
digitalWrite(motor_pin_4, HIGH);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @return The version number of this library.
|
|
*/
|
|
byte FirmataStepper::version(void)
|
|
{
|
|
return 2;
|
|
}
|