Diff: main.cpp

Revision:

6:cfbef7079a6f

Parent:

5:63fe92075b71

--- a/main.cpp	Mon Jun 16 05:25:02 2014 +0000
+++ b/main.cpp	Tue Jun 17 13:02:17 2014 +0000
@@ -1,16 +1,21 @@
 #include "mbed.h"
-#include "cmsis_os.h"
+#include <math.h>
+#include "cmsis_os.h" // https://mbed.org/handbook/CMSIS-RTOS
+// http://mbed.org/users/AjK/notebook/regarding-interrupts-use-and-blocking/
 
 DigitalOut StepLED(LED1);
 DigitalOut CmdLED(LED2);
 DigitalOut DirLED(LED3);
+DigitalOut TestLED(LED4);
+DigitalOut xDir(p22);
+DigitalOut xStep(p24);
+DigitalIn xHome(p8);
 
 typedef struct {
-    int steps;  // number of steps to take
-    int speed;  // speed in steps/sec
-    int accel;  // accelleration in steps/sec^2
-    int maxspeed; // maximum speed in steps/sec
-    int minspeed; // minimum speed in steps/sec
+    float steps;  // number of steps to take
+    float accel;  // accelleration in steps/sec^2
+    float maxspeed; // maximum speed in steps/sec
+    float minspeed; // minimum speed in steps/sec
     bool dir;    // direction
 } stepcmd;
 
@@ -19,56 +24,246 @@
 
 osMessageQDef(queue, 16, stepcmd);
 osMessageQId  queue;
-int qcnt = 0;
-Timeout timer;
+unsigned int qcnt = 0;
+Timeout timer,steptimer;
+Timer counter;
+
+void decelmove();
+void accelmove();
+void flatmove();
+void stepper();
+void stepdown();
+
+const bool xStepHigh = 1;
+const float pulseTime = 5; // pulse time in microseconds
+bool N_ESTOP = 1;
+float asteps, lsteps, dsteps, speed, accel, maxspeed, minspeed, ttime, dtime, wtime , etime, rtime, tsteps;
+char output[256];
+
+Serial pc(USBTX, USBRX); // tx, rx
+
+void stepdown()
+{
+    xStep = !xStepHigh;
+    tsteps++;
+}
 
