alvierahman90/MMME3085_Lab_2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add lab resources

Browse Source
This commit is contained in:
Akbar Rahman
2023-11-29 23:52:08 +00:00
parent f766105846
commit 78850704ba
11 changed files with 2099 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
res/Lab 2 - Motion Control 23-24.pdf Normal file
View File

Binary file not shown.

BIN
res/LeibRamp.pdf Normal file
View File

Binary file not shown.

204
res/lab_2/LeibRampStepper.c Normal file
View File

@@ -0,0 +1,204 @@
#include <stdio.h>
#include <stdlib.h>
#include <windows.h>
#include <math.h>
/* Only needed in Windows program to maintain compatibility with Arduino version of C/C++ */
#define bool BOOL
#define true 1
#define false 0
const bool FWDS = true;
const bool BWDS = false;
const long ticksPerSec = 1000; // ms on PC
// on Arduino it is 1E6 for micros (for s/w) or 1.6E7 for 62.5 ns ticks (for h/w)
/* Function prototypes: */
/* PC only, don't need function prototypes on Arduino as they get added within compilation process */
void moveOneStep();
void computeNewSpeed();
long computeStepsToGo();
void goToPosition(long newPosition);
void printLoop();
long millis(void);
/* Note: we are using global variables ONLY to preserve compatibility with the Arduino program structure.
They should not normally be used in C or C++ programs as they make for a poor software design. */
/* 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 */
float maxSpeed; /* Maximum speed in present movement (not nec. max permitted) */
bool direction; /* Direction of present movement: FWDS or BWDS */
/* Global variables used in simplistic and Leib Ramp algorithms */
volatile float p; /* Step interval in clock ticks or microseconds */
float p1, ps; /* Minimum and maximum step periods */
float deltaP; /* You'll be able to get rid of this later */
float R; /* Multiplying constant used in Eiderman's algorithm */
/* Global variable used for noting previous time of a step in timed loop and for calculating speed and accel */
long prevStepTime=0;
long millisAtStart;
float prevSpeed=0.0;
/* Define permissible parameters for motor */
// For testing on PC only, not for use in Arduino program: try movements in order of 50-100 steps
float accelSteps=20; /* leave this as a variable as we may over-write it */
const float minSpeed = 1.0;
const float maxPermissSpeed = 20.0;
const float maxAccel = 10.0;
int main()
{
unsigned long currentMillis = millis();
prevStepTime = 0;
long positionToMoveTo;
while(true)
{
printf("Enter position to move to in profile (or 999 to terminate)\n");
scanf("%ld", &positionToMoveTo);
if (positionToMoveTo==999) break;
printf(" Time (s), Speed (steps/s), Accel (steps/s^2), Posit'n (steps), Step time (ticks)\n");
goToPosition(positionToMoveTo);
/* -------------------------------------------------------------- */
/* Start of pre-computation code - only executed once per profile */
// STEP 1
// Define number of steps in acceleration phase using Equation (3)
accelSteps = (long)(( maxPermissSpeed * maxPermissSpeed - minSpeed * minSpeed) / ( 2.0 * (float)maxAccel)); // Equation 4 but need to consider initial speed
stepsToGo = computeStepsToGo();
maxSpeed = maxPermissSpeed;
if (2 * accelSteps > stepsToGo)
{
// STEP 2
// Define maximum speed in profile and number of steps in acceleration phase
// Use Equations (4) and (5)
maxSpeed = sqrt(minSpeed * minSpeed + stepsToGo * maxAccel); // Modified version of eq. 5
accelSteps = (long)(stepsToGo / 2);
}
// STEPS 3 and 5
// Calculate initial value of and p1 and R Set p = p1
p1 = (float)ticksPerSec / sqrt( minSpeed * minSpeed + 2 * maxAccel); // Eq 17 incorporating initial velocity
p = p1;
R = (float) maxAccel / ((float)ticksPerSec * (float)ticksPerSec); // Eq 19
ps = ((float)ticksPerSec) / maxSpeed; // STEP 4 Eq 7 in paper
/* End of pre-computation code */
/* -------------------------------------------------------------- */
millisAtStart = millis(); /* Needed only to tabulate speed vs. time */
/* Timed loop for stepping, and associated coding */
while(stepsToGo > 0)
{
currentMillis = millis();
if (currentMillis - prevStepTime >= p)
{
moveOneStep();
prevStepTime = currentMillis;
computeNewSpeed();
}
}
}
return 0;
}
/* Only needed for compatibility with Arduino program because millis() is not a native Windows API function */
long millis(void)
{
return GetTickCount();
}
/* Move a single step. */
void moveOneStep()
{
if (p != 0) /* p=0 is code for "don't make steps" */
{
// Print to screen instead of making a step
if (direction == FWDS)
{
currentPosition++;
}
else
{
currentPosition--;
}
/* Instead of actually making step, print out parameters for current step */
float speed = (float)(ticksPerSec)/p;
float accel = (float)(ticksPerSec)*(speed-prevSpeed)/p;
printf("%16.3f, %16.3f, %16.3f, %16ld, %16.3f\n", 0.001*(millis()-millisAtStart), speed, accel, currentPosition, p);
prevSpeed = speed;
}
}
/* Calcuate new value of step interval p based on constants defined in loop() */
void computeNewSpeed()
{
float q;
float m;
stepsToGo = computeStepsToGo();
/* ----------------------------------------------------------------- */
/* Start of on-the-fly step calculation code, executed once per step */
if (stepsToGo == 0) // STEP 6a
{
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; // Equation (9)
}
else if (stepsToGo <= accelSteps) // Slowing down
{
m = R; // Equation 10
}
else // Running at constant speed
{
m = 0; // Equation (11)
}
/* else p is unchanged: running at constant speed */
/* Update to step interval based on Eiderman's algorithm, using temporary variables */
// STEP 6b, c and d using Equations (12) and (13)
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 > 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;
}
}

286
res/lab_2/LeibRampStepper/LeibRampStepper.ino Normal file
View File

