287 lines
8.8 KiB
C++
287 lines
8.8 KiB
C++
/* Stepper motor demonstration program written by Arthur Jones,
|
|
4 November 2018. Implements a simplistic and ineffective ramping
|
|
algorithm but provides framework for implementation of LeibRamp
|
|
algorithm described by Aryeh Eiderman, http://hwml.com/LeibRamp.pdf
|
|
|
|
Makes use of background work and some aspects of code developed
|
|
by Choaran Wang, 2017-18. This in turn incorporates some ideas
|
|
used in the AccelStepper library:
|
|
https://www.airspayce.com/mikem/arduino/AccelStepper/
|
|
|
|
Serial input aspects are based closely upon:
|
|
http://forum.arduino.cc/index.php?topic=396450
|
|
Example 4 - Receive a number as text and convert it to an int
|
|
Modified to read a long */
|
|
|
|
const int stepPin = 13;
|
|
const int dirPin = 9;
|
|
const bool FWDS = true;
|
|
const bool BWDS = false;
|
|
|
|
const long ticksPerSec = 1000000; // microseconds in this case
|
|
|
|
|
|
/* Define permissible parameters for motor */
|
|
// For testing by watching LED: try movements in order of 100 steps
|
|
//float accelSteps=20; /* leave this as a variable as we may over-write it */
|
|
//const float minSpeed = 2.0;
|
|
//const float maxPermissSpeed = 20.0;
|
|
//const float maxAccel = 10.0;
|
|
//const long stepLengthMus = 10000;
|
|
// For lab testing with real motor: try movements in the order of 3000 steps
|
|
float accelSteps=1000; /* leave this as a variable as we may over-write it */
|
|
const float minSpeed=10.0;
|
|
const float maxPermissSpeed=100000000.0;
|
|
const float maxAccel=500.0;
|
|
const long stepLengthMus=100;
|
|
|
|
/* Intervals in milliseconds for user-defined timed loops */
|
|
const long printInterval = 1000;
|
|
|
|
/* Global variables used for loop timing */
|
|
unsigned long prevMillisPrint = 0; /* stores last time values were printed */
|
|
|
|
/* Global variables used in serial input */
|
|
enum {numChars = 32};
|
|
char receivedChars[numChars]; /* an array to store the received data */
|
|
long dataNumber = 0; /* Value read from serial monitor input */
|
|
boolean newData = false;
|
|
|
|
/* Global variables relating to stepper motor position counting etc. */
|
|
long stepsToGo; /* Number of steps left to make in present movement */
|
|
long targetPosition; /* Intended destination of motor for given movement */
|
|
volatile long currentPosition = 0; /* Position in steps of motor relative to startup position */
|
|
double maxSpeed; /* Maximum speed in present movement (not nec. max permitted) */
|
|
bool direction; /* Direction of present movement: FWDS or BWDS */
|
|
|
|
volatile float p; /* Step interval in clock ticks or microseconds */
|
|
float p1, ps; /* Minimum and maximum step periods */
|
|
double deltaP; /* You'll be able to get rid of this later */
|
|
double R; /* Multiplying constant used in Eiderman's algorithm */
|
|
|
|
/* Global variable used for noting previous time of a step in timed loop */
|
|
long prevStepTime;
|
|
|
|
void setup()
|
|
{
|
|
long stepsToGo = 0;
|
|
currentPosition = 0;
|
|
goToPosition(dataNumber);
|
|
pinMode(stepPin, OUTPUT);
|
|
pinMode(dirPin, OUTPUT);
|
|
Serial.begin(9600);
|
|
Serial.println("Enter target position in number of steps and hit return");
|
|
|
|
prevStepTime = micros();
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
unsigned long currentMillis = millis();
|
|
unsigned long currentMicros;
|
|
recvWithEndMarker();
|
|
stepsToGo = computeStepsToGo();
|
|
if (convertNewNumber())
|
|
{
|
|
Serial.print("Converted number: datanumber is: ");
|
|
Serial.println(dataNumber);
|
|
// Only get to this stage if there was new data to convert
|
|
if (stepsToGo <= 0)
|
|
{
|
|
// Only get to this stage if not busy, otherwise will have thrown away input
|
|
goToPosition(dataNumber);
|
|
Serial.print("Got target position: ");
|
|
Serial.println(targetPosition);
|
|
|
|
|
|
/* Define number of steps in acceleration phase using Equation (3) */
|
|
accelSteps = long(( maxPermissSpeed * maxPermissSpeed) / ( 2.0 * (double)maxAccel)); // Equation 4 but need to consider initial speed
|
|
stepsToGo = computeStepsToGo();
|
|
maxSpeed = maxPermissSpeed;
|
|
|
|
if (2 * accelSteps > stepsToGo)
|
|
{
|
|
// Define maximum speed in profile and number of steps in acceleration phase
|
|
maxSpeed = sqrt(minSpeed * minSpeed + stepsToGo * maxAccel); // Modified version of eq. 5
|
|
accelSteps = (long)(stepsToGo / 2);
|
|
}
|
|
ps = ((double)ticksPerSec) / maxSpeed; // Eq 7
|
|
|
|
p1 = (double)ticksPerSec / sqrt( minSpeed * minSpeed + 2 * maxAccel); // Eq 17 but need initial velocity
|
|
p = p1;
|
|
R = (double) maxAccel / ((double)ticksPerSec * (double)ticksPerSec); // Eq 19
|
|
}
|
|
}
|
|
|
|
/* Timed loop for stepping */
|
|
currentMicros = micros();
|
|
if (currentMicros - prevStepTime >= p)
|
|
{
|
|
moveOneStep();
|
|
prevStepTime = currentMicros;
|
|
computeNewSpeed();
|
|
}
|
|
|
|
/* Timed loop for printing */
|
|
if (currentMillis - prevMillisPrint >= printInterval)
|
|
{
|
|
// save the last time you printed output
|
|
prevMillisPrint = currentMillis;
|
|
printLoop();
|
|
}
|
|
}
|
|
|
|
/* Move a single step, holding pulse high for delayMicroSeconds */
|
|
void moveOneStep()
|
|
{
|
|
if (p != 0) /* p=0 is code for "don't make steps" */
|
|
{
|
|
digitalWrite(stepPin, HIGH);
|
|
if (direction == FWDS)
|
|
{
|
|
/* Is something missing here? */
|
|
digitalWrite(dirPin, HIGH);
|
|
currentPosition++;
|
|
}
|
|
else
|
|
{
|
|
/* Is something missing here? */
|
|
digitalWrite(dirPin, LOW);
|
|
currentPosition--;
|
|
}
|
|
delayMicroseconds(stepLengthMus);
|
|
digitalWrite(stepPin, LOW);
|
|
}
|
|
}
|
|
|
|
/* Calcuate new value of step interval p based on constants defined in loop() */
|
|
void computeNewSpeed()
|
|
{
|
|
double q;
|
|
double m;
|
|
stepsToGo = computeStepsToGo();
|
|
|
|
/* ----------------------------------------------------------------- */
|
|
/* Start of on-the-fly step calculation code, executed once per step */
|
|
if (stepsToGo == 0)
|
|
{
|
|
p = 0; // Not actually a zero step interval, used to switch stepping off
|
|
return;
|
|
}
|
|
else if (stepsToGo > accelSteps && (long)p > long(ps)) //Speeding up
|
|
{
|
|
m = -R; // definition following equation 20
|
|
}
|
|
else if (stepsToGo <= accelSteps) // Slowing down
|
|
{
|
|
m = R;
|
|
}
|
|
else // Running at constant speed
|
|
{
|
|
m = 0;
|
|
}
|
|
|
|
/* Update to step interval based on Eiderman's algorithm, using temporary variables */
|
|
q = m * p * p; // this is a part of optional enhancement
|
|
p = p * ( 1 + q + 1.5 * q * q); // this is an enhanced approximation -equation [22]
|
|
/* Need to ensure rounding error does not cause drift outside acceptable interval range:
|
|
replace p with relevant bound if it strays outside */
|
|
if (p < ps)
|
|
{
|
|
p = ps;
|
|
}
|
|
if (p > p1)
|
|
{
|
|
p = p1;
|
|
}
|
|
/* End of on-the-fly step calculation code */
|
|
/* ----------------------------------------------------------------- */
|
|
}
|
|
|
|
/* Work out how far the stepper motor still needs to move */
|
|
long computeStepsToGo()
|
|
{
|
|
if (direction == FWDS)
|
|
{
|
|
return targetPosition - currentPosition;
|
|
}
|
|
else
|
|
{
|
|
return currentPosition - targetPosition;
|
|
}
|
|
}
|
|
|
|
/* Set the target position and determine direction of intended movement */
|
|
void goToPosition(long newPosition)
|
|
{
|
|
targetPosition = newPosition;
|
|
if (targetPosition - currentPosition > 0)
|
|
{
|
|
direction = FWDS;
|
|
}
|
|
else
|
|
{
|
|
direction = BWDS;
|
|
}
|
|
}
|
|
|
|
/* Receive data from serial port finishing with "newline" character.
|
|
Based on http://forum.arduino.cc/index.php?topic=396450 Example 4 */
|
|
void recvWithEndMarker()
|
|
{
|
|
static byte ndx = 0;
|
|
char endMarker = '\n';
|
|
char rc;
|
|
|
|
if (Serial.available() > 0)
|
|
{
|
|
rc = Serial.read();
|
|
|
|
if (rc != endMarker)
|
|
{
|
|
receivedChars[ndx] = rc;
|
|
ndx++;
|
|
if (ndx >= numChars)
|
|
{
|
|
ndx = numChars - 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
receivedChars[ndx] = '\0'; // terminate the string
|
|
ndx = 0;
|
|
newData = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
bool convertNewNumber()
|
|
/* Converts character string to long integer only if there are new
|
|
data to convert, otherwise returns false */
|
|
{
|
|
if (newData)
|
|
{
|
|
dataNumber = 0.0;
|
|
dataNumber = atol(receivedChars);
|
|
newData = false;
|
|
Serial.println(dataNumber);
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/* Print current position of stepper using timed loop */
|
|
void printLoop()
|
|
{
|
|
/* Sample all counters one after the other to avoid delay-related offsets */
|
|
Serial.print("Current position = ");
|
|
Serial.print(currentPosition);
|
|
Serial.print("\r\n");
|
|
Serial.print("p = ");
|
|
Serial.print(p);
|
|
Serial.print("\r\n");
|
|
}
|