-void read_isr()
+void decelmove()
+{
+    if (N_ESTOP && (dsteps-- > 0)) {
+        wtime = 1000000 / (speed - accel * (dtime/1000000));
+        timer.attach_us(decelmove, wtime);
+        xStep = xStepHigh;
+        steptimer.attach_us(stepdown, pulseTime);
+        dtime += wtime;
+    } else {
+        ttime += dtime;
+        etime = ttime;
+        counter.stop();
+        rtime = counter.read_us();
+        timer.attach_us(stepper, wtime);
+    }
+}
+
+void flatmove()
+{
+    if (N_ESTOP && (lsteps-- > 0)) {
+        timer.attach_us(flatmove, wtime);
+        xStep = xStepHigh;
+        steptimer.attach_us(stepdown, pulseTime);
+        ttime += wtime;
+        dtime = 0;
+    } else {
+        // ttime was only accelleration, not flatmove!
+        // after calculating wtime, set to 0 for use in decelmove
+        speed = 1000000.0 / wtime;
+        timer.attach_us(decelmove, wtime);
+    }
+}
+
+void accelmove()
 {
-        StepLED = !StepLED;
-        osEvent evt = osMessageGet(queue, 0);
-        if (evt.status == osEventMessage) {
-            stepcmd *cmd = (stepcmd*)evt.value.p;
-            DirLED = cmd->dir;
-            osPoolFree(cmdpool, cmd);
-            qcnt--;
-        } 
-        timer.attach(read_isr, 2);
+    TestLED = 1;
+    if (N_ESTOP && (asteps-- > 0)) {
+        wtime = 1000000 / (speed + accel * (ttime/1000000));
+        if (wtime < pulseTime) wtime = pulseTime;
+        timer.attach_us(accelmove, wtime);
+        xStep = xStepHigh;
+        steptimer.attach_us(stepdown, pulseTime);
+        ttime += wtime;
+
+
+    } else {
+        timer.attach_us(flatmove, wtime);
+    }
+}
+
+void stepper()
+{
+    osEvent evt = osMessageGet(queue, 0);
+    if (evt.status == osEventMessage) {
+        stepcmd *cmd = (stepcmd*)evt.value.p;
+        xDir = DirLED = cmd->dir;
+        accel = cmd->accel;
+        speed = cmd->minspeed;
+        lsteps = cmd->steps;
+        ttime = 0;
+        tsteps = 0;
+        wtime = 1000000 / speed;
+        float atime = 0;
+        if (accel == 0) { // linear move
+            asteps = dsteps = 0;
+        } else {
+            // calculate time to reach max speed:
+            // speed = startspeed + accel * time;
+            atime = (cmd->maxspeed - cmd->minspeed) / cmd->accel;
+            asteps = speed * atime + 0.5 * accel * pow(atime,2);
+            if ((lsteps /2) > asteps) {
+                // Accellerate to max speed, then flat, then decellerate
+                dsteps = asteps;
+                lsteps -= 2 * asteps;
+            } else {
+                // Accellerate to half of the space, then decellerate
+                asteps = lsteps/ 2;
+                dsteps = lsteps-asteps;
+                lsteps = 0;
+            }
+        }
+        sprintf(output, "atime: %2.6f steps: %f, %f, %f wtime: %f\n\r", atime, asteps, lsteps, dsteps, wtime);
+        counter.reset();
+        counter.start();
+        timer.attach_us(accelmove, wtime);
+        osPoolFree(cmdpool, cmd);
+        qcnt--;
+    } else {
+        timer.attach(stepper, 2);
+    }
+}
+void home(float speed)
+{
+    int wtime = int(1000000.0/speed);
+    xDir = 1;
+    while (! xHome) {
+        xStep = 1;
+        wait_us(5);
+        xStep = 0;
+        wait_us(wtime);
+    }
+    xDir = 0;
+    for (int x=0; x<1000; x++) {
+        xStep = 1;
+        wait_us(5);
+        xStep = 0;
+        wait_us(wtime);
+    }
 }
 
 int main (void)
 {
+    printf("Keys:\n\r");
+    printf("0-1: Adjust minimum speed (m/s)\n\r");
+    printf("3-4: Adjust maximum speed (m/s)\n\r");
+    printf("5-6: Adjust Accelleration (m/s^2)\n\r");
+    printf("7-8: Adjust total distance (m)\n\r");
+    printf("9-0: Steps per meter\n\r");
+    printf("h = home, p = print result, q = E_STOP, t=toggle direction\n\r");
+    printf("SPACE = move\n\r");
+    printf("CTRL-Break = reset\n\r\n\r");
+    TestLED = 0;
     cmdpool = osPoolCreate(osPool(cmdpool));
     queue = osMessageCreate(osMessageQ(queue), NULL);
-
-    timer.attach(read_isr, 2);
-    bool mydir = true;
-
+    timer.attach(stepper, 2);
+    bool mydir = false;
+    etime = 0;
+    // 902.51 met kleine motor: 0.25/0.8/1.1/80000 = 12.5 micron
+    float startspeed = 0.25;  // speed in m/s
+    float maxspeed = 0.8; // speed in m/s
+    float accel = 1.1; // accelleration in m/s^2
+    float distance = 1.26; // maximum distance on the rod in m
+    float steps = 80000; // steps per meter
+    float precision = 1000000 / steps;
+    pc.printf("Settings: min: %3.2f, max %3.2f, accel %3.2f, dist %3.2f, steps %ld\n\r",
+              startspeed, maxspeed, accel, distance, steps);
     while (true) {
-        const float startspeed = 0.05;  // speed in m/s
-        const float maxspeed = 2; // speed in m/s
-        const float accel = 2; // accelleration in m/s^2
-        const float distance = 1.26; // maximum distance on the rod in m
-        const int steps = 36000; // steps per meter
-        if (qcnt < 15) {
-            stepcmd *cmd = (stepcmd*)osPoolAlloc(cmdpool);
-            cmd -> minspeed = int(startspeed * steps);
-            cmd -> maxspeed = int(maxspeed * steps);
-            cmd -> accel = int(accel * steps);
-            cmd -> steps = int(distance * steps);
-            cmd -> speed = cmd -> minspeed;
-            cmd -> dir = mydir;
-            mydir = !mydir;
+        if (pc.readable()) {
+            char c = pc.getc();
+            switch (c) {
+                case 'h':
+                    home(startspeed * steps);
+                    break;
+                case '1':
+                    if (startspeed > 0) startspeed -= 0.01;
+                    break;
+                case '2':
+                    startspeed += 0.01;
+                    break;
+                case '3':
+                    if (maxspeed > startspeed) maxspeed -=0.01;
+                    break;
+                case '4':
+                    maxspeed += 0.01;
+                    break;
+                case '5':
+                    if (accel > 0) accel -= 0.01;
+                    break;
+                case '6':
+                    accel += 0.01;
+                    break;
+                case '7':
+                    if (distance > 0) distance -= 0.01;
+                    break;
+                case '8':
+                    distance += 0.01;
+                    break;
+                case '9':
+                    if (steps > 0) steps -= 100;
+                    break;
+                case '0':
+                    steps += 100;
+                    break;
+                case 't':
+                    mydir = !mydir;
+                    break;
+                case 'p':
+                    pc.printf("Etime: %2.3f, Rtime: %2.3f a,l,d,steps: %f,%f,%f,%f\n\r", (float)(etime/1000000.0), rtime/1000000, asteps, lsteps, dsteps, tsteps);
+                    break;
+                case 'q':
+                    N_ESTOP = ! N_ESTOP;
+                    break;
+                case ' ':
+                    if (qcnt < 15) {
+                        stepcmd *cmd = (stepcmd*)osPoolAlloc(cmdpool);
+                        cmd -> minspeed = startspeed *steps;
+                        cmd -> maxspeed = maxspeed * steps;
+                        cmd -> accel = accel * steps;
+                        cmd -> steps = distance * steps;
 
-            osStatus res = osMessagePut(queue, (uint32_t)cmd, 0);
-            while (res != osOK) {
-                osDelay(500);
-                res = osMessagePut(queue, (uint32_t)cmd, 0);
+                        pc.printf("Stepsettings: min: %f, max %f, accel %f, distance: %f\n\r",
+                                  cmd->minspeed, cmd->maxspeed, cmd->accel, cmd->steps, precision);
+                        cmd -> dir = mydir;
+                        osStatus res = osMessagePut(queue, (uint32_t)cmd, 0);
+                        while (res != osOK) {
+                            osDelay(500);
+                            res = osMessagePut(queue, (uint32_t)cmd, 0);
+                        }
+                        pc.printf("Command sent\n\r", ++qcnt);
+                        mydir = !mydir;
+                        osDelay(100);
+                    } else {
+                        // pc.printf("Storage full\n\r");
+                    }
+                    break;
             }
-            printf("Storage succeeded %d\n\r", ++qcnt);
+            precision = 1000000 / steps;
+            pc.printf("Settings: min: %3.2f, max %3.2f, accel %3.2f, dist %3.2f, steps %7.0f DIR: %d, precision: %2.3f\n\r",
+                      startspeed, maxspeed, accel, distance, steps, mydir, precision);
         } else {
-            printf("Storage full\n\r");
+            osDelay(2000);
         }
-        osDelay(1000);
         CmdLED = ! CmdLED;
+        if (strlen(output)>0) {
+            pc.printf("%s\n\r", output);
+            sprintf(output, "");
+        }
     }
-}
+}
\ No newline at end of file