Alexander Danilov
Firmware for single step driver with fast step (up to 340kHz) and simple gcode interpreter.
Dependencies: FastPWM SimpleIOMacros mbed
Diff: main.cpp
- Revision:
- 2:33a99f65551d
- Parent:
- 1:86997189bb6b
--- a/main.cpp Thu Feb 28 11:40:50 2013 +0000
+++ b/main.cpp Sun Mar 03 19:56:48 2013 +0000
@@ -1,443 +1,557 @@
-/* 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);
- }
-}
+/* picocom -b 115200 -c --imap lfcrlf --omap crlf /dev/ttyACM0 */
+
+
+/*
+ todo: when command complete it should 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 count_int_mask = p30_SET_MASK;
+
+volatile int32_t position;
+
+FastPWM stepper(p21);
+
+// Dir pin is p22 - P2.4
+
+
+/* G code interpreter state*/
+bool abs_movement_mode = true;
+double position_mm = 0.0; // mm
+double feed_rate = 1.0; // mm/sec^2
+
+/* *** Serial port *** */
+#define INPUT_BUFFER_SIZE 40
+
+Serial pc(USBTX, USBRX);
+
+char input_buffer[INPUT_BUFFER_SIZE+1];
+int input_position = 0;
+
+
+/* *** Settings *** */
+
+typedef struct {
+ double steps_per_mm;
+ double acceleration; // mm/sec^2
+ double start_speed; // 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 = 12800.0 / 150.0; // 170.666667;
+ settings.acceleration = 2000.000; // mm/sec^2
+ settings.start_speed = 100.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_speed);
+ 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)\n", settings.start_speed);
+ 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 *rest = value;
+ double dbl_value;
+ bool error = false;
+
+ dbl_value = strtod(value, &rest);
+ if (value == rest || *rest != '\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, dbl_value, rest);
+#endif
+
+ switch (name) {
+ case '0':
+ if (dbl_value > 0.0) {
+ settings.steps_per_mm = dbl_value;
+ } else {
+ pc.printf("err value should be > 0.0, but %f\n", dbl_value);
+ error = true;
+ }
+ break;
+ case '1':
+ if (dbl_value > 0.0) {
+ settings.acceleration = dbl_value;
+ } else {
+ pc.printf("err value should be > 0.0, but %f\n", dbl_value);
+ error = true;
+ }
+ break;
+ case '2':
+ if (dbl_value >= 0.0) {
+ settings.start_speed = dbl_value;
+ } else {
+ pc.printf("err value should be >= 0.0, but %f\n", dbl_value);
+ 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 invalid_command() {
+ pc.printf("err invalid command %s\n", input_buffer);
+}
+
+enum MOVE_STATE {
+ STOP,
+ ACCELERATION,
+ MOVE,
+ DECELERATION
+};
+
+enum MOVE_DIR {
+ CW = 1,
+ CCW = -1
+};
+
+volatile double current_freq, max_freq;
+volatile double delta_freq;
+volatile uint64_t accel_pulses = 0;
+volatile uint32_t accel_pulses_left, decel_pulses_left, move_pulses_left; // pulses left until end of movement
+volatile MOVE_DIR dir;
+
+// todo: remove, because useless
+volatile double last_current_freq;
+volatile MOVE_STATE last_move_state;
+
+volatile MOVE_STATE move_state = STOP;
+
+void move_module(double new_position_mm) {
+ uint32_t distance_pulses;
+
+ LED1_OFF;
+ LED2_OFF;
+ LED3_OFF;
+
+ new_position_mm = abs_movement_mode ? new_position_mm : position_mm + new_position_mm;
+
+ distance_pulses = (uint32_t) ceil(fabs(new_position_mm - position_mm) * settings.steps_per_mm);
+
+#ifdef DEBUG
+ pc.printf("debug move from %f to %f mm\n", position_mm, new_position_mm);
+ pc.printf("debug move from %f to %f pulses\n", position_mm * settings.steps_per_mm, new_position_mm * settings.steps_per_mm);
+#endif
+
+ if (new_position_mm > position_mm) {
+ p22_CLR; // positive dir
+ dir = CW;
+ } else {
+ p22_SET; // negative dir
+ dir = CCW;
+ }
+
+ // todo: remove
+ //position = 0;
+
+ double start_freq = settings.start_speed * settings.steps_per_mm ;
+
+ // = freeRate / 60sec / acc
+ double accTime = feed_rate / 60.0 / settings.acceleration;
+ // = pulse/mm mm/sec
+ max_freq = settings.steps_per_mm * (feed_rate/60.0);
+
+
+ if (max_freq < start_freq) {
+ delta_freq = 0.0;
+ } else {
+ delta_freq = (max_freq - start_freq) / (((start_freq + max_freq) / 2.0) * accTime);
+ }
+
+ accel_pulses = (uint32_t) ceil((max_freq - start_freq) / delta_freq);
+
+ accel_pulses_left = accel_pulses;
+ decel_pulses_left = accel_pulses;
+ move_pulses_left = distance_pulses - accel_pulses_left - decel_pulses_left;
+
+ current_freq = start_freq > 1.0 ? start_freq : 1.0;
+
+#ifdef DEBUG
+ pc.printf("debug frequency from %f to %f step %f time %f\n", start_freq, max_freq, delta_freq, accTime);
+ pc.printf("debug distance_pulses %u\n", distance_pulses);
+ pc.printf("debug %u + %u + %u\n", accel_pulses_left, move_pulses_left, decel_pulses_left);
+ pc.printf("debug current_freq %f Hz\n", current_freq);
+#endif
+
+
+
+ LED1_ON;
+
+ __disable_irq();
+
+ move_state = ACCELERATION;
+
+ stepper.period(1.0/current_freq); //1.0/ (settings.steps_per_mm * 1000));
+ stepper.write(0.50);
+
+ __enable_irq();
+
+
+ //position_mm = newposition_mm;
+}
+
+
+
+void stop_module() {
+ if (move_state != STOP) {
+ move_state = DECELERATION;
+
+#ifdef DEBUG
+ pc.printf("position = %d steps\n", position);
+ pc.printf("debug STOP: %u %u %u %f\n", accel_pulses_left, move_pulses_left, decel_pulses_left,current_freq);
+ pc.printf("debug last_current_freq = %f\tlast_move_state=%d\n", last_current_freq, last_move_state);
+#endif
+
+ position_mm = position / settings.steps_per_mm;
+ pc.printf("ok manual stop position %f mm\n", position_mm);
+ }
+}
+
+void update_position() {
+ // position += dir;
+// __disable_irq();
+
+ switch (move_state) {
+
+ case ACCELERATION:
+ accel_pulses_left--;
+ if (accel_pulses_left > 0) {
+ current_freq += delta_freq;
+ } else {
+ current_freq = max_freq;
+ move_state = MOVE;
+ LED2_ON;
+ }
+ stepper.period(1.0/current_freq);
+
+ break;
+
+ case MOVE:
+ move_pulses_left--;
+ if (move_pulses_left > 0) {
+
+ } else {
+ move_state = DECELERATION;
+ LED3_ON;
+ }
+ break;
+
+ case DECELERATION:
+ decel_pulses_left--;
+
+ if (decel_pulses_left > 0) {
+ current_freq -= delta_freq;
+ } else {
+ move_state = STOP;
+ current_freq = 0.0;
+ }
+
+ if (fabs(current_freq) > 0.0001) {
+ stepper.period(1.0/current_freq);
+ } else {
+ stepper.period(0.0);
+ }
+
+ last_current_freq = current_freq;
+ last_move_state = move_state;
+ break;
+
+ case STOP:
+ stepper.period(0.0);
+ stepper.write(0.0);
+ p27_SET;
+ LED1_OFF;
+ LED2_OFF;
+ LED3_OFF;
+ //pc.printf("ok stop position %f mm\n", position_mm);
+ break;
+ }
+
+// __enable_irq();
+}
+
+
+
+void process_command(void) {
+
+#ifdef DEBUG
+ pc.printf("debug serial buffer <%s>\n", input_buffer);
+#endif
+
+ if (input_buffer[0] == '\0') {
+ // todo: empty command stop the stage
+ stop_module();
+ } else if (input_buffer[0] == '$') {
+ // '$' - print or change settings
+ if (input_buffer[1] == '\0') {
+ print_settings();
+ } else if (input_buffer[1] >= '0' and input_buffer[1] <='9' and input_buffer[2] == '=') {
+ // todo: save settings to file
+ if (set_param(input_buffer[1], &input_buffer[3])) {
+ save_settings(SETTINGS_FILE);
+ }
+ } else {
+ invalid_command();
+ }
+ } else if (input_buffer[0] == '?' && input_buffer[1] == '\0') {
+ pc.printf("ok %f\n", position_mm);
+ } else {
+ char *p = input_buffer, *rest = input_buffer;
+ uint32_t ul_value;
+ double dbl_value;
+
+ bool error = false;
+
+ bool newabs_movement_mode = abs_movement_mode;
+ double newposition_mm = position_mm;
+ double new_feed_rate = feed_rate;
+
+ bool move = false;
+
+ while (*p != '\0') {
+ switch (*p) {
+ case 'G':
+ p++;
+ ul_value = strtoul(p, &rest, 10);
+ if (p == rest) {
+ pc.printf("err invalid value for command G: %s\n", p);
+ error = true;
+ } else {
+#ifdef DEBUG
+ pc.printf("debug G%u -> %s\n", ul_value, rest);
+#endif
+ p = rest;
+ switch (ul_value) {
+ case 0:
+ // todo: implement
+ break;
+ case 1:
+ // todo: implement
+ break;
+ case 90:
+ newabs_movement_mode = true;
+ break;
+ case 91:
+ newabs_movement_mode = false;
+ break;
+ default:
+ pc.printf("err invalid value for command G: %u\n", ul_value);
+ error = true;
+ break;
+ }
+ }
+ break;
+ case 'X':
+ p++;
+ dbl_value = strtod(p, &rest);
+ if (p == rest) {
+ pc.printf("err invalid value for command X: %s\n", p);
+ error = true;
+ } else {
+#ifdef DEBUG
+ pc.printf("debug X%f -> %s\n", dbl_value, rest);
+#endif
+ p = rest;
+ newposition_mm = dbl_value;
+
+ move = true;
+ }
+ break;
+ case 'F':
+ p++;
+ dbl_value = strtod(p, &rest);
+ if (p == rest || dbl_value < 0.0) {
+ pc.printf("err invalid value for command F: %s\n", p);
+ error = true;
+ } else {
+#ifdef DEBUG
+ pc.printf("debug parse F%f => %s\n", dbl_value, rest);
+#endif
+ p = rest;
+ new_feed_rate = dbl_value;
+ }
+ break;
+ default:
+ pc.printf("err invalid command %s\n", p);
+ error = true;
+ break;
+ }
+
+ if (error) {
+ break;
+ }
+
+ }
+
+ if (!error) {
+ abs_movement_mode = newabs_movement_mode;
+ feed_rate = new_feed_rate;
+
+ if (move) {
+ move_module(newposition_mm);
+ }
+ }
+ }
+}
+
+
+void read_char(void) {
+ int ch;
+
+#ifdef DEBUG
+ LED4_ON;
+#endif
+
+ ch = pc.getc();
+ if (input_position < INPUT_BUFFER_SIZE) {
+
+ } else {
+ pc.printf("\nToo long string, should be <= %d characters.\n", INPUT_BUFFER_SIZE);
+ input_position = 0;
+ }
+
+ if (ch == ' ' || ch == '\t') {
+ // ignore space characters
+ } else {
+
+ if (ch == '\n') {
+ input_buffer[input_position] = '\0';
+ process_command();
+ input_position = 0;
+ input_buffer[input_position] = '\0';
+ } else {
+ if (ch >= 'a' and ch <= 'z') {
+ ch = 'A' + (ch - 'a'); // convert to upper case
+ }
+ input_buffer[input_position++] = ch;
+ }
+ }
+
+#ifdef DEBUG
+ LED4_OFF;
+#endif
+}
+
+
+extern "C" void EINT3_IRQHandler (void) __irq {
+ if (LPC_GPIOINT->IntStatus & 0x1) {
+ if (LPC_GPIOINT->IO0IntStatF & count_int_mask) {
+ update_position();
+ }
+ }
+
+ LPC_GPIOINT->IO2IntClr = (LPC_GPIOINT->IO2IntStatR | LPC_GPIOINT->IO2IntStatF);
+ LPC_GPIOINT->IO0IntClr = (LPC_GPIOINT->IO0IntStatR | LPC_GPIOINT->IO0IntStatF);
+
+}
+
+void event_irq_init(void) {
+ p30_AS_INPUT;
+ // p30_MODE(PIN_REPEAT);
+ // Enable p30 is P0_4 for rising edge interrupt generation.
+ LPC_GPIOINT->IO0IntEnF |= count_int_mask;
+ NVIC_SetPriority(EINT3_IRQn, 1);
+ // Enable the interrupt
+ NVIC_EnableIRQ(EINT3_IRQn);
+}
+
+
+
+int main() {
+ LED1_USE; // acceleration
+ LED2_USE; // move
+ LED3_USE; // deceleration
+ LED4_USE; // serial port receive
+
+ p22_AS_OUTPUT; // dir pin
+
+ p27_AS_OUTPUT;
+
+ position = 0;
+
+ pc.baud(115200);
+ pc.attach(read_char);
+
+ pc.printf("IGNB-SM %s ['$' for help]\n", VERSION);
+
+ load_settings(SETTINGS_FILE);
+
+ event_irq_init();
+
+ while(1) {}
+}