add lab resources

res/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");
+}