@@ -0,0 +1,286 @@
/* 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");
}

209
res/lab_2/LeibRampStepperMac.c Normal file
View File

@@ -0,0 +1,209 @@
// Leib Ramp Stepper program with millis function adapted for MacOS
#include <stdio.h>
#include <stdlib.h>
#include <windows.h>
#include <math.h>
#include <time.h>
/* Only needed in Windows program to maintain compatibility with Arduino version of C/C++ */
typedef enum { false, true } bool;
#define true 1
#define false 0
const bool FWDS = true;
const bool BWDS = false;
const long ticksPerSec = 1000; // ms on PC
// on Arduino it is 1E6 for micros (for s/w) or 1.6E7 for 62.5 ns ticks (for h/w)
/* Function prototypes: */
/* PC only, don't need function prototypes on Arduino as they get added within compilation process */
void moveOneStep();
void computeNewSpeed();
long computeStepsToGo();
void goToPosition(long newPosition);
void printLoop();
long millis(void);
/* Note: we are using global variables ONLY to preserve compatibility with the Arduino program structure.
They should not normally be used in C or C++ programs as they make for a poor software design. */
/* 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 */
float maxSpeed; /* Maximum speed in present movement (not nec. max permitted) */
bool direction; /* Direction of present movement: FWDS or BWDS */
/* Global variables used in simplistic and Leib Ramp algorithms */
volatile float p; /* Step interval in clock ticks or microseconds */
float p1, ps; /* Minimum and maximum step periods */
float deltaP; /* You'll be able to get rid of this later */
float R; /* Multiplying constant used in Eiderman's algorithm */
/* Global variable used for noting previous time of a step in timed loop and for calculating speed and accel */
long prevStepTime=0;
long millisAtStart;
float prevSpeed=0.0;
/* Define permissible parameters for motor */
// For testing on PC only, not for use in Arduino program: try movements in order of 50-100 steps
float accelSteps=20; /* leave this as a variable as we may over-write it */
const float minSpeed = 1.0;
const float maxPermissSpeed = 20.0;
const float maxAccel = 10.0;
int main()
{
unsigned long currentMillis = millis();
prevStepTime = 0;
long positionToMoveTo;
while(true)
{
printf("Enter position to move to in profile (or 999 to terminate)\n");
scanf("%ld", &positionToMoveTo);
if (positionToMoveTo==999) break;
printf(" Time (s), Speed (steps/s), Accel (steps/s^2), Posit'n (steps), Step time (ticks)\n");
goToPosition(positionToMoveTo);
/* -------------------------------------------------------------- */
/* Start of pre-computation code - only executed once per profile */
// STEP 1
// Define number of steps in acceleration phase using Equation (3)
accelSteps = (long)(( maxPermissSpeed * maxPermissSpeed - minSpeed * minSpeed) / ( 2.0 * (float)maxAccel)); // Equation 4 but need to consider initial speed
stepsToGo = computeStepsToGo();
maxSpeed = maxPermissSpeed;
if (2 * accelSteps > stepsToGo)
{
// STEP 2
// Define maximum speed in profile and number of steps in acceleration phase
// Use Equations (4) and (5)
maxSpeed = sqrt(minSpeed * minSpeed + stepsToGo * maxAccel); // Modified version of eq. 5
accelSteps = (long)(stepsToGo / 2);
}
// STEPS 3 and 5
// Calculate initial value of and p1 and R Set p = p1
p1 = (float)ticksPerSec / sqrt( minSpeed * minSpeed + 2 * maxAccel); // Eq 17 incorporating initial velocity
p = p1;
R = (float) maxAccel / ((float)ticksPerSec * (float)ticksPerSec); // Eq 19
ps = ((float)ticksPerSec) / maxSpeed; // STEP 4 Eq 7 in paper
/* End of pre-computation code */
/* -------------------------------------------------------------- */
millisAtStart = millis(); /* Needed only to tabulate speed vs. time */
/* Timed loop for stepping, and associated coding */
while(stepsToGo > 0)
{
currentMillis = millis();
if (currentMillis - prevStepTime >= p)
{
moveOneStep();
prevStepTime = currentMillis;
computeNewSpeed();
}
}
}
return 0;
}
/* Only needed for compatibility with Arduino program because millis() is not a native MacOS function */
long millis(void)
{
struct timespec _t;
clock_gettime(CLOCK_REALTIME, &_t);
return _t.tv_sec*1000 + lround(_t.tv_nsec/1e6);
}
/* Move a single step. */
void moveOneStep()
{
if (p != 0) /* p=0 is code for "don't make steps" */
{
// Print to screen instead of making a step
if (direction == FWDS)
{
currentPosition++;
}
else
{
currentPosition--;
}
/* Instead of actually making step, print out parameters for current step */
float speed = (float)(ticksPerSec)/p;
float accel = (float)(ticksPerSec)*(speed-prevSpeed)/p;
printf("%16.3f, %16.3f, %16.3f, %16ld, %16.3f\n", 0.001*(millis()-millisAtStart), speed, accel, currentPosition, p);
prevSpeed = speed;
}
}
/* Calcuate new value of step interval p based on constants defined in loop() */
void computeNewSpeed()
{
float q;
float m;
stepsToGo = computeStepsToGo();
/* ----------------------------------------------------------------- */
/* Start of on-the-fly step calculation code, executed once per step */
if (stepsToGo == 0) // STEP 6a
{
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; // Equation (9)
}
else if (stepsToGo <= accelSteps) // Slowing down
{
m = R; // Equation 10
}
else // Running at constant speed
{
m = 0; // Equation (11)
}
/* else p is unchanged: running at constant speed */
/* Update to step interval based on Eiderman's algorithm, using temporary variables */
// STEP 6b, c and d using Equations (12) and (13)
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 > 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;
}
}

247
res/lab_2/PIDClosedLoop/PIDClosedLoop.ino Normal file
View File

