From 6d16857453d594e8c0a76efbf299db3af3a0f3e5 Mon Sep 17 00:00:00 2001 From: Alvie Rahman Date: Wed, 6 Dec 2023 15:38:01 +0000 Subject: [PATCH] copy lab_2 code to src folders --- src/lab_2/LeibRampStepper.c | 204 ++++++++++ src/lab_2/LeibRampStepper/LeibRampStepper.ino | 286 ++++++++++++++ src/lab_2/LeibRampStepperMac.c | 203 ++++++++++ src/lab_2/PIDClosedLoop/PIDClosedLoop.ino | 247 +++++++++++++ .../ProportionalClosedLoop.ino | 235 ++++++++++++ .../SimpleControlNoFeedback.ino | 208 +++++++++++ src/lab_2/SimplisticRampStepper.c | 178 +++++++++ .../SimplisticRampStepper.ino | 349 ++++++++++++++++++ src/lab_2/SimplisticRampStepperMac.c | 177 +++++++++ 9 files changed, 2087 insertions(+) create mode 100644 src/lab_2/LeibRampStepper.c create mode 100644 src/lab_2/LeibRampStepper/LeibRampStepper.ino create mode 100644 src/lab_2/LeibRampStepperMac.c create mode 100644 src/lab_2/PIDClosedLoop/PIDClosedLoop.ino create mode 100644 src/lab_2/ProportionalClosedLoop/ProportionalClosedLoop.ino create mode 100644 src/lab_2/SimpleControlNoFeedback/SimpleControlNoFeedback.ino create mode 100644 src/lab_2/SimplisticRampStepper.c create mode 100644 src/lab_2/SimplisticRampStepper/SimplisticRampStepper.ino create mode 100644 src/lab_2/SimplisticRampStepperMac.c diff --git a/src/lab_2/LeibRampStepper.c b/src/lab_2/LeibRampStepper.c new file mode 100644 index 0000000..07ce45f --- /dev/null +++ b/src/lab_2/LeibRampStepper.c @@ -0,0 +1,204 @@ +#include +#include +#include +#include + +/* 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; + } +} diff --git a/src/lab_2/LeibRampStepper/LeibRampStepper.ino b/src/lab_2/LeibRampStepper/LeibRampStepper.ino new file mode 100644 index 0000000..d91fb6c --- /dev/null +++ b/src/lab_2/LeibRampStepper/LeibRampStepper.ino @@ -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"); +} diff --git a/src/lab_2/LeibRampStepperMac.c b/src/lab_2/LeibRampStepperMac.c new file mode 100644 index 0000000..762fb08 --- /dev/null +++ b/src/lab_2/LeibRampStepperMac.c @@ -0,0 +1,203 @@ +// Leib Ramp Stepper program with millis function adapted for MacOS +#include +#include +#include +#include + + +/* 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; + } +} diff --git a/src/lab_2/PIDClosedLoop/PIDClosedLoop.ino b/src/lab_2/PIDClosedLoop/PIDClosedLoop.ino new file mode 100644 index 0000000..5f04792 --- /dev/null +++ b/src/lab_2/PIDClosedLoop/PIDClosedLoop.ino @@ -0,0 +1,247 @@ + +/* Example of driving servomotor using PID closed loop control */ + +#include /* Needed to communicate with LS7366R (Counter Click) */ +#include + +/* 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; + } +} diff --git a/src/lab_2/ProportionalClosedLoop/ProportionalClosedLoop.ino b/src/lab_2/ProportionalClosedLoop/ProportionalClosedLoop.ino new file mode 100644 index 0000000..7449a51 --- /dev/null +++ b/src/lab_2/ProportionalClosedLoop/ProportionalClosedLoop.ino @@ -0,0 +1,235 @@ + +/* Example of driving servomotor using simple proportional closed loop control */ + +#include /* 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; + } +} diff --git a/src/lab_2/SimpleControlNoFeedback/SimpleControlNoFeedback.ino b/src/lab_2/SimpleControlNoFeedback/SimpleControlNoFeedback.ino new file mode 100644 index 0000000..527c6ce --- /dev/null +++ b/src/lab_2/SimpleControlNoFeedback/SimpleControlNoFeedback.ino @@ -0,0 +1,208 @@ +/* Example of driving servomotor and reading encoder signals */ + +#include /* 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; + } +} diff --git a/src/lab_2/SimplisticRampStepper.c b/src/lab_2/SimplisticRampStepper.c new file mode 100644 index 0000000..b6907e2 --- /dev/null +++ b/src/lab_2/SimplisticRampStepper.c @@ -0,0 +1,178 @@ +#include +#include +#include +#include + +/* Only needed in Windows program to maintain compatibility with Arduino version of C/C++ */ +#define bool BOOL +#define true 1 +#define false 0 +const bool FWDS = true; +const bool BWDS = false; + +const long ticksPerSec = 1000; // ms on PC +// on Arduino it is 1E6 for micros (for s/w) or 1.6E7 for 62.5 ns ticks (for h/w) + +void moveOneStep(); +void computeNewSpeed(); +long computeStepsToGo(); +void goToPosition(long newPosition); +long millis(void); + +/* Note: we are using global variables ONLY to preserve compatibility with the Arduino program structure. + They should not normally be used in C or C++ programs as they make for a poor software design. */ +/* Global variables relating to stepper motor position counting etc. */ +long stepsToGo; /* Number of steps left to make in present movement */ +long targetPosition; /* Intended destination of motor for given movement */ +volatile long currentPosition = 0; /* Position in steps of motor relative to startup position */ +float maxSpeed; /* Maximum speed in present movement (not nec. max permitted) */ +bool direction; /* Direction of present movement: FWDS or BWDS */ + +/* Global variables used in simplistic and Leib Ramp algorithms */ +volatile float p; /* Step interval in clock ticks or microseconds */ +float ps; /* Maximum step periods */ +float deltaP; /* You'll be able to get rid of this later */ + +/* Global variable used for noting previous time of a step in timed loop and for calculating speed and accel */ +long prevStepTime=0; +long millisAtStart; +float prevSpeed=0.0; + +/* Define permissible parameters for motor */ +// For testing on PC only, not for use in Arduino program: try movements in order of 50-100 steps +float accelSteps=20; /* leave this as a variable as we may over-write it */ +const float minSpeed = 1.0; // in steps/s +const float maxPermissSpeed = 20.0; // in steps/s +const float maxAccel = 10.0; // in steps/s^2 + +int main() +{ + unsigned long currentMillis = millis(); + prevStepTime = 0; + long positionToMoveTo; + while(true) + { + printf("Enter position to move to in profile (or 999 to terminate)\n"); + scanf("%ld", &positionToMoveTo); + if (positionToMoveTo==999) break; + printf(" Time (s), Speed (steps/s), Accel (steps/s^2), Posit'n (steps), Step time (ticks)\n"); + + goToPosition(positionToMoveTo); + + /* -------------------------------------------------------------- */ + /* Start of pre-computation code - only executed once per profile */ + + float maxInterval = ((float)ticksPerSec) / minSpeed; + ps = ((float)ticksPerSec) / maxPermissSpeed; + deltaP = (maxInterval - ps) / accelSteps; + stepsToGo = computeStepsToGo(); + maxSpeed = maxPermissSpeed; + if (2 * accelSteps > stepsToGo) + { + accelSteps = (long)(stepsToGo / 2); + } + + p = maxInterval; + + ps = ((float)ticksPerSec) / maxSpeed; + /* End of pre-computation code */ + /* -------------------------------------------------------------- */ + millisAtStart = millis(); /* Needed only to tabulate speed vs. time */ + + /* Timed loop for stepping */ + while(stepsToGo > 0) + { + currentMillis = millis(); + if (currentMillis - prevStepTime >= p) + { + moveOneStep(); + prevStepTime = currentMillis; + computeNewSpeed(); + } + } + } + return 0; +} + +long millis(void) +/* Only needed for compatibility with Arduino program because millis() is not a native Windows API function */ +{ + return GetTickCount(); +} + +void moveOneStep() +/* Move a single step. If this were running on the Arduino we would holding pulse high for delayMicroSeconds */ +{ + if (p != 0) /* p=0 is code for "don't make steps" */ + { + // Print to screen instead of making a step + if (direction == FWDS) + { + currentPosition++; + } + else + { + currentPosition--; + } + + /* Instead of actually making step, print out parameters for current step */ + float speed = (float)(ticksPerSec)/p; + float accel = (float)(ticksPerSec)*(speed-prevSpeed)/p; + printf("%16.3f, %16.3f, %16.3f, %16ld, %16.3f\n", 0.001*(millis()-millisAtStart), speed, accel, currentPosition, p); + prevSpeed = speed; + } +} + +/* Calcuate new value of step interval p based on constants defined in timed loop */ +void computeNewSpeed() +{ + stepsToGo = computeStepsToGo(); + + /* Start of on-the-fly step calculation code, executed once per step */ + if (stepsToGo == 0) + { + p = 0; // Not actually a zero step interval, used to switch stepping off + return; + } + else if (stepsToGo >= accelSteps && (long)p > (long)ps) // Changed to make ramps even length + /* Speeding up */ + { + p -= deltaP; + } + else if (stepsToGo <= accelSteps) + /* Slowing down */ + { + p += deltaP; + } + /* else p is unchanged: running at constant speed */ + + /* End of on-the-fly step calculation code */ +} + +/* Work out how far the stepper motor still needs to move */ +long computeStepsToGo() +{ + if (direction == FWDS) + { + return targetPosition - currentPosition; + } + else + { + return currentPosition - targetPosition; + } +} + +/* Set the target position and determine direction of intended movement */ +void goToPosition(long newPosition) +{ + targetPosition = newPosition; + if (targetPosition - currentPosition > 0) + { + direction = FWDS; + } + else + { + direction = BWDS; + } +} + diff --git a/src/lab_2/SimplisticRampStepper/SimplisticRampStepper.ino b/src/lab_2/SimplisticRampStepper/SimplisticRampStepper.ino new file mode 100644 index 0000000..e7be0b2 --- /dev/null +++ b/src/lab_2/SimplisticRampStepper/SimplisticRampStepper.ino @@ -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 diff --git a/src/lab_2/SimplisticRampStepperMac.c b/src/lab_2/SimplisticRampStepperMac.c new file mode 100644 index 0000000..9a48543 --- /dev/null +++ b/src/lab_2/SimplisticRampStepperMac.c @@ -0,0 +1,177 @@ +// Simplistic Ramp Stepper program with millis function adapted for MacOS + +#include +#include +#include +#include + +/* 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; + } +} +