Diff: main.cpp

Revision:

1:86997189bb6b

Parent:

0:b53649cd217f

Child:

2:33a99f65551d

--- a/main.cpp	Mon Feb 25 23:27:01 2013 +0000
+++ b/main.cpp	Thu Feb 28 11:40:50 2013 +0000
@@ -1,277 +1,443 @@
-/* picocom -b 115200 -c --imap lfcrlf --omap crlf /dev/ttyACM0 */
-
-
-/*
-    todo: attach callback to Serial when no command executed only with STOP command
-    todo: when command complete it should print "ok"
-    todo: when all settings printed print "ok"
-    todo: when value of parameter changed print "ok saved"
-    todo: when value of parameter is invalid print "err invalid value"
-    todo: add G command for set zero here
-    todo: add G90/G91
-*/
-
-#include "mbed.h"
-#include "FastPWM.h"
-#include "IOMacros.h"
-
-#define VERSION "0.1"
-
-
-#define DEBUG
-
-/* p30 - P0_4 */
-const uint32_t stepPin = 4;
-const uint32_t stepIntMask = (1UL << stepPin);
-
-volatile int32_t position;
-
-
-/* G code interpreter state*/
-bool absMovementMode = true;
-double mmPosition = 0.0; // mm
-double feedRate = 0.0; // mm/sec^2
-
-/* *** Serial port *** */
-Serial pc(USBTX, USBRX);
-#define SERIAL_BUFFER_SIZE 40
-char serialBuffer[SERIAL_BUFFER_SIZE+1];
-int serialPosition = 0;
-
-void invalidCommand() {
-    pc.printf("err invalid command %s\n", serialBuffer);
-}
-
-void processCommand(void) {
-#ifdef DEBUG
-    pc.printf("<%s>\n", serialBuffer);
-#endif
-
-    if (serialBuffer[0] == '\0') {
-        // todo: empty command stop the stage
-    } else if (serialBuffer[0] == '$') {
-        // '$' - print or change settings
-        if (serialBuffer[1] == '\0') {
-            pc.printf("todo: show settings\n");
-        } else if (serialBuffer[1] >= '0' and serialBuffer[1] <='9' and serialBuffer[2] == '=') {
-            pc.printf("todo: set value here\n");
-            // todo: save settings to file
-        } else {
-            invalidCommand();
-        }
-    } else if (serialBuffer[0] == '?' && serialBuffer[1] == '\0') {
-        // todo: print in millimeters
-        pc.printf("ok %d\n", position);
-    } else {
-        // todo: parse G-code here
-        char *p = serialBuffer, *endP = serialBuffer;
-        uint32_t ulValue;
-        double dblValue;
-        
-        bool error = false;
-        
-        bool newAbsMovementMode = absMovementMode;
-        double newmmPosition = mmPosition;
-        double newFeedRate = feedRate;
-        
-        bool move = false;
-        
-        while (*p != '\0') {
-            switch (*p) {
-                case 'G':
-                    p++;
-                    ulValue = strtoul(p, &endP, 10);
-                    if (p == endP) {
-                        pc.printf("err invalid value for command G: %s\n", p);
-                        error = true;
-                    } else {
-#ifdef DEBUG
-                        pc.printf("debug G%u -> %s\n", ulValue, endP);
-#endif
-                        p = endP;
-                        switch (ulValue) {
-                            case 0:
-                                // todo: implement
-                                break;
-                            case 1:
-                                // todo: implement
-                                break;
-                            case 90:
-                                newAbsMovementMode = true;
-                                break;
-                            case 91:
-                                newAbsMovementMode = false;
-                                break;
-                            default:
-                                pc.printf("err invalid value for command G: %u\n", ulValue);
-                                error = true;
-                                break;
-                        }
-                    }
-                    break;
-                case 'X':
-                    p++;
-                    dblValue = strtod(p, &endP);
-                    if (p == endP) {
-                        pc.printf("err invalid value for command X: %f\n", dblValue);
-                        error = true;
-                    } else {
-#ifdef DEBUG
-                        pc.printf("debug X%f -> %s\n", dblValue, endP);
-#endif
-                        p = endP;
-                        newmmPosition = dblValue;
-                        
-                        move = true;
-                    }
-                    break;
-                case 'F':
-                    p++;
-                    dblValue = strtod(p, &endP);
-                    if (p == endP || dblValue < 0.0) {
-                        pc.printf("err invalid value for command F: %f\n", dblValue);
-                        error = true;
-                    } else {
-#ifdef DEBUG
-                        pc.printf("debug F%f -> %s\n", dblValue, endP);
-#endif
-                        p = endP;
-                        newFeedRate = dblValue;
-                    }
-                    break;
-                default:
-                    pc.printf("err invalid command %s\n", p);
-                    error = true;
-                    break;
-            }
-            
-            if (error) {
-                break;
-            }
-            
-        }
-        
-        if (!error) {
-            // todo: check all flags and execute commands here
-            absMovementMode = newAbsMovementMode;
-            mmPosition = newmmPosition;
-            feedRate = newFeedRate;
-            // todo: run line module here
-            
-            pc.printf("absMovementMode = %u\n", absMovementMode);
-            pc.printf("mmPosition = %f\n", mmPosition);
-            pc.printf("feedRate = %f\n", feedRate);
-#ifdef DEBUG
-            if (move) {
-                pc.printf("MOVE\n");
-            }
-#endif
-        }
-    }
-}
-
-
-void readChar(void) {
-    int ch;
-    
-#ifdef DEBUG
-    LED4_ON;
-#endif
-    
-    ch = pc.getc();
-    if (serialPosition < SERIAL_BUFFER_SIZE) {
-        
-    } else {
-        pc.printf("\nToo long string, should be <= %d characters.\n", SERIAL_BUFFER_SIZE);
-        serialPosition = 0;
-    }
-    if (ch == ' ' || ch == '\t') {
-        // ignore space characters
-    } else {
-    
-        if (ch == '\n') {
-            serialBuffer[serialPosition] = '\0';
-            processCommand();
-            serialPosition = 0;
-            serialBuffer[serialPosition] = '\0';
-        } else {
-            if (ch >= 'a' and ch <= 'z') {
-                ch = 'A' + (ch - 'a'); // convert to upper case
-            }
-            serialBuffer[serialPosition++] = ch;
-        }
-    }
-
-#ifdef DEBUG
-    LED4_OFF;
-#endif
-}
-
-
-FastPWM stepper(p21);
-
-
-void update_position();
-
-
-extern "C" void EINT3_IRQHandler (void) __irq {
-    if (LPC_GPIOINT->IntStatus & 0x1) {
-        if (LPC_GPIOINT->IO0IntStatR & stepIntMask) {
-            update_position();
-        }
-    }
-    
-    LPC_GPIOINT->IO2IntClr = (LPC_GPIOINT->IO2IntStatR | LPC_GPIOINT->IO2IntStatF);
-    LPC_GPIOINT->IO0IntClr = (LPC_GPIOINT->IO0IntStatR | LPC_GPIOINT->IO0IntStatF);
-
-}
-
-void update_position() {
-    //position++;
-    if (position > 0) {
-        position--;
-    } else {
-        position = 170000;
-    }
-}
-
-
-void event_irq_init(void) {
-    p30_AS_INPUT;
-    // Enable p30 is P0_4 for rising edge interrupt generation.
-    LPC_GPIOINT->IO0IntEnR |= stepIntMask;
-    //NVIC_SetPriority(EINT3_IRQn, 1);
-    // Enable the interrupt
-    NVIC_EnableIRQ(EINT3_IRQn);
-}
-
-int main() {
-    LED1_USE; // step movement
-    LED4_USE; // serial port receive
-    position = 0;
-    
-    pc.baud(115200);
-    pc.attach(readChar);
-
-    pc.printf("IGNB module driver version %s. Input '$' for settings.\n", VERSION);
-    
-    // todo: load settings from file
-    
-    //stepper.period(1.0/170000.0);
-    //stepper.write(0.50);
-
-    //event_irq_init();
-
-    while(1) {
-        //position = 0;
-
-        //LED1_ON;
-        //stepper.write(0.5);
-        //wait(1);
-        //stepper.write(0.0);
-        //LED1_OFF;
-        //pc.printf("%d\r\n", position);
-        //wait(1);
-    }
-
-}
+/* picocom -b 115200 -c --imap lfcrlf --omap crlf /dev/ttyACM0 */
+
+
+/*
+    todo: attach callback to Serial when no command executed only with STOP command
+    todo: when command complete it should print "ok"
+    todo: when all settings printed print "ok"
+    todo: when value of parameter changed print "ok saved"
+    todo: when value of parameter is invalid print "err invalid value"
+    todo: add G command for set zero here
+*/
+
+#include "mbed.h"
+#include "FastPWM.h"
+#include "IOMacros.h"
+
+#define VERSION "0.1"
+
+#define DEBUG
+
+#define FILESYSTEM_ROOT "local"
+#define SETTINGS_FILE ("/" FILESYSTEM_ROOT "/settings.txt")
+#define SETTINGS_BACKUP_FILE ("/" FILESYSTEM_ROOT "/settings.bak")
+
+LocalFileSystem local(FILESYSTEM_ROOT);
+
+
+/* p30 - P0_4 */
+// const uint32_t counterPin = 4;
+const uint32_t counterIntMask = p30_SET_MASK;
+
+volatile int32_t position;
+
+FastPWM stepper(p21);
+
+// Dir pin is p22 - P2.4
+
+
+/* G code interpreter state*/
+bool absMovementMode = true;
+double mmPosition = 0.0; // mm
+double feedRate = 1.0; // mm/sec^2
+
+/* *** Serial port *** */
+Serial pc(USBTX, USBRX);
+#define SERIAL_BUFFER_SIZE 40
+char serialBuffer[SERIAL_BUFFER_SIZE+1];
+int serialPosition = 0;
+
+
+/* *** Settings *** */
+
+typedef struct {
+    double steps_per_mm;
+    double acceleration; // mm/sec^2
+    double start_acceleration; // mm/sec^2
+} settings_t;
+
+settings_t settings;
+
+// return true - if parsed succesfully
+// return false - if error during parsing
+bool parse_settings(FILE *fp, settings_t *settings) {
+    // todo: write parser
+    return true;
+}
+
+void load_settings(char *filename) {
+    FILE *fp;
+
+#ifdef DEBUG
+    pc.printf("debug load settings from file \"%s\"\n", filename);
+#endif
+
+    fp = fopen(filename, "r");
+    if (fp == NULL) {
+        settings.steps_per_mm = 25600.0 / 150.0; // 170.666667;
+        settings.acceleration = 1000.000;
+        settings.start_acceleration = 0.0;
+    } else {
+        parse_settings(fp, &settings);
+        fclose(fp);
+    }
+}
+
+void save_settings(char *filename) {
+    FILE *fp;
+    
+    fp = fopen(SETTINGS_FILE, "w");
+    if (fp != NULL) {
+        fprintf(fp, "$0=%f\n", settings.steps_per_mm);
+        fprintf(fp, "$1=%f\n", settings.acceleration);
+        fprintf(fp, "$2=%f\n", settings.start_acceleration);
+        fclose(fp);
+#ifdef DEBUG
+        pc.printf("ok settings saved to %s\n", SETTINGS_FILE);
+#endif
+    } else {
+        pc.printf("err unable to open %s for writing\n", SETTINGS_FILE);
+    }
+}
+
+void print_settings() {
+    pc.printf("$0=%f (x, steps/mm)\n", settings.steps_per_mm);
+    pc.printf("$1=%f (x accel, mm/sec^2)\n", settings.acceleration);
+    pc.printf("$2=%f (x start acceleration, mm/sec^2)\n", settings.start_acceleration);
+    pc.printf("ok\n");
+}
+
+// return true if no error
+// return false if error (invalid parameter or value)
+bool set_param(char name, char *value) {
+    if (name >= '0' && name <= '2') {
+        char *endP = value;
+        double dblValue;
+        bool error = false;
+
+        dblValue = strtod(value, &endP);
+        if (value == endP || *endP != '\0') {
+            pc.printf("err invalid value for command $%c: %s\n", name, value);
+            error = true;
+        } else {
+
+#ifdef DEBUG
+            pc.printf("debug $%c=%f -> %s\n", name, dblValue, endP);
+#endif
+                                    
+            switch (name) {
+                case '0':
+                    if (dblValue > 0.0) {
+                        settings.steps_per_mm = dblValue;
+                    } else {
+                        pc.printf("err value should be > 0.0, but %f\n", dblValue);
+                        error = true;
+                    }
+                    break;
+                case '1':
+                    if (dblValue > 0.0) {
+                        settings.acceleration = dblValue;
+                    } else {
+                        pc.printf("err value should be > 0.0, but %f\n", dblValue);
+                        error = true;
+                    }
+                    break;
+                case '2':
+                    if (dblValue >= 0.0) {
+                        settings.start_acceleration = dblValue;
+                    } else {
+                        pc.printf("err value should be >= 0.0, but %f\n", dblValue);
+                        error = true;
+                    }
+                    break;
+                default:
+                    pc.printf("err parameter %c unrecognized\n", name);
+                    error = true;
+                    break;
+            }
+        }
+        return !error;
+    } else {
+        pc.printf("err invalid parameter %c\n", name);
+        return false;
+    }
+}
+
+
+void invalidCommand() {
+    pc.printf("err invalid command %s\n", serialBuffer);
+}
+
+void moveModule(double position) {
+    double newmmPosition;
+    newmmPosition = absMovementMode ? position : mmPosition + position;
+
+#ifdef DEBUG    
+    pc.printf("debug move from %f to %f\n", mmPosition, newmmPosition);
+    pc.printf("debug move from %f to %f\n", mmPosition * settings.steps_per_mm, newmmPosition * settings.steps_per_mm);
+#endif
+
+    if (newmmPosition > mmPosition) {
+        p22_CLR;
+    } else {
+        p22_SET;
+    }
+    
+    // todo: remove
+    position = 0;
+    
+    stepper.period(1.0/170000.0);
+    stepper.write(0.50);
+    
+    mmPosition = newmmPosition;
+}
+
+void stopModule() {
+    stepper.write(0);
+
+#ifdef DEBUG
+    pc.printf("position = %d steps\n", position);
+#endif
+
+    mmPosition = position / settings.steps_per_mm;
+    pc.printf("ok stop position %f mm\n", mmPosition);
+}
+
+void processCommand(void) {
+#ifdef DEBUG
+    pc.printf("debug serial buffer <%s>\n", serialBuffer);
+#endif
+
+    if (serialBuffer[0] == '\0') {
+        // todo: empty command stop the stage
+        stopModule();
+    } else if (serialBuffer[0] == '$') {
+        // '$' - print or change settings
+        if (serialBuffer[1] == '\0') {
+            print_settings();
+        } else if (serialBuffer[1] >= '0' and serialBuffer[1] <='9' and serialBuffer[2] == '=') {
+            // todo: save settings to file
+            if (set_param(serialBuffer[1], &serialBuffer[3])) {
+                save_settings(SETTINGS_FILE);
+            }
+        } else {
+            invalidCommand();
+        }
+    } else if (serialBuffer[0] == '?' && serialBuffer[1] == '\0') {
+        // todo: print in millimeters
+        pc.printf("ok %f\n", mmPosition);
+    } else {
+        // todo: parse G-code here
+        char *p = serialBuffer, *endP = serialBuffer;
+        uint32_t ulValue;
+        double dblValue;
+        
+        bool error = false;
+        
+        bool newAbsMovementMode = absMovementMode;
+        double newmmPosition = mmPosition;
+        double newFeedRate = feedRate;
+        
+        bool move = false;
+        
+        while (*p != '\0') {
+            switch (*p) {
+                case 'G':
+                    p++;
+                    ulValue = strtoul(p, &endP, 10);
+                    if (p == endP) {
+                        pc.printf("err invalid value for command G: %s\n", p);
+                        error = true;
+                    } else {
+#ifdef DEBUG
+                        pc.printf("debug G%u -> %s\n", ulValue, endP);
+#endif
+                        p = endP;
+                        switch (ulValue) {
+                            case 0:
+                                // todo: implement
+                                break;
+                            case 1:
+                                // todo: implement
+                                break;
+                            case 90:
+                                newAbsMovementMode = true;
+                                break;
+                            case 91:
+                                newAbsMovementMode = false;
+                                break;
+                            default:
+                                pc.printf("err invalid value for command G: %u\n", ulValue);
+                                error = true;
+                                break;
+                        }
+                    }
+                    break;
+                case 'X':
+                    p++;
+                    dblValue = strtod(p, &endP);
+                    if (p == endP) {
+                        pc.printf("err invalid value for command X: %s\n", p);
+                        error = true;
+                    } else {
+#ifdef DEBUG
+                        pc.printf("debug X%f -> %s\n", dblValue, endP);
+#endif
+                        p = endP;
+                        newmmPosition = dblValue;
+                        
+                        move = true;
+                    }
+                    break;
+                case 'F':
+                    p++;
+                    dblValue = strtod(p, &endP);
+                    if (p == endP || dblValue < 0.0) {
+                        pc.printf("err invalid value for command F: %s\n", p);
+                        error = true;
+                    } else {
+#ifdef DEBUG
+                        pc.printf("debug parse F%f => rest %s\n", dblValue, endP);
+#endif
+                        p = endP;
+                        newFeedRate = dblValue;
+                    }
+                    break;
+                default:
+                    pc.printf("err invalid command %s\n", p);
+                    error = true;
+                    break;
+            }
+            
+            if (error) {
+                break;
+            }
+            
+        }
+        
+        if (!error) {
+            // todo: check all flags and execute commands here
+            absMovementMode = newAbsMovementMode;
+            //mmPosition = newmmPosition;
+            feedRate = newFeedRate;
+            // todo: run line module here
+            
+            pc.printf("absMovementMode = %u\n", absMovementMode);
+            pc.printf("mmPosition = %f\n", mmPosition);
+            pc.printf("feedRate = %f\n", feedRate);
+
+            if (move) {
+                moveModule(newmmPosition);
+            }
+        }
+    }
+}
+
+
+void readChar(void) {
+    int ch;
+    
+#ifdef DEBUG
+    LED4_ON;
+#endif
+    
+    ch = pc.getc();
+    if (serialPosition < SERIAL_BUFFER_SIZE) {
+        
+    } else {
+        pc.printf("\nToo long string, should be <= %d characters.\n", SERIAL_BUFFER_SIZE);
+        serialPosition = 0;
+    }
+    if (ch == ' ' || ch == '\t') {
+        // ignore space characters
+    } else {
+    
+        if (ch == '\n') {
+            serialBuffer[serialPosition] = '\0';
+            processCommand();
+            serialPosition = 0;
+            serialBuffer[serialPosition] = '\0';
+        } else {
+            if (ch >= 'a' and ch <= 'z') {
+                ch = 'A' + (ch - 'a'); // convert to upper case
+            }
+            serialBuffer[serialPosition++] = ch;
+        }
+    }
+
+#ifdef DEBUG
+    LED4_OFF;
+#endif
+}
+
+
+
+void update_position();
+
+
+extern "C" void EINT3_IRQHandler (void) __irq {
+    if (LPC_GPIOINT->IntStatus & 0x1) {
+        if (LPC_GPIOINT->IO0IntStatF & counterIntMask) {
+            update_position();
+        }
+    }
+    
+    LPC_GPIOINT->IO2IntClr = (LPC_GPIOINT->IO2IntStatR | LPC_GPIOINT->IO2IntStatF);
+    LPC_GPIOINT->IO0IntClr = (LPC_GPIOINT->IO0IntStatR | LPC_GPIOINT->IO0IntStatF);
+
+}
+
+void update_position() {
+    position++;
+    /*
+    if (position > 0) {
+        position--;
+    } else {
+        position = 170000;
+    }*/
+}
+
+
+void event_irq_init(void) {
+    p30_AS_INPUT;
+    // Enable p30 is P0_4 for rising edge interrupt generation.
+    LPC_GPIOINT->IO0IntEnF |= counterIntMask;
+    //NVIC_SetPriority(EINT3_IRQn, 1);
+    // Enable the interrupt
+    NVIC_EnableIRQ(EINT3_IRQn);
+}
+
+
+
+int main() {
+    LED1_USE; // step movement
+    LED4_USE; // serial port receive
+    
+    p22_AS_OUTPUT;
+    
+    position = 0;
+    
+    pc.baud(115200);
+    pc.attach(readChar);
+
+    pc.printf("IGNB-SM %s ['$' for help]\n", VERSION);
+    
+    load_settings(SETTINGS_FILE);
+    
+    //stepper.period(1.0/170000.0);
+    //stepper.write(0.50);
+
+    event_irq_init();
+
+    while(1) {
+        //position = 0;
+
+        //LED1_ON;
+        //stepper.write(0.5);
+        //wait(1);
+        //stepper.write(0.0);
+        //LED1_OFF;
+        //pc.printf("%d\r\n", position);
+        //wait(1);
+    }
+}