@@ -0,0 +1,247 @@
/* Example of driving servomotor using PID closed loop control */
#include <SPI.h> /* Needed to communicate with LS7366R (Counter Click) */
#include <PID_v1.h>
/* 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 float */
/* LS7366R aspects very loosely based on concepts used in controlling
the Robogaia 3-axis encoder shield though implementation is very different
https://www.robogaia.com/3-axis-encoder-conter-arduino-shield.html */
/* Pins used for L298 driver */
const int enA = 13; /* PWM output, also visible as LED */
const int in1 = 8; /* H bridge selection input 1 */
const int in2 = 9; /* H bridge selection input 2 */
const float minPercent = -100.0;
const float maxPercent = 100.0;
/* Used to to initiate SPI communication to LS7366R chip (Counter click) */
const int chipSelectPin = 10;
/* Size of buffer used to store received characters */
enum {numChars = 32};
/* Intervals in milliseconds for user-defined timed loops */
const int printInterval = 1000;
const int controlInterval = 20;
/* Global variables used in serial input */
char receivedChars[numChars]; // an array to store the received data
float dataNumber = 0;
boolean newData = false;
/* Global variables used for motor control and encoder reading */
double percentSpeed = 0;
double encoderPosnMeasured = 0;
double positionSetPoint = 0;
/* PID */
double Kp = 0.1;
double Ki = 0.1;
double Kd = 0.05;
PID myPID(&encoderPosnMeasured, &percentSpeed, &positionSetPoint, Kp, Ki, Kd, DIRECT);
/* Global variables used for loop timing */
unsigned long prevMillisPrint = 0; /* stores last time values were printed */
unsigned long prevMillisControl = 0;
/* Overlapping regions of memory used to convert four bytes to a long integer */
union fourBytesToLong
{
long result;
unsigned char bytes [4];
};
void setup()
{
Serial.begin(9600);
Serial.print("Kp"); Serial.print(Kp);
Serial.print("Ki"); Serial.print(Ki);
Serial.print("Kd"); Serial.println(Kd);
Serial.println("Enter desired motor position: ");
/* Set up and initialise pin used for selecting LS7366R counter: hi=inactive */
pinMode(chipSelectPin, OUTPUT);
digitalWrite(chipSelectPin, HIGH);
SetUpLS7366RCounter();
delay(100);
/* Configure control pins for L298 H bridge */
pinMode(enA, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
/* Set initial rotation direction */
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
delay(100);
positionSetPoint = 0;
encoderPosnMeasured=readEncoderCountFromLS7366R();
myPID.SetOutputLimits(-100,100);
myPID.SetMode(AUTOMATIC);
}
void loop()
{
unsigned long currentMillis = millis();
// Call control loop at frequency controInterval
if (currentMillis - prevMillisControl >= controlInterval) {
// save the last time the control loop was called
prevMillisControl = currentMillis;
controlLoop();
}
// Call print loop at frequency of printInterval
if (currentMillis - prevMillisPrint >= printInterval) {
// save the last time you printed output
prevMillisPrint = currentMillis;
printLoop();
}
recvWithEndMarker();// Update value read from serial line
// If a valid number has been read this is set to the current required position
if(convertNewNumber()){
positionSetPoint=dataNumber;
}
}
void controlLoop()
{
// Get the current position from the encoder
encoderPosnMeasured=readEncoderCountFromLS7366R(); // Get current motor position
myPID.Compute(); // Use the PID library to compute new value for motor input
driveMotorPercent(percentSpeed); // Send value to motor
}
void driveMotorPercent(double percentSpeed)
/* Output PWM and H bridge signals based on positive or negative duty cycle % */
{
percentSpeed = constrain(percentSpeed, -100, 100);
int regVal = map(percentSpeed, -100, 100, -255, 255);
analogWrite(enA, (int)abs(regVal)); // Write value to speed control pin
digitalWrite(in1, regVal>0); // Set the value of direction control pins to true or false
digitalWrite(in2, !(regVal>0));
}
void printLoop()
/* Print count and control information */
{
double error;
Serial.print("Actual position: ");
Serial.print(encoderPosnMeasured);
Serial.print("\t");
Serial.print("Desired position: ");
Serial.print(positionSetPoint);
Serial.print("\t");
error = positionSetPoint - encoderPosnMeasured;
Serial.print("Error: ");
Serial.print(error);
Serial.print("\t");
// Serial.print(percentSpeed);
Serial.print("\r\n");
}
long readEncoderCountFromLS7366R()
/* Reads the LS7366R chip to obtain up/down count from encoder. Reads four
bytes separately then concverts them to a long integer using a union */
{
fourBytesToLong converter; /* Union of four bytes and a long integer */
digitalWrite(chipSelectPin,LOW); /* Make LS7366R active */
SPI.transfer(0x60); // Request count
converter.bytes[3] = SPI.transfer(0x00); /* Read highest order byte */
converter.bytes[2] = SPI.transfer(0x00);
converter.bytes[1] = SPI.transfer(0x00);
converter.bytes[0] = SPI.transfer(0x00); /* Read lowest order byte */
digitalWrite(chipSelectPin,HIGH); /* Make LS7366R inactive */
return converter.result;
}
void SetUpLS7366RCounter(void)
/* Initialises LS7366R hardware counter on Counter Click board to read quadrature signals */
{
/* Control registers in LS7366R - see LS7366R datasheet for this and subsequent control words */
unsigned char IR = 0x00, MRD0=0x00;
// SPI initialization
SPI.begin();
//SPI.setClockDivider(SPI_CLOCK_DIV16); // SPI at 1Mhz (on 16Mhz clock)
delay(10);
/* Configure as free-running 4x quadrature counter */
digitalWrite(chipSelectPin,LOW); /* Select chip and initialise transfer */
/* Instruction register IR */
IR |= 0x80; /* Write to register (B7=1, B6=0) */
IR |= 0x08; /* Select register MDR0: B5=0, B4=0, B3=1 */
SPI.transfer(IR); /* Write to instruction register */
/* Mode register 0 */
MRD0 |= 0x03; /* 4x quadrature count: B0=1, B1=1 */
/* B2=B3=0: free running. B4=B5=0: disable index. */
/* B6=0: asynchronous index. B7: Filter division factor = 1. */
SPI.transfer(MRD0);
digitalWrite(chipSelectPin,HIGH);
/* Clear the counter i.e. set it to zero */
IR = 0x00; /* Clear the instructino register IR */
digitalWrite(chipSelectPin,LOW); /* Select chip and initialise transfer */
IR |= 0x20; /* Select CNTR: B5=1,B4=0,B3=0; CLR register: B7=0,B6=0 */
SPI.transfer(IR); /* Write to instruction register */
digitalWrite(chipSelectPin,HIGH);
}
void recvWithEndMarker()
/* Receive data from serial port finishing with "newline" character.
Based on http://forum.arduino.cc/index.php?topic=396450 Example 4 */
{
static byte ndx = 0;
char endMarker = '\n';
char rc;
if (Serial.available() > 0) { // If data is available read value from serial monitor
rc = Serial.read();
if (rc != endMarker) { // Store character in buffer if not end marker
receivedChars[ndx] = rc;
ndx++;
if (ndx >= numChars) {
ndx = numChars - 1;
}
}
else { // Add end of string character and change flag to indicate new data is available
receivedChars[ndx] = '\0'; // terminate the string
ndx = 0;
newData = true;
}
}
}
bool convertNewNumber()
/* Converts character string to floating point number only if there are new
data to convert, otherwise returns false */
{
if (newData) {
dataNumber = 0.0;
dataNumber = atof(receivedChars);
newData = false;
return true;
}
else
{
return false;
}
}

235
res/lab_2/ProportionalClosedLoop/ProportionalClosedLoop.ino Normal file
View File

@@ -0,0 +1,235 @@
/* Example of driving servomotor using simple proportional closed loop control */
#include <SPI.h> /* Needed to communicate with LS7366R (Counter Click) */
/* 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 float */
/* LS7366R aspects very loosely based on concepts used in controlling
the Robogaia 3-axis encoder shield though implementation is very different
https://www.robogaia.com/3-axis-encoder-conter-arduino-shield.html */
/* Pins used for L298 driver */
const int enA = 13; /* PWM output, also visible as LED */
const int in1 = 8; /* H bridge selection input 1 */
const int in2 = 9; /* H bridge selection input 2 */
const float minPercent = -100.0;
const float maxPercent = 100.0;
/* Used to to initiate SPI communication to LS7366R chip (Counter click) */
const int chipSelectPin = 10;
/* Size of buffer used to store received characters */
enum {numChars = 32};
/* Intervals in milliseconds for user-defined timed loops */
const int printInterval = 1000;
const int controlInterval = 20;
/* Global variables used in serial input */
char receivedChars[numChars]; // an array to store the received data
float dataNumber = 0; // new for this version
boolean newData = false;
/* Global variable used for motor control */
double percentDutyCycle;
long encoderPosnMeasured = 0;
double positionSetPoint = 0;
/* Proportional gain constant */
double Kp = 0.1;
/* Global variables used for loop timing */
unsigned long prevMillisPrint = 0; /* stores last time values were printed */
unsigned long prevMillisControl = 0; /* Stores last time control loop executed */
/* Overlapping regions of memory used to convert four bytes to a long integer */
union fourBytesToLong
{
long result;
unsigned char bytes [4];
};
void setup()
{
Serial.begin(9600);
Serial.println("Enter desired motor position: ");
/* Set up and initialise pin used for selecting LS7366R counter: hi=inactive */
pinMode(chipSelectPin, OUTPUT);
digitalWrite(chipSelectPin, HIGH);
SetUpLS7366RCounter();
delay(100);
/* Configure control pins for L298 H bridge */
pinMode(enA, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
/* Set initial rotation direction */
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
}
void loop()
{
unsigned long currentMillis = millis();
// Call control loop at frequency controInterval
if (currentMillis - prevMillisControl >= controlInterval)
{
// Save the current time for comparison the next time the loop is called
prevMillisControl = currentMillis;
controlLoop();
}
// Call print loop at frequency of printInterval
if (currentMillis - prevMillisPrint >= printInterval)
{
// Save the current time for comparison the next time the loop is called
prevMillisPrint = currentMillis;
printLoop();
}
recvWithEndMarker(); // Update value read from serial line
// If a valid number has been read this is set to the current required position
if(convertNewNumber())
{
positionSetPoint = dataNumber;
}
}
void controlLoop()
{
double error;
// Get the current position from the encoder
encoderPosnMeasured = readEncoderCountFromLS7366R();
// Calculate the difference in position from the required position
error = positionSetPoint - (double)encoderPosnMeasured;
// Multiply by the gain
percentDutyCycle = error * Kp;
driveMotorPercent(percentDutyCycle);
}
/* Output PWM and H bridge signals based on positive or negative duty cycle % */
void driveMotorPercent(double percentDutyCycle)
{
// constrain the duty cycle to a value between -100 and 100 then map to +-255
percentDutyCycle = constrain(percentDutyCycle, -100, 100);
int regVal = map(percentDutyCycle, -100, 100, -255, 255);
analogWrite(enA, (int)abs(regVal)); // Write value to speed control pin
digitalWrite(in1, regVal>0); // Set the value of direction control pins to true or false
digitalWrite(in2, !(regVal>0));
}
/* Print count and control information */
void printLoop()
{ Serial.print("Actual position: ");
Serial.print(encoderPosnMeasured);
Serial.print("\t");
Serial.print("Desired position: ");
Serial.print(positionSetPoint);
Serial.print("\t");
Serial.print("Error: ");
Serial.print(positionSetPoint - (double)encoderPosnMeasured);
Serial.print("\r\n");
}
/* Reads the LS7366R chip to obtain up/down count from encoder. Reads four
bytes separately then concverts them to a long integer using a union */
long readEncoderCountFromLS7366R()
{
fourBytesToLong converter; /* Union of four bytes and a long integer */
digitalWrite(chipSelectPin,LOW); /* Make LS7366R active */
SPI.transfer(0x60); // Request count
converter.bytes[3] = SPI.transfer(0x00); /* Read highest order byte */
converter.bytes[2] = SPI.transfer(0x00);
converter.bytes[1] = SPI.transfer(0x00);
converter.bytes[0] = SPI.transfer(0x00); /* Read lowest order byte */
digitalWrite(chipSelectPin,HIGH); /* Make LS7366R inactive */
return converter.result;
}
/* Initialises LS7366R hardware counter on Counter Click board to read quadrature signals */
void SetUpLS7366RCounter(void)
{
/* Control registers in LS7366R - see LS7366R datasheet for this and subsequent control words */
unsigned char IR = 0x00, MRD0=0x00;
// SPI initialization
SPI.begin();
//SPI.setClockDivider(SPI_CLOCK_DIV16); // SPI at 1Mhz (on 16Mhz clock)
delay(10);
/* Configure as free-running 4x quadrature counter */
digitalWrite(chipSelectPin,LOW); /* Select chip and initialise transfer */
/* Instruction register IR */
IR |= 0x80; /* Write to register (B7=1, B6=0) */
IR |= 0x08; /* Select register MDR0: B5=0, B4=0, B3=1 */
SPI.transfer(IR); /* Write to instruction register */
/* Mode register 0 */
MRD0 |= 0x03; /* 4x quadrature count: B0=1, B1=1 */
/* B2=B3=0: free running. B4=B5=0: disable index. */
/* B6=0: asynchronous index. B7: Filter division factor = 1. */
SPI.transfer(MRD0);
digitalWrite(chipSelectPin,HIGH);
/* Clear the counter i.e. set it to zero */
IR = 0x00; /* Clear the instructino register IR */
digitalWrite(chipSelectPin,LOW); /* Select chip and initialise transfer */
IR |= 0x20; /* Select CNTR: B5=1,B4=0,B3=0; CLR register: B7=0,B6=0 */
SPI.transfer(IR); /* Write to instruction register */
digitalWrite(chipSelectPin,HIGH);
}
/* 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) { // If data is available read value from serial monitor
rc = Serial.read();
if (rc != endMarker) { // Store character in buffer if not end marker
receivedChars[ndx] = rc;
ndx++;
if (ndx >= numChars) {
ndx = numChars - 1;
}
}
else { // Add end of string character and change flag to indicate new data is available
receivedChars[ndx] = '\0'; // terminate the string
ndx = 0;
newData = true;
}
}
}
bool convertNewNumber()
/* Converts character string to floating point number only if there are new
data to convert, otherwise returns false */
{
if (newData) {
dataNumber = 0.0;
dataNumber = atof(receivedChars);
newData = false;
return true;
}
else
{
return false;
}
}

208
res/lab_2/SimpleControlNoFeedback/SimpleControlNoFeedback.ino Normal file
View File

@@ -0,0 +1,208 @@
/* Example of driving servomotor and reading encoder signals */
#include <SPI.h> /* Needed to communicate with LS7366R (Counter Click) */
/* 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 float */
/* LS7366R aspects very loosely based on concepts used in controlling
the Robogaia 3-axis encoder shield though implementation is very different
https://www.robogaia.com/3-axis-encoder-conter-arduino-shield.html */
/* Pins used for L298 DC Motor driver */
#define enA 13 /* PWM output, also visible as LED */
#define in1 8 /* H bridge selection input 1 */
#define in2 9 /* H bridge selection input 2 */
#define minPercent -100.0
#define maxPercent 100.0
/* Used to to initiate SPI communication to LS7366R chip (Counter click) */
#define chipSelectPin 10
/* Size of buffer used to store received characters */
#define numChars 32
/* Intervals in milliseconds for user-defined timed loops */
#define printInterval 1000
/* Global variables used in serial input */
char receivedChars[numChars]; // an array to store the received data
float dataNumber = 0;
boolean newData = false;
/* Global variable used for motor control */
double percentSpeed;
/* Global variables used for loop timing */
unsigned long prevMillisPrint = 0; /* stores last time values were printed */
/* Overlapping regions of memory used to convert four bytes to a long integer */
union fourBytesToLong
{
long result;
unsigned char bytes [4];
};
void setup()
{
Serial.begin(9600);
Serial.println("Enter PWM duty cycle as a percentage (positive for forward, negative for reverse");
/* Set up and initialise pin used for selecting LS7366R counter: hi=inactive */
pinMode(chipSelectPin, OUTPUT);
digitalWrite(chipSelectPin, HIGH);
SetUpLS7366RCounter();
delay(100);
/* Configure control pins for L298 H bridge */
pinMode(enA, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
/* Set initial rotation direction */
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
}
void loop()
{
unsigned long currentMillis = millis();
// Print out value to serial monitor at interval specified by printInterval variable
if (currentMillis - prevMillisPrint >= printInterval) {
// save the last time you printed output
prevMillisPrint = currentMillis;
printLoop();
}
// Check if new data has been input via serial monitor
recvWithEndMarker();
if(convertNewNumber()) // Update value read from serial line
{
percentSpeed=dataNumber;
driveMotorPercent(percentSpeed); // Send new speed value to motor driver
}
}
void driveMotorPercent(double percentSpeed)
/* Output PWM and H bridge signals based on positive or negative duty cycle % */
{
percentSpeed = constrain(percentSpeed, -100, 100); // Value must be in range -100 to +100
int regVal = map(percentSpeed, -100, 100, -255, 255); // Scale value to range -255 to +255
analogWrite(enA, (int)abs(regVal)); // Write value to speed control pin
digitalWrite(in1, regVal>0); // Set the value of direction control pins to true or false
digitalWrite(in2, !(regVal>0)); // depending on whether speed is positive or negative
}
void printLoop()
/* Print count and control information */
{
/* Sample counter chip and output position and requested speed */
long encoderCountFromLS7366R = readEncoderCountFromLS7366R();
Serial.print("Count from LS7366R = ");
Serial.print(encoderCountFromLS7366R);
Serial.print(" Percent speed = ");
Serial.print(percentSpeed);
Serial.print("\r\n");
}
long readEncoderCountFromLS7366R()
/* Reads the LS7366R chip to obtain up/down count from encoder. Reads four
bytes separately then concverts them to a long integer using a union */
{
fourBytesToLong converter; /* Union of four bytes and a long integer */
digitalWrite(chipSelectPin,LOW); /* Make LS7366R active */
SPI.transfer(0x60); // Request count
converter.bytes[3] = SPI.transfer(0x00); /* Read highest order byte */
converter.bytes[2] = SPI.transfer(0x00);
converter.bytes[1] = SPI.transfer(0x00);
converter.bytes[0] = SPI.transfer(0x00); /* Read lowest order byte */
digitalWrite(chipSelectPin,HIGH); /* Make LS7366R inactive */
return converter.result;
}
void SetUpLS7366RCounter(void)
/* Initialiseds LS7366R hardware counter on Counter Click board to read quadrature signals */
{
/* Control registers in LS7366R - see LS7366R datasheet for this and subsequent control words */
unsigned char IR = 0x00, MRD0=0x00;
// SPI initialization
SPI.begin();
//SPI.setClockDivider(SPI_CLOCK_DIV16); // SPI at 1Mhz (on 16Mhz clock)
delay(10);
/* Configure as free-running 4x quadrature counter */
digitalWrite(chipSelectPin,LOW); /* Select chip and initialise transfer */
/* Instruction register IR */
IR |= 0x80; /* Write to register (B7=1, B6=0) */
IR |= 0x08; /* Select register MDR0: B5=0, B4=0, B3=1 */
SPI.transfer(IR); /* Write to instruction register */
/* Mode register 0 */
MRD0 |= 0x03; /* 4x quadrature count: B0=1, B1=1 */
/* B2=B3=0: free running. B4=B5=0: disable index. */
/* B6=0: asynchronous index. B7: Filter division factor = 1. */
SPI.transfer(MRD0);
digitalWrite(chipSelectPin,HIGH);
/* Clear the counter i.e. set it to zero */
IR = 0x00; /* Clear the instructino register IR */
digitalWrite(chipSelectPin,LOW); /* Select chip and initialise transfer */
IR |= 0x20; /* Select CNTR: B5=1,B4=0,B3=0; CLR register: B7=0,B6=0 */
SPI.transfer(IR); /* Write to instruction register */
digitalWrite(chipSelectPin,HIGH);
}
void recvWithEndMarker()
/* Receive data from serial port finishing with "newline" character.
Based on http://forum.arduino.cc/index.php?topic=396450 Example 4 */
{
static byte ndx = 0;
char endMarker = '\n';
char rc;
if (Serial.available() > 0) { // If data is available read value from serial monitor
rc = Serial.read();
if (rc != endMarker) { // Store character in buffer if not end marker
receivedChars[ndx] = rc;
ndx++;
if (ndx >= numChars) {
ndx = numChars - 1;
}
}
else { // Add end of string character and change flag to indicate new data is available
receivedChars[ndx] = '\0'; // terminate the string
ndx = 0;
newData = true;
}
}
}
bool convertNewNumber()
/* Converts character string to floating point number only if there are new
data to convert, otherwise returns false */
{
if (newData) {
dataNumber = 0.0;
dataNumber = atof(receivedChars);
newData = false;
return true;
}
else
{
return false;
}
}

183
res/lab_2/SimplisticRampStepper Mac.c Normal file
View File

@@ -0,0 +1,183 @@
// Simplistic Ramp Stepper program with millis function adapted for MacOS
#include <stdio.h>
#include <stdlib.h>
#include <windows.h>
#include <math.h>
#include <time.h>
/* Only needed in Windows program to maintain compatibility with Arduino version of C/C++ */
typedef enum { false, true } bool;
#define true 1
#define false 0
const bool FWDS = true;
const bool BWDS = false;
const long ticksPerSec = 1000; // ms on PC
// on Arduino it is 1E6 for micros (for s/w) or 1.6E7 for 62.5 ns ticks (for h/w)
void moveOneStep();
void computeNewSpeed();
long computeStepsToGo();
void goToPosition(long newPosition);
long millis(void);
/* Note: we are using global variables ONLY to preserve compatibility with the Arduino program structure.
They should not normally be used in C or C++ programs as they make for a poor software design. */
/* 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 */
float maxSpeed; /* Maximum speed in present movement (not nec. max permitted) */
bool direction; /* Direction of present movement: FWDS or BWDS */
/* Global variables used in simplistic and Leib Ramp algorithms */
volatile float p; /* Step interval in clock ticks or microseconds */
float ps; /* Maximum step periods */
float deltaP; /* You'll be able to get rid of this later */
/* Global variable used for noting previous time of a step in timed loop and for calculating speed and accel */
long prevStepTime=0;
long millisAtStart;
float prevSpeed=0.0;
/* Define permissible parameters for motor */
// For testing on PC only, not for use in Arduino program: try movements in order of 50-100 steps
float accelSteps=20; /* leave this as a variable as we may over-write it */
const float minSpeed = 1.0; // in steps/s
const float maxPermissSpeed = 20.0; // in steps/s
const float maxAccel = 10.0; // in steps/s^2
int main()
{
unsigned long currentMillis = millis();
prevStepTime = 0;
long positionToMoveTo;
while(true)
{
printf("Enter position to move to in profile (or 999 to terminate)\n");
scanf("%ld", &positionToMoveTo);
if (positionToMoveTo==999) break;
printf(" Time (s), Speed (steps/s), Accel (steps/s^2), Posit'n (steps), Step time (ticks)\n");
goToPosition(positionToMoveTo);
/* -------------------------------------------------------------- */
/* Start of pre-computation code - only executed once per profile */
float maxInterval = ((float)ticksPerSec) / minSpeed;
ps = ((float)ticksPerSec) / maxPermissSpeed;
deltaP = (maxInterval - ps) / accelSteps;
stepsToGo = computeStepsToGo();
maxSpeed = maxPermissSpeed;
if (2 * accelSteps > stepsToGo)
{
accelSteps = (long)(stepsToGo / 2);
}
p = maxInterval;
ps = ((float)ticksPerSec) / maxSpeed;
/* End of pre-computation code */
/* -------------------------------------------------------------- */
millisAtStart = millis(); /* Needed only to tabulate speed vs. time */
/* Timed loop for stepping */
while(stepsToGo > 0)
{
currentMillis = millis();
if (currentMillis - prevStepTime >= p)
{
moveOneStep();
prevStepTime = currentMillis;
computeNewSpeed();
}
}
}
return 0;
}
/* Only needed for compatibility with Arduino program because millis() is not a native MacOS function */
long millis(void)
{
struct timespec _t;
clock_gettime(CLOCK_REALTIME, &_t);
return _t.tv_sec*1000 + lround(_t.tv_nsec/1e6);
}
/* Move a single step. If this were running on the Arduino we would holding pulse high for delayMicroSeconds */
void moveOneStep()
{
if (p != 0) /* p=0 is code for "don't make steps" */
{
// Print to screen instead of making a step
if (direction == FWDS)
{
currentPosition++;
}
else
{
currentPosition--;
}
/* Instead of actually making step, print out parameters for current step */
float speed = (float)(ticksPerSec)/p;
float accel = (float)(ticksPerSec)*(speed-prevSpeed)/p;
printf("%16.3f, %16.3f, %16.3f, %16ld, %16.3f\n", 0.001*(millis()-millisAtStart), speed, accel, currentPosition, p);
prevSpeed = speed;
}
}
/* Calcuate new value of step interval p based on constants defined in timed loop */
void computeNewSpeed()
{
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) // Changed to make ramps even length
/* Speeding up */
{
p -= deltaP;
}
else if (stepsToGo <= accelSteps)
/* Slowing down */
{
p += deltaP;
}
/* else p is unchanged: running at constant speed */
/* 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;
}
}

178
res/lab_2/SimplisticRampStepper.c Normal file
View File

@@ -0,0 +1,178 @@
#include <stdio.h>
#include <stdlib.h>
#include <windows.h>
#include <math.h>
/* Only needed in Windows program to maintain compatibility with Arduino version of C/C++ */
#define bool BOOL
#define true 1
#define false 0
const bool FWDS = true;
const bool BWDS = false;
const long ticksPerSec = 1000; // ms on PC
// on Arduino it is 1E6 for micros (for s/w) or 1.6E7 for 62.5 ns ticks (for h/w)
void moveOneStep();
void computeNewSpeed();
long computeStepsToGo();
void goToPosition(long newPosition);
long millis(void);
/* Note: we are using global variables ONLY to preserve compatibility with the Arduino program structure.
They should not normally be used in C or C++ programs as they make for a poor software design. */
/* 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 */
float maxSpeed; /* Maximum speed in present movement (not nec. max permitted) */
bool direction; /* Direction of present movement: FWDS or BWDS */
/* Global variables used in simplistic and Leib Ramp algorithms */
volatile float p; /* Step interval in clock ticks or microseconds */
float ps; /* Maximum step periods */
float deltaP; /* You'll be able to get rid of this later */
/* Global variable used for noting previous time of a step in timed loop and for calculating speed and accel */
long prevStepTime=0;
long millisAtStart;
float prevSpeed=0.0;
/* Define permissible parameters for motor */
// For testing on PC only, not for use in Arduino program: try movements in order of 50-100 steps
float accelSteps=20; /* leave this as a variable as we may over-write it */
const float minSpeed = 1.0; // in steps/s
const float maxPermissSpeed = 20.0; // in steps/s
const float maxAccel = 10.0; // in steps/s^2
int main()
{
unsigned long currentMillis = millis();
prevStepTime = 0;
long positionToMoveTo;
while(true)
{
printf("Enter position to move to in profile (or 999 to terminate)\n");
scanf("%ld", &positionToMoveTo);
if (positionToMoveTo==999) break;
printf(" Time (s), Speed (steps/s), Accel (steps/s^2), Posit'n (steps), Step time (ticks)\n");
goToPosition(positionToMoveTo);
/* -------------------------------------------------------------- */
/* Start of pre-computation code - only executed once per profile */
float maxInterval = ((float)ticksPerSec) / minSpeed;
ps = ((float)ticksPerSec) / maxPermissSpeed;
deltaP = (maxInterval - ps) / accelSteps;
stepsToGo = computeStepsToGo();
maxSpeed = maxPermissSpeed;
if (2 * accelSteps > stepsToGo)
{
accelSteps = (long)(stepsToGo / 2);
}
p = maxInterval;
ps = ((float)ticksPerSec) / maxSpeed;
/* End of pre-computation code */
/* -------------------------------------------------------------- */
millisAtStart = millis(); /* Needed only to tabulate speed vs. time */
/* Timed loop for stepping */
while(stepsToGo > 0)
{
currentMillis = millis();
if (currentMillis - prevStepTime >= p)
{
moveOneStep();
prevStepTime = currentMillis;
computeNewSpeed();
}
}
}
return 0;
}
long millis(void)
/* Only needed for compatibility with Arduino program because millis() is not a native Windows API function */
{
return GetTickCount();
}
void moveOneStep()
/* Move a single step. If this were running on the Arduino we would holding pulse high for delayMicroSeconds */
{
if (p != 0) /* p=0 is code for "don't make steps" */
{
// Print to screen instead of making a step
if (direction == FWDS)
{
currentPosition++;
}
else
{
currentPosition--;
}
/* Instead of actually making step, print out parameters for current step */
float speed = (float)(ticksPerSec)/p;
float accel = (float)(ticksPerSec)*(speed-prevSpeed)/p;
printf("%16.3f, %16.3f, %16.3f, %16ld, %16.3f\n", 0.001*(millis()-millisAtStart), speed, accel, currentPosition, p);
prevSpeed = speed;
}
}
/* Calcuate new value of step interval p based on constants defined in timed loop */
void computeNewSpeed()
{
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) // Changed to make ramps even length
/* Speeding up */
{
p -= deltaP;
}
else if (stepsToGo <= accelSteps)
/* Slowing down */
{
p += deltaP;
}
/* else p is unchanged: running at constant speed */
/* 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;
}
}

349
res/lab_2/SimplisticRampStepper/SimplisticRampStepper.ino Normal file
View File

@@ -0,0 +1,349 @@
/* 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 */
// #define USEINTERRUPTS
const int stepPin = 13;
const int dirPin = 9;
const bool FWDS = true;
const bool BWDS = false;
#ifdef USEINTERRUPTS
const long ticksPerSec = 16000000; // Clock speed of Arduino
#else
const long ticksPerSec = 1000000; // microseconds in this case
#endif
/* 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=1000.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");
// If USEINTERRUPTS is defined at the start of the program this section will be used
#ifdef USEINTERRUPTS
cli();
TCCR1A = 0; // No output compare
TCCR1B = 0;
TCCR1B |= (1 << WGM12); //CTC mode
OCR1A = 0; // Set to zero for the present time: catch this to switch interrupt off
TCCR1B |= (1 << CS12); // 256 prescaler: overwritten in ISR
TIMSK1 |= (1 << OCIE1A); //enable timer compare interrupt
sei();
#else //
// Use built-in Arduino function micros() to get the time in microseconds since the program started running.
// Documentation: https://www.arduino.cc/reference/en/language/functions/time/micros/
prevStepTime = micros(); // This is a built-in Arduino function.
#endif
}
void loop()
{
unsigned long currentMillis = millis();
unsigned long currentMicros;
recvWithEndMarker();
stepsToGo = computeStepsToGo();
if (convertNewNumber())
{
/* 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);
/* Delete these and replace with Leib Ramp formulae */
double maxInterval = ((double)ticksPerSec) / minSpeed;
ps = ((double)ticksPerSec) / maxPermissSpeed;
deltaP = (maxInterval - ps) / accelSteps;
/* End of section requiring redefinitions */
stepsToGo = computeStepsToGo();
maxSpeed = maxPermissSpeed;
if (2 * accelSteps > stepsToGo)
{
/* Definiiton of S where ther is no constant speed period - check it is still applicable */
accelSteps = (long)(stepsToGo / 2);
/* Need to redefine maxSpeed here as we never fully accelerate */
}
/* Will need to over-write these with correct initial value of p and p1, along with R */
p = maxInterval;
p1 = (double)ticksPerSec/minSpeed;
/* End of section requiring redefinitions */
ps = ((double)ticksPerSec) / maxSpeed; /* Eq 7 in paper: this is OK */
#ifdef USEINTERRUPTS
if (p != 0)
{
// Re-enable interrupts if non-zero steps
TIMSK1 |= (1 << OCIE1A);
}
#endif
}
}
#ifndef USEINTERRUPTS
/* Timed loop for stepping, and associated coding */
currentMicros = micros();
if (currentMicros - prevStepTime >= p)
{
moveOneStep();
prevStepTime = currentMicros;
computeNewSpeed();
}
#endif
/* Timed loop for printing */
if (currentMillis - prevMillisPrint >= printInterval)
{
/* Save the last time output was printed */
prevMillisPrint = currentMillis;
printLoop();
}
}
void moveOneStep()
/* Move a single step, holding pulse high for delayMicroSeconds */
{
if (p != 0) /* p=0 is code for "don't make steps" */
{
digitalWrite(stepPin, HIGH);
if (direction == FWDS)
{
/* Is something missing here? */
currentPosition++;
}
else
{
/* Is something missing here? */
currentPosition--;
}
delayMicroseconds(stepLengthMus);
digitalWrite(stepPin, LOW);
}
}
void computeNewSpeed()
/* Calcuate new value of step interval p based on constants defined in loop() */
{
/* You may need to declare some temporary variables for this function... */
stepsToGo = computeStepsToGo();
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 */
{
/* Delete this simplistic change to p and replace with something else */
p -= deltaP;
}
else if (stepsToGo <= accelSteps)
/* Slowing down */
{
/* Delete this simplistic change to p and replace with something else */
p += deltaP;
}
else
/* Running at constant speed */
{
/* For simpplistic approach, p=p so do nothing to it.
But you will need to put something here for Leib ramp ... */
}
/* Update to step interval based on Leib ramp algorithm, using temporary variables */
/* Need to ensure rounding error does not cause drift outside acceptable interval range:
replace p with relevant bound if it strays outside - so need to write some code here */
}
long computeStepsToGo()
/* Work out how far the stepper motor still needs to move */
{
if (direction == FWDS)
{
return targetPosition - currentPosition;
}
else
{
return currentPosition - targetPosition;
}
}
void goToPosition(long newPosition)
/* Set the target position and determine direction of intended movement */
{
targetPosition = newPosition;
if (targetPosition - currentPosition > 0)
{
direction = FWDS;
}
else
{
direction = BWDS;
}
}
void recvWithEndMarker()
/* Receive data from serial port finishing with "newline" character.
Based on http://forum.arduino.cc/index.php?topic=396450 Example 4 */
{
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; // new for this version
dataNumber = atol(receivedChars); // new for this version
newData = false;
return true;
}
else
{
return false;
}
}
void printLoop()
/* Print current position of stepper using timed loop */
{
/* Sample all counters one after the other to avoid delay-related offsets */
Serial.print("Current position = ");
Serial.print(currentPosition);
Serial.print("\r\n");
}
#ifdef USEINTERRUPTS
ISR(TIMER1_COMPA_vect)
/* Interrupt service routine which essentially just calls moveOneStep and computeNewSpeed.
However, it also changes the prescale value on-the-fly so that the full range of possible
step rates can be exploited, from around 0.25 Hz upwards, limited by step pulse width. */
{
if (p == 0)
{
// Disable interrupt to avoid endless calling of interrupt if not needed
TIMSK1 &= !(1 << OCIE1A);
}
moveOneStep();
/* Adapt prescaler to keep OCR1A as large as possible within acceptable range */
if (p < 65536)
{
// Prescaler 1
OCR1A = (long)p - 1;
TCCR1B = (TCCR1B & 0xF8) | 0x01;
}
else if (p < 524288)
{
// Prescaler 8
OCR1A = ((long)p >> 3) - 1;
TCCR1B = (TCCR1B & 0xF8) | 0x02;
}
else if (p < 4194304)
{
// Prescaler 64
OCR1A = ((long)p >> 6) - 1;
TCCR1B = (TCCR1B & 0xF8) | 0x03;
}
else if (p < 16777216)
{
// Prescaler 256
OCR1A = ((long)p >> 8) - 1;
TCCR1B = (TCCR1B & 0xF8) | 0x04;
}
else
{
// Prescaler 1024
OCR1A = ((long)p >> 10) - 1;
TCCR1B = (TCCR1B & 0xF8) | 0x05;
}
computeNewSpeed();
}
#endif