Compare commits
7 Commits
b9e91a14ab
...
main
| Author | SHA1 | Date | |
|---|---|---|---|
f580f77cf2
|
|||
|
6d16857453
|
|||
|
7799bbf6ea
|
|||
|
1059a7f791
|
|||
|
d18f125995
|
|||
|
d68eb0fa95
|
|||
|
e188f0f2f4
|
1
.gitignore
vendored
1
.gitignore
vendored
@@ -1 +1,2 @@
|
|||||||
out
|
out
|
||||||
|
plots
|
||||||
|
|||||||
5
Makefile
5
Makefile
@@ -20,6 +20,11 @@ $(SUBMISSION_FILENAME): .PHONY $(OUT_DIR)
|
|||||||
$(TEX) $(SRC_DIR)/submission.md -o $(OUT_DIR)/$@ $(TEX_FLAGS)
|
$(TEX) $(SRC_DIR)/submission.md -o $(OUT_DIR)/$@ $(TEX_FLAGS)
|
||||||
mv mermaid-filter.err $(OUT_DIR)
|
mv mermaid-filter.err $(OUT_DIR)
|
||||||
|
|
||||||
|
zip: .PHONY $(OUT_DIR)
|
||||||
|
rm -rf $(OUT_DIR)/lab_2_submission_akbar_alvi_rahman_20386125.zip
|
||||||
|
zip -j $(OUT_DIR)/lab_2_submission_akbar_alvi_rahman_20386125 $(OUT_DIR)/*.pdf
|
||||||
|
zip -r $(OUT_DIR)/lab_2_submission_akbar_alvi_rahman_20386125 images csv src/lab_2
|
||||||
|
|
||||||
$(OUT_DIR):
|
$(OUT_DIR):
|
||||||
mkdir -p $(OUT_DIR)
|
mkdir -p $(OUT_DIR)
|
||||||
|
|
||||||
|
|||||||
51
csv/LeibRampStepper_out_50.csv
Normal file
51
csv/LeibRampStepper_out_50.csv
Normal file
@@ -0,0 +1,51 @@
|
|||||||
|
Time (s), Speed (steps/s), Accel (steps/s^2), Posit'n (steps), Step time (ticks)
|
||||||
|
0.000, 4.583, 21.000, 1, 218.218
|
||||||
|
0.189, 5.304, 3.828, 2, 188.528
|
||||||
|
0.347, 6.359, 6.711, 3, 157.245
|
||||||
|
0.480, 7.531, 8.822, 4, 132.785
|
||||||
|
0.596, 8.653, 9.711, 5, 115.565
|
||||||
|
0.700, 9.688, 10.023, 6, 103.223
|
||||||
|
0.794, 10.640, 10.135, 7, 93.982
|
||||||
|
0.881, 11.523, 10.175, 8, 86.781
|
||||||
|
0.962, 12.348, 10.186, 9, 80.984
|
||||||
|
1.039, 13.124, 10.185, 10, 76.195
|
||||||
|
1.112, 13.859, 10.180, 11, 72.157
|
||||||
|
1.181, 14.558, 10.172, 12, 68.693
|
||||||
|
1.247, 15.225, 10.164, 13, 65.681
|
||||||
|
1.311, 15.865, 10.156, 14, 63.031
|
||||||
|
1.372, 16.481, 10.148, 15, 60.676
|
||||||
|
1.431, 17.075, 10.141, 16, 58.566
|
||||||
|
1.488, 17.649, 10.134, 17, 56.660
|
||||||
|
1.543, 18.205, 10.128, 18, 54.929
|
||||||
|
1.597, 18.745, 10.122, 19, 53.346
|
||||||
|
1.649, 19.270, 10.117, 20, 51.893
|
||||||
|
1.700, 19.782, 10.112, 21, 50.552
|
||||||
|
1.751, 19.782, 0.000, 22, 50.552
|
||||||
|
1.802, 19.782, 0.000, 23, 50.552
|
||||||
|
1.853, 19.782, 0.000, 24, 50.552
|
||||||
|
1.904, 19.782, 0.000, 25, 50.552
|
||||||
|
1.955, 19.782, 0.000, 26, 50.552
|
||||||
|
2.006, 19.782, 0.000, 27, 50.552
|
||||||
|
2.057, 19.782, 0.000, 28, 50.552
|
||||||
|
2.108, 19.782, 0.000, 29, 50.552
|
||||||
|
2.159, 19.782, 0.000, 30, 50.552
|
||||||
|
2.210, 19.782, 0.000, 31, 50.552
|
||||||
|
2.262, 19.270, -9.853, 32, 51.894
|
||||||
|
2.316, 18.745, -9.845, 33, 53.347
|
||||||
|
2.371, 18.205, -9.835, 34, 54.930
|
||||||
|
2.428, 17.648, -9.823, 35, 56.663
|
||||||
|
2.487, 17.074, -9.810, 36, 58.570
|
||||||
|
2.548, 16.479, -9.795, 37, 60.682
|
||||||
|
2.612, 15.863, -9.778, 38, 63.040
|
||||||
|
2.678, 15.222, -9.757, 39, 65.695
|
||||||
|
2.747, 14.553, -9.732, 40, 68.714
|
||||||
|
2.820, 13.853, -9.702, 41, 72.188
|
||||||
|
2.897, 13.116, -9.665, 42, 76.244
|
||||||
|
2.979, 12.336, -9.618, 43, 81.062
|
||||||
|
3.066, 11.506, -9.556, 44, 86.914
|
||||||
|
3.161, 10.613, -9.473, 45, 94.224
|
||||||
|
3.265, 9.643, -9.355, 46, 103.703
|
||||||
|
3.382, 8.572, -9.178, 47, 116.655
|
||||||
|
3.518, 7.365, -8.889, 48, 135.770
|
||||||
|
3.686, 5.962, -8.365, 49, 167.717
|
||||||
|
3.905, 4.583, -6.323, 50, 218.218
|
||||||
|
51
csv/SimplisticRampStepper_out_50.csv
Normal file
51
csv/SimplisticRampStepper_out_50.csv
Normal file
@@ -0,0 +1,51 @@
|
|||||||
|
Time (s), Speed (steps/s), Accel (steps/s^2), Posit'n (steps), Step time (ticks)
|
||||||
|
0.000, 1.000, 1.000, 1, 1000.000
|
||||||
|
0.953, 1.050, 0.052, 2, 952.500
|
||||||
|
1.858, 1.105, 0.061, 3, 905.000
|
||||||
|
2.716, 1.166, 0.071, 4, 857.500
|
||||||
|
3.526, 1.235, 0.084, 5, 810.000
|
||||||
|
4.289, 1.311, 0.101, 6, 762.500
|
||||||
|
5.004, 1.399, 0.122, 7, 715.000
|
||||||
|
5.672, 1.498, 0.149, 8, 667.500
|
||||||
|
6.292, 1.613, 0.185, 9, 620.000
|
||||||
|
6.865, 1.747, 0.234, 10, 572.500
|
||||||
|
7.390, 1.905, 0.301, 11, 525.000
|
||||||
|
7.868, 2.094, 0.397, 12, 477.500
|
||||||
|
8.298, 2.326, 0.538, 13, 430.000
|
||||||
|
8.681, 2.614, 0.755, 14, 382.500
|
||||||
|
9.016, 2.985, 1.107, 15, 335.000
|
||||||
|
9.304, 3.478, 1.715, 16, 287.500
|
||||||
|
9.544, 4.167, 2.868, 17, 240.000
|
||||||
|
9.737, 5.195, 5.341, 18, 192.500
|
||||||
|
9.882, 6.897, 11.736, 19, 145.000
|
||||||
|
9.980, 10.256, 34.460, 20, 97.500
|
||||||
|
10.030, 20.000, 194.872, 21, 50.000
|
||||||
|
10.080, 20.000, 0.000, 22, 50.000
|
||||||
|
10.130, 20.000, 0.000, 23, 50.000
|
||||||
|
10.180, 20.000, 0.000, 24, 50.000
|
||||||
|
10.230, 20.000, 0.000, 25, 50.000
|
||||||
|
10.280, 20.000, 0.000, 26, 50.000
|
||||||
|
10.330, 20.000, 0.000, 27, 50.000
|
||||||
|
10.380, 20.000, 0.000, 28, 50.000
|
||||||
|
10.430, 20.000, 0.000, 29, 50.000
|
||||||
|
10.480, 20.000, 0.000, 30, 50.000
|
||||||
|
10.578, 10.256, -99.934, 31, 97.500
|
||||||
|
10.723, 6.897, -23.171, 32, 145.000
|
||||||
|
10.916, 5.195, -8.840, 33, 192.500
|
||||||
|
11.156, 4.167, -4.284, 34, 240.000
|
||||||
|
11.444, 3.478, -2.394, 35, 287.500
|
||||||
|
11.779, 2.985, -1.472, 36, 335.000
|
||||||
|
12.162, 2.614, -0.969, 37, 382.500
|
||||||
|
12.592, 2.326, -0.672, 38, 430.000
|
||||||
|
13.070, 2.094, -0.484, 39, 477.500
|
||||||
|
13.595, 1.905, -0.361, 40, 525.000
|
||||||
|
14.168, 1.747, -0.276, 41, 572.500
|
||||||
|
14.788, 1.613, -0.216, 42, 620.000
|
||||||
|
15.456, 1.498, -0.172, 43, 667.500
|
||||||
|
16.171, 1.399, -0.139, 44, 715.000
|
||||||
|
16.934, 1.311, -0.114, 45, 762.500
|
||||||
|
17.744, 1.235, -0.095, 46, 810.000
|
||||||
|
18.602, 1.166, -0.080, 47, 857.500
|
||||||
|
19.507, 1.105, -0.068, 48, 905.000
|
||||||
|
20.460, 1.050, -0.058, 49, 952.500
|
||||||
|
21.460, 1.000, -0.050, 50, 1000.000
|
||||||
|
BIN
images/LeibRampStepper.png
Normal file
BIN
images/LeibRampStepper.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 130 KiB |
BIN
images/SimplisticRampStepper.png
Normal file
BIN
images/SimplisticRampStepper.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 126 KiB |
@@ -1,7 +1,6 @@
|
|||||||
// Leib Ramp Stepper program with millis function adapted for MacOS
|
// Leib Ramp Stepper program with millis function adapted for MacOS
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
#include <windows.h>
|
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include <time.h>
|
#include <time.h>
|
||||||
|
|
||||||
@@ -52,57 +51,52 @@ const float minSpeed = 1.0;
|
|||||||
const float maxPermissSpeed = 20.0;
|
const float maxPermissSpeed = 20.0;
|
||||||
const float maxAccel = 10.0;
|
const float maxAccel = 10.0;
|
||||||
|
|
||||||
int main()
|
int main(int argc, char *argv[1])
|
||||||
{
|
{
|
||||||
unsigned long currentMillis = millis();
|
unsigned long currentMillis = millis();
|
||||||
prevStepTime = 0;
|
prevStepTime = 0;
|
||||||
long positionToMoveTo;
|
long positionToMoveTo;
|
||||||
while(true)
|
sscanf(argv[1], "%ld", &positionToMoveTo);
|
||||||
|
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)
|
||||||
{
|
{
|
||||||
printf("Enter position to move to in profile (or 999 to terminate)\n");
|
// STEP 2
|
||||||
scanf("%ld", &positionToMoveTo);
|
// Define maximum speed in profile and number of steps in acceleration phase
|
||||||
if (positionToMoveTo==999) break;
|
// Use Equations (4) and (5)
|
||||||
printf(" Time (s), Speed (steps/s), Accel (steps/s^2), Posit'n (steps), Step time (ticks)\n");
|
maxSpeed = sqrt(minSpeed * minSpeed + stepsToGo * maxAccel); // Modified version of eq. 5
|
||||||
|
accelSteps = (long)(stepsToGo / 2);
|
||||||
|
}
|
||||||
|
|
||||||
goToPosition(positionToMoveTo);
|
// 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 */
|
||||||
/* Start of pre-computation code - only executed once per profile */
|
/* -------------------------------------------------------------- */
|
||||||
// STEP 1
|
millisAtStart = millis(); /* Needed only to tabulate speed vs. time */
|
||||||
// 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
|
/* Timed loop for stepping, and associated coding */
|
||||||
stepsToGo = computeStepsToGo();
|
while(stepsToGo > 0)
|
||||||
maxSpeed = maxPermissSpeed;
|
{
|
||||||
if (2 * accelSteps > stepsToGo)
|
currentMillis = millis();
|
||||||
|
if (currentMillis - prevStepTime >= p)
|
||||||
{
|
{
|
||||||
// STEP 2
|
moveOneStep();
|
||||||
// Define maximum speed in profile and number of steps in acceleration phase
|
prevStepTime = currentMillis;
|
||||||
// Use Equations (4) and (5)
|
computeNewSpeed();
|
||||||
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;
|
return 0;
|
||||||
|
|||||||
177
res/lab_2/SimplisticRampStepperMac.c
Normal file
177
res/lab_2/SimplisticRampStepperMac.c
Normal file
@@ -0,0 +1,177 @@
|
|||||||
|
// Simplistic Ramp Stepper program with millis function adapted for MacOS
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.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(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
unsigned long currentMillis = millis();
|
||||||
|
prevStepTime = 0;
|
||||||
|
long positionToMoveTo;
|
||||||
|
sscanf(argv[1], "%ld", &positionToMoveTo);
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
204
src/lab_2/LeibRampStepper.c
Normal file
204
src/lab_2/LeibRampStepper.c
Normal 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
src/lab_2/LeibRampStepper/LeibRampStepper.ino
Normal file
286
src/lab_2/LeibRampStepper/LeibRampStepper.ino
Normal 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");
|
||||||
|
}
|
||||||
203
src/lab_2/LeibRampStepperMac.c
Normal file
203
src/lab_2/LeibRampStepperMac.c
Normal file
@@ -0,0 +1,203 @@
|
|||||||
|
// Leib Ramp Stepper program with millis function adapted for MacOS
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.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(int argc, char *argv[1])
|
||||||
|
{
|
||||||
|
unsigned long currentMillis = millis();
|
||||||
|
prevStepTime = 0;
|
||||||
|
long positionToMoveTo;
|
||||||
|
sscanf(argv[1], "%ld", &positionToMoveTo);
|
||||||
|
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
src/lab_2/PIDClosedLoop/PIDClosedLoop.ino
Normal file
247
src/lab_2/PIDClosedLoop/PIDClosedLoop.ino
Normal 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
src/lab_2/ProportionalClosedLoop/ProportionalClosedLoop.ino
Normal file
235
src/lab_2/ProportionalClosedLoop/ProportionalClosedLoop.ino
Normal 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
src/lab_2/SimpleControlNoFeedback/SimpleControlNoFeedback.ino
Normal file
208
src/lab_2/SimpleControlNoFeedback/SimpleControlNoFeedback.ino
Normal 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;
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -1,13 +1,10 @@
|
|||||||
// Simplistic Ramp Stepper program with millis function adapted for MacOS
|
|
||||||
|
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
#include <windows.h>
|
#include <windows.h>
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include <time.h>
|
|
||||||
|
|
||||||
/* Only needed in Windows program to maintain compatibility with Arduino version of C/C++ */
|
/* Only needed in Windows program to maintain compatibility with Arduino version of C/C++ */
|
||||||
typedef enum { false, true } bool;
|
#define bool BOOL
|
||||||
#define true 1
|
#define true 1
|
||||||
#define false 0
|
#define false 0
|
||||||
const bool FWDS = true;
|
const bool FWDS = true;
|
||||||
@@ -97,16 +94,14 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Only needed for compatibility with Arduino program because millis() is not a native MacOS function */
|
|
||||||
long millis(void)
|
long millis(void)
|
||||||
|
/* Only needed for compatibility with Arduino program because millis() is not a native Windows API function */
|
||||||
{
|
{
|
||||||
struct timespec _t;
|
return GetTickCount();
|
||||||
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()
|
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" */
|
if (p != 0) /* p=0 is code for "don't make steps" */
|
||||||
{
|
{
|
||||||
349
src/lab_2/SimplisticRampStepper/SimplisticRampStepper.ino
Normal file
349
src/lab_2/SimplisticRampStepper/SimplisticRampStepper.ino
Normal 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
|
||||||
177
src/lab_2/SimplisticRampStepperMac.c
Normal file
177
src/lab_2/SimplisticRampStepperMac.c
Normal file
@@ -0,0 +1,177 @@
|
|||||||
|
// Simplistic Ramp Stepper program with millis function adapted for MacOS
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.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(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
unsigned long currentMillis = millis();
|
||||||
|
prevStepTime = 0;
|
||||||
|
long positionToMoveTo;
|
||||||
|
sscanf(argv[1], "%ld", &positionToMoveTo);
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
@@ -1,9 +1,8 @@
|
|||||||
---
|
---
|
||||||
author: Akbar Rahman (20386125)
|
author: Akbar Rahman (20386125)
|
||||||
date: \today
|
date: \today
|
||||||
title: Change Me
|
title: MMME3085---Lab 2 Coursework
|
||||||
tags: [ change_me ]
|
tags: [ mmme, mmme3086 ]
|
||||||
uuid: Change Me
|
|
||||||
documentclass: article
|
documentclass: article
|
||||||
geometry:
|
geometry:
|
||||||
- margin=1in
|
- margin=1in
|
||||||
@@ -12,7 +11,163 @@ geometry:
|
|||||||
\maketitle
|
\maketitle
|
||||||
\thispagestyle{empty}
|
\thispagestyle{empty}
|
||||||
\newpage
|
\newpage
|
||||||
\tableofcontents
|
|
||||||
|
# Question 1
|
||||||
|
|
||||||
|
As you increase the proportional gain, you increase the responsiveness of the motor to error,
|
||||||
|
but it is easy to go too high and cause the motor to oscillate around the target position.
|
||||||
|
This is because the motor current is set proportional to the error.
|
||||||
|
This means that if the motor is spinning too quickly, the proportional control will not slow
|
||||||
|
down the motor in time to avoid overshoot.
|
||||||
|
Additionally, if the target position is set too close to the motor, the proportional control
|
||||||
|
maybe not push enough current to motor to actually get it spinning in some cases.
|
||||||
|
|
||||||
|
# Question 2
|
||||||
|
|
||||||
|
When derivative control is added at low levels (0.02) it reduces the oscillations.
|
||||||
|
|
||||||
|
This is because derivative control only considers the rate of change of of the error
|
||||||
|
and tries to bring the rate of change to zero.
|
||||||
|
This means that adds a *damping* effect to the controller, slowing it down when it's moving,
|
||||||
|
and thereby reducing/eliminating overshoot and oscillations.
|
||||||
|
|
||||||
|
# Question 3
|
||||||
|
|
||||||
|
There are two if statements in the loop function as the controller will work best and be able to respond
|
||||||
|
to changes faster if the control functions are run more frequently.
|
||||||
|
The faster the better.
|
||||||
|
However, the Arduino does not really need to print to serial every time as it is only used to ensure
|
||||||
|
the Arduino is running properly.
|
||||||
|
|
||||||
|
If `prevMillisControl` and `prevMillisPrint` was replaced with just one variable, the `printLoop` function
|
||||||
|
would never run as the variable would have been updated to equal `currentMillis` before the print loop's
|
||||||
|
if statement will check its condition.
|
||||||
|
|
||||||
|
# Question 4
|
||||||
|
|
||||||
|
First the library is `include`d on line 4:
|
||||||
|
|
||||||
|
```c
|
||||||
|
#include <PID_V1.h>
|
||||||
|
```
|
||||||
|
|
||||||
|
Then a PID object is created using the `myPID` function on line 48.
|
||||||
|
This object holds all the properties of the PID controller including values of $K_p$, $K_i$, and $K_d$,
|
||||||
|
as well as a pointers to variables with the current motor position, speed, and set point.
|
||||||
|
|
||||||
|
```c
|
||||||
|
PID myPID(&encoderPosnMeasured, &percentSpeed, &positionSetPoint, Kp, Ki, Kd, DIRECT);
|
||||||
|
```
|
||||||
|
|
||||||
|
Some initialisation is run in `setup` on lines 89 and 90:
|
||||||
|
|
||||||
|
```c
|
||||||
|
myPID.SetOutputLimits(-100,100);
|
||||||
|
myPID.SetMode(AUTOMATIC);
|
||||||
|
```
|
||||||
|
|
||||||
|
Then the main control loop starts.
|
||||||
|
The `loop` function runs the `controlLoop` function every 20 milliseconds (as defined by
|
||||||
|
`controlInterval`) on line 101.
|
||||||
|
|
||||||
|
The `controlLoop` function retrieves the current motor position and stores it in the variable
|
||||||
|
that the PID controller has a pointer to.
|
||||||
|
|
||||||
|
`myPID.Compute()` is then called to recompute the motor speed required to get to the set point
|
||||||
|
and this is stored in the `percentSpeed` variable as the controller has a pointer to it.
|
||||||
|
This new speed is passed to the `driveMotorPercent` function to update the motor's speed.
|
||||||
|
|
||||||
\newpage
|
\newpage
|
||||||
|
|
||||||
# Header
|
# Question 5
|
||||||
|
|
||||||
|
The mathematically complex operations in `loop()` as it is run infrequently.
|
||||||
|
This is because it is inside an if statement with condition `convertNewNumber()`, which
|
||||||
|
returns `false` if there is no new data to convert (i.e. if there has not been any serial input
|
||||||
|
since the last loop).
|
||||||
|
|
||||||
|
Essentially, the complex operations only run once per serial input, meaning it is run very infrequently
|
||||||
|
compared to how many times the loop runs.
|
||||||
|
|
||||||
|
# Question 6
|
||||||
|
|
||||||
|
Complex operations in `computeNewSpeed()` have been avoided because this function runs very frequently
|
||||||
|
(on the order of microseconds).
|
||||||
|
Therefore it is important that the function does not consume too many processor cycles,
|
||||||
|
else the Arduino will be able to do little else.
|
||||||
|
|
||||||
|
# Question 7
|
||||||
|
|
||||||
|
If the stepper motor is run too quickly, steps will be skipped and it will not accelerate or
|
||||||
|
accelerate slowly.
|
||||||
|
It also makes a different noise to normal operation.
|
||||||
|
|
||||||
|
This is because the pole pairs of the motor will be activated too soon if attempting to spin
|
||||||
|
the motor too fast.
|
||||||
|
If they are activated too soon, they will not have the desired effect of speeding up the rotation
|
||||||
|
of the shaft, as the magnets of the motor will not be aligned as needed.
|
||||||
|
|
||||||
|
This is also why the steppers use a ramp function, so that the shaft has time accelerate to the
|
||||||
|
desired speed without skipping steps.
|
||||||
|
|
||||||
|
\newpage
|
||||||
|
|
||||||
|
# Question 8
|
||||||
|
|
||||||
|
```{ .matplotlib caption="A plot of velocity and acceleration for SimplisticRampStepper" dpi=300 }
|
||||||
|
import numpy as np
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
|
||||||
|
TIME = 'Time_s'
|
||||||
|
SPEED = 'Speed_stepss'
|
||||||
|
ACCEL = 'Accel_stepss2'
|
||||||
|
POS = 'Positn_steps'
|
||||||
|
STEP = 'Step_time_ticks'
|
||||||
|
|
||||||
|
data = np.genfromtxt("csv/SimplisticRampStepper_out_50.csv",
|
||||||
|
delimiter=",", names=True, dtype=float)
|
||||||
|
|
||||||
|
fig, ax1 = plt.subplots()
|
||||||
|
ax2 = ax1.twinx()
|
||||||
|
|
||||||
|
lines1 = ax1.plot(data[TIME], data[SPEED], label='Speed [steps/s]', color='tab:blue')
|
||||||
|
lines2 = ax2.plot(data[TIME], data[ACCEL], label='Acceleration [steps/s^2]', color='tab:red')
|
||||||
|
ax1.set_ylabel('Speed [steps/s]')
|
||||||
|
ax2.set_ylabel('Acceleration [steps/s^2]')
|
||||||
|
|
||||||
|
lines = lines1 + lines2
|
||||||
|
ax1.legend(lines, [ l.get_label() for l in lines ])
|
||||||
|
|
||||||
|
ax1.set_xlabel("Time [s]")
|
||||||
|
|
||||||
|
fig.tight_layout()
|
||||||
|
```
|
||||||
|
|
||||||
|
```{ .matplotlib caption="A plot of velocity and acceleration for LeibRampStepper" dpi=300 }
|
||||||
|
import numpy as np
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
|
||||||
|
TIME = 'Time_s'
|
||||||
|
SPEED = 'Speed_stepss'
|
||||||
|
ACCEL = 'Accel_stepss2'
|
||||||
|
POS = 'Positn_steps'
|
||||||
|
STEP = 'Step_time_ticks'
|
||||||
|
|
||||||
|
data = np.genfromtxt("csv/LeibRampStepper_out_50.csv",
|
||||||
|
delimiter=",", names=True, dtype=float)
|
||||||
|
|
||||||
|
fig, ax1 = plt.subplots()
|
||||||
|
ax2 = ax1.twinx()
|
||||||
|
|
||||||
|
lines1 = ax1.plot(data[TIME], data[SPEED], label='Speed [steps/s]', color='tab:blue')
|
||||||
|
lines2 = ax2.plot(data[TIME], data[ACCEL], label='Acceleration [steps/s^2]', color='tab:red')
|
||||||
|
ax1.set_ylabel('Speed [steps/s]')
|
||||||
|
ax2.set_ylabel('Acceleration [steps/s^2]')
|
||||||
|
|
||||||
|
lines = lines1 + lines2
|
||||||
|
ax1.legend(lines, [ l.get_label() for l in lines ], loc='upper right')
|
||||||
|
|
||||||
|
ax1.set_xlabel("Time [s]")
|
||||||
|
|
||||||
|
fig.tight_layout()
|
||||||
|
```
|
||||||
|
|||||||
@@ -1 +1 @@
|
|||||||
SUBMISSION_FILENAME=changeme.pdf
|
SUBMISSION_FILENAME=mmme3086_lab2_coursework_akbar_alvi_rahman_20386125.pdf
|
||||||
|
|||||||
Reference in New Issue
Block a user