Alexander Danilov / Mbed 2 deprecated FastStepDriver

Firmware for single step driver with fast step (up to 340kHz) and simple gcode interpreter.

Dependencies:   FastPWM SimpleIOMacros mbed

main.cpp@2:33a99f65551d, 2013-03-03 (annotated)

Committer:
daapp
Date:
Sun Mar 03 19:56:48 2013 +0000
Revision:
2:33a99f65551d
Parent:
1:86997189bb6b 
Acceleration begin with incorrect delay.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daapp 2:33a99f65551d 1 /* picocom -b 115200 -c --imap lfcrlf --omap crlf /dev/ttyACM0 */
daapp 2:33a99f65551d 2
daapp 2:33a99f65551d 3
daapp 2:33a99f65551d 4 /*
daapp 2:33a99f65551d 5 todo: when command complete it should print "ok"
daapp 2:33a99f65551d 6 todo: when value of parameter changed print "ok saved"
daapp 2:33a99f65551d 7 todo: when value of parameter is invalid print "err invalid value"
daapp 2:33a99f65551d 8 todo: add G command for set zero here
daapp 2:33a99f65551d 9 */
daapp 2:33a99f65551d 10
daapp 2:33a99f65551d 11 #include "mbed.h"
daapp 2:33a99f65551d 12 #include "FastPWM.h"
daapp 2:33a99f65551d 13 #include "IOMacros.h"
daapp 2:33a99f65551d 14
daapp 2:33a99f65551d 15 #define VERSION "0.1"
daapp 2:33a99f65551d 16
daapp 2:33a99f65551d 17 #define DEBUG
daapp 2:33a99f65551d 18
daapp 2:33a99f65551d 19 #define FILESYSTEM_ROOT "local"
daapp 2:33a99f65551d 20 #define SETTINGS_FILE ("/" FILESYSTEM_ROOT "/settings.txt")
daapp 2:33a99f65551d 21 #define SETTINGS_BACKUP_FILE ("/" FILESYSTEM_ROOT "/settings.bak")
daapp 2:33a99f65551d 22
daapp 2:33a99f65551d 23 LocalFileSystem local(FILESYSTEM_ROOT);
daapp 2:33a99f65551d 24
daapp 2:33a99f65551d 25
daapp 2:33a99f65551d 26 /* p30 - P0_4 */
daapp 2:33a99f65551d 27 // const uint32_t counterPin = 4;
daapp 2:33a99f65551d 28 const uint32_t count_int_mask = p30_SET_MASK;
daapp 2:33a99f65551d 29
daapp 2:33a99f65551d 30 volatile int32_t position;
daapp 2:33a99f65551d 31
daapp 2:33a99f65551d 32 FastPWM stepper(p21);
daapp 2:33a99f65551d 33
daapp 2:33a99f65551d 34 // Dir pin is p22 - P2.4
daapp 2:33a99f65551d 35
daapp 2:33a99f65551d 36
daapp 2:33a99f65551d 37 /* G code interpreter state*/
daapp 2:33a99f65551d 38 bool abs_movement_mode = true;
daapp 2:33a99f65551d 39 double position_mm = 0.0; // mm
daapp 2:33a99f65551d 40 double feed_rate = 1.0; // mm/sec^2
daapp 2:33a99f65551d 41
daapp 2:33a99f65551d 42 /* *** Serial port *** */
daapp 2:33a99f65551d 43 #define INPUT_BUFFER_SIZE 40
daapp 2:33a99f65551d 44
daapp 2:33a99f65551d 45 Serial pc(USBTX, USBRX);
daapp 2:33a99f65551d 46
daapp 2:33a99f65551d 47 char input_buffer[INPUT_BUFFER_SIZE+1];
daapp 2:33a99f65551d 48 int input_position = 0;
daapp 2:33a99f65551d 49
daapp 2:33a99f65551d 50
daapp 2:33a99f65551d 51 /* *** Settings *** */
daapp 2:33a99f65551d 52
daapp 2:33a99f65551d 53 typedef struct {
daapp 2:33a99f65551d 54 double steps_per_mm;
daapp 2:33a99f65551d 55 double acceleration; // mm/sec^2
daapp 2:33a99f65551d 56 double start_speed; // mm/sec^2
daapp 2:33a99f65551d 57 } settings_t;
daapp 2:33a99f65551d 58
daapp 2:33a99f65551d 59 settings_t settings;
daapp 2:33a99f65551d 60
daapp 2:33a99f65551d 61 // return true - if parsed succesfully
daapp 2:33a99f65551d 62 // return false - if error during parsing
daapp 2:33a99f65551d 63 bool parse_settings(FILE *fp, settings_t *settings) {
daapp 2:33a99f65551d 64 // todo: write parser
daapp 2:33a99f65551d 65 return true;
daapp 2:33a99f65551d 66 }
daapp 2:33a99f65551d 67
daapp 2:33a99f65551d 68 void load_settings(char *filename) {
daapp 2:33a99f65551d 69 //FILE *fp;
daapp 2:33a99f65551d 70
daapp 2:33a99f65551d 71 #ifdef DEBUG
daapp 2:33a99f65551d 72 pc.printf("debug load settings from file \"%s\"\n", filename);
daapp 2:33a99f65551d 73 #endif
daapp 2:33a99f65551d 74
daapp 2:33a99f65551d 75 //fp = fopen(filename, "r");
daapp 2:33a99f65551d 76 //if (fp == NULL) {
daapp 2:33a99f65551d 77 settings.steps_per_mm = 12800.0 / 150.0; // 170.666667;
daapp 2:33a99f65551d 78 settings.acceleration = 2000.000; // mm/sec^2
daapp 2:33a99f65551d 79 settings.start_speed = 100.0;
daapp 2:33a99f65551d 80 //} else {
daapp 2:33a99f65551d 81 // parse_settings(fp, &settings);
daapp 2:33a99f65551d 82 // fclose(fp);
daapp 2:33a99f65551d 83 //}
daapp 2:33a99f65551d 84 }
daapp 2:33a99f65551d 85
daapp 2:33a99f65551d 86 void save_settings(char *filename) {
daapp 2:33a99f65551d 87 FILE *fp;
daapp 2:33a99f65551d 88
daapp 2:33a99f65551d 89 fp = fopen(SETTINGS_FILE, "w");
daapp 2:33a99f65551d 90 if (fp != NULL) {
daapp 2:33a99f65551d 91 fprintf(fp, "$0=%f\n", settings.steps_per_mm);
daapp 2:33a99f65551d 92 fprintf(fp, "$1=%f\n", settings.acceleration);
daapp 2:33a99f65551d 93 fprintf(fp, "$2=%f\n", settings.start_speed);
daapp 2:33a99f65551d 94 fclose(fp);
daapp 2:33a99f65551d 95 #ifdef DEBUG
daapp 2:33a99f65551d 96 pc.printf("ok settings saved to %s\n", SETTINGS_FILE);
daapp 2:33a99f65551d 97 #endif
daapp 2:33a99f65551d 98 } else {
daapp 2:33a99f65551d 99 pc.printf("err unable to open %s for writing\n", SETTINGS_FILE);
daapp 2:33a99f65551d 100 }
daapp 2:33a99f65551d 101 }
daapp 2:33a99f65551d 102
daapp 2:33a99f65551d 103 void print_settings() {
daapp 2:33a99f65551d 104 pc.printf("$0=%f (x, steps/mm)\n", settings.steps_per_mm);
daapp 2:33a99f65551d 105 pc.printf("$1=%f (x accel, mm/sec^2)\n", settings.acceleration);
daapp 2:33a99f65551d 106 pc.printf("$2=%f (x start acceleration, mm/sec)\n", settings.start_speed);
daapp 2:33a99f65551d 107 pc.printf("ok\n");
daapp 2:33a99f65551d 108 }
daapp 2:33a99f65551d 109
daapp 2:33a99f65551d 110 // return true if no error
daapp 2:33a99f65551d 111 // return false if error (invalid parameter or value)
daapp 2:33a99f65551d 112 bool set_param(char name, char *value) {
daapp 2:33a99f65551d 113 if (name >= '0' && name <= '2') {
daapp 2:33a99f65551d 114 char *rest = value;
daapp 2:33a99f65551d 115 double dbl_value;
daapp 2:33a99f65551d 116 bool error = false;
daapp 2:33a99f65551d 117
daapp 2:33a99f65551d 118 dbl_value = strtod(value, &rest);
daapp 2:33a99f65551d 119 if (value == rest || *rest != '\0') {
daapp 2:33a99f65551d 120 pc.printf("err invalid value for command $%c: %s\n", name, value);
daapp 2:33a99f65551d 121 error = true;
daapp 2:33a99f65551d 122 } else {
daapp 2:33a99f65551d 123
daapp 2:33a99f65551d 124 #ifdef DEBUG
daapp 2:33a99f65551d 125 pc.printf("debug $%c=%f -> %s\n", name, dbl_value, rest);
daapp 2:33a99f65551d 126 #endif
daapp 2:33a99f65551d 127
daapp 2:33a99f65551d 128 switch (name) {
daapp 2:33a99f65551d 129 case '0':
daapp 2:33a99f65551d 130 if (dbl_value > 0.0) {
daapp 2:33a99f65551d 131 settings.steps_per_mm = dbl_value;
daapp 2:33a99f65551d 132 } else {
daapp 2:33a99f65551d 133 pc.printf("err value should be > 0.0, but %f\n", dbl_value);
daapp 2:33a99f65551d 134 error = true;
daapp 2:33a99f65551d 135 }
daapp 2:33a99f65551d 136 break;
daapp 2:33a99f65551d 137 case '1':
daapp 2:33a99f65551d 138 if (dbl_value > 0.0) {
daapp 2:33a99f65551d 139 settings.acceleration = dbl_value;
daapp 2:33a99f65551d 140 } else {
daapp 2:33a99f65551d 141 pc.printf("err value should be > 0.0, but %f\n", dbl_value);
daapp 2:33a99f65551d 142 error = true;
daapp 2:33a99f65551d 143 }
daapp 2:33a99f65551d 144 break;
daapp 2:33a99f65551d 145 case '2':
daapp 2:33a99f65551d 146 if (dbl_value >= 0.0) {
daapp 2:33a99f65551d 147 settings.start_speed = dbl_value;
daapp 2:33a99f65551d 148 } else {
daapp 2:33a99f65551d 149 pc.printf("err value should be >= 0.0, but %f\n", dbl_value);
daapp 2:33a99f65551d 150 error = true;
daapp 2:33a99f65551d 151 }
daapp 2:33a99f65551d 152 break;
daapp 2:33a99f65551d 153 default:
daapp 2:33a99f65551d 154 pc.printf("err parameter %c unrecognized\n", name);
daapp 2:33a99f65551d 155 error = true;
daapp 2:33a99f65551d 156 break;
daapp 2:33a99f65551d 157 }
daapp 2:33a99f65551d 158 }
daapp 2:33a99f65551d 159 return !error;
daapp 2:33a99f65551d 160 } else {
daapp 2:33a99f65551d 161 pc.printf("err invalid parameter %c\n", name);
daapp 2:33a99f65551d 162 return false;
daapp 2:33a99f65551d 163 }
daapp 2:33a99f65551d 164 }
daapp 2:33a99f65551d 165
daapp 2:33a99f65551d 166
daapp 2:33a99f65551d 167 void invalid_command() {
daapp 2:33a99f65551d 168 pc.printf("err invalid command %s\n", input_buffer);
daapp 2:33a99f65551d 169 }
daapp 2:33a99f65551d 170
daapp 2:33a99f65551d 171 enum MOVE_STATE {
daapp 2:33a99f65551d 172 STOP,
daapp 2:33a99f65551d 173 ACCELERATION,
daapp 2:33a99f65551d 174 MOVE,
daapp 2:33a99f65551d 175 DECELERATION
daapp 2:33a99f65551d 176 };
daapp 2:33a99f65551d 177
daapp 2:33a99f65551d 178 enum MOVE_DIR {
daapp 2:33a99f65551d 179 CW = 1,
daapp 2:33a99f65551d 180 CCW = -1
daapp 2:33a99f65551d 181 };
daapp 2:33a99f65551d 182
daapp 2:33a99f65551d 183 volatile double current_freq, max_freq;
daapp 2:33a99f65551d 184 volatile double delta_freq;
daapp 2:33a99f65551d 185 volatile uint64_t accel_pulses = 0;
daapp 2:33a99f65551d 186 volatile uint32_t accel_pulses_left, decel_pulses_left, move_pulses_left; // pulses left until end of movement
daapp 2:33a99f65551d 187 volatile MOVE_DIR dir;
daapp 2:33a99f65551d 188
daapp 2:33a99f65551d 189 // todo: remove, because useless
daapp 2:33a99f65551d 190 volatile double last_current_freq;
daapp 2:33a99f65551d 191 volatile MOVE_STATE last_move_state;
daapp 2:33a99f65551d 192
daapp 2:33a99f65551d 193 volatile MOVE_STATE move_state = STOP;
daapp 2:33a99f65551d 194
daapp 2:33a99f65551d 195 void move_module(double new_position_mm) {
daapp 2:33a99f65551d 196 uint32_t distance_pulses;
daapp 2:33a99f65551d 197
daapp 2:33a99f65551d 198 LED1_OFF;
daapp 2:33a99f65551d 199 LED2_OFF;
daapp 2:33a99f65551d 200 LED3_OFF;
daapp 2:33a99f65551d 201
daapp 2:33a99f65551d 202 new_position_mm = abs_movement_mode ? new_position_mm : position_mm + new_position_mm;
daapp 2:33a99f65551d 203
daapp 2:33a99f65551d 204 distance_pulses = (uint32_t) ceil(fabs(new_position_mm - position_mm) * settings.steps_per_mm);
daapp 2:33a99f65551d 205
daapp 2:33a99f65551d 206 #ifdef DEBUG
daapp 2:33a99f65551d 207 pc.printf("debug move from %f to %f mm\n", position_mm, new_position_mm);
daapp 2:33a99f65551d 208 pc.printf("debug move from %f to %f pulses\n", position_mm * settings.steps_per_mm, new_position_mm * settings.steps_per_mm);
daapp 2:33a99f65551d 209 #endif
daapp 2:33a99f65551d 210
daapp 2:33a99f65551d 211 if (new_position_mm > position_mm) {
daapp 2:33a99f65551d 212 p22_CLR; // positive dir
daapp 2:33a99f65551d 213 dir = CW;
daapp 2:33a99f65551d 214 } else {
daapp 2:33a99f65551d 215 p22_SET; // negative dir
daapp 2:33a99f65551d 216 dir = CCW;
daapp 2:33a99f65551d 217 }
daapp 2:33a99f65551d 218
daapp 2:33a99f65551d 219 // todo: remove
daapp 2:33a99f65551d 220 //position = 0;
daapp 2:33a99f65551d 221
daapp 2:33a99f65551d 222 double start_freq = settings.start_speed * settings.steps_per_mm ;
daapp 2:33a99f65551d 223
daapp 2:33a99f65551d 224 // = freeRate / 60sec / acc
daapp 2:33a99f65551d 225 double accTime = feed_rate / 60.0 / settings.acceleration;
daapp 2:33a99f65551d 226 // = pulse/mm mm/sec
daapp 2:33a99f65551d 227 max_freq = settings.steps_per_mm * (feed_rate/60.0);
daapp 2:33a99f65551d 228
daapp 2:33a99f65551d 229
daapp 2:33a99f65551d 230 if (max_freq < start_freq) {
daapp 2:33a99f65551d 231 delta_freq = 0.0;
daapp 2:33a99f65551d 232 } else {
daapp 2:33a99f65551d 233 delta_freq = (max_freq - start_freq) / (((start_freq + max_freq) / 2.0) * accTime);
daapp 2:33a99f65551d 234 }
daapp 2:33a99f65551d 235
daapp 2:33a99f65551d 236 accel_pulses = (uint32_t) ceil((max_freq - start_freq) / delta_freq);
daapp 2:33a99f65551d 237
daapp 2:33a99f65551d 238 accel_pulses_left = accel_pulses;
daapp 2:33a99f65551d 239 decel_pulses_left = accel_pulses;
daapp 2:33a99f65551d 240 move_pulses_left = distance_pulses - accel_pulses_left - decel_pulses_left;
daapp 2:33a99f65551d 241
daapp 2:33a99f65551d 242 current_freq = start_freq > 1.0 ? start_freq : 1.0;
daapp 2:33a99f65551d 243
daapp 2:33a99f65551d 244 #ifdef DEBUG
daapp 2:33a99f65551d 245 pc.printf("debug frequency from %f to %f step %f time %f\n", start_freq, max_freq, delta_freq, accTime);
daapp 2:33a99f65551d 246 pc.printf("debug distance_pulses %u\n", distance_pulses);
daapp 2:33a99f65551d 247 pc.printf("debug %u + %u + %u\n", accel_pulses_left, move_pulses_left, decel_pulses_left);
daapp 2:33a99f65551d 248 pc.printf("debug current_freq %f Hz\n", current_freq);
daapp 2:33a99f65551d 249 #endif
daapp 2:33a99f65551d 250
daapp 2:33a99f65551d 251
daapp 2:33a99f65551d 252
daapp 2:33a99f65551d 253 LED1_ON;
daapp 2:33a99f65551d 254
daapp 2:33a99f65551d 255 __disable_irq();
daapp 2:33a99f65551d 256
daapp 2:33a99f65551d 257 move_state = ACCELERATION;
daapp 2:33a99f65551d 258
daapp 2:33a99f65551d 259 stepper.period(1.0/current_freq); //1.0/ (settings.steps_per_mm * 1000));
daapp 2:33a99f65551d 260 stepper.write(0.50);
daapp 2:33a99f65551d 261
daapp 2:33a99f65551d 262 __enable_irq();
daapp 2:33a99f65551d 263
daapp 2:33a99f65551d 264
daapp 2:33a99f65551d 265 //position_mm = newposition_mm;
daapp 2:33a99f65551d 266 }
daapp 2:33a99f65551d 267
daapp 2:33a99f65551d 268
daapp 2:33a99f65551d 269
daapp 2:33a99f65551d 270 void stop_module() {
daapp 2:33a99f65551d 271 if (move_state != STOP) {
daapp 2:33a99f65551d 272 move_state = DECELERATION;
daapp 2:33a99f65551d 273
daapp 2:33a99f65551d 274 #ifdef DEBUG
daapp 2:33a99f65551d 275 pc.printf("position = %d steps\n", position);
daapp 2:33a99f65551d 276 pc.printf("debug STOP: %u %u %u %f\n", accel_pulses_left, move_pulses_left, decel_pulses_left,current_freq);
daapp 2:33a99f65551d 277 pc.printf("debug last_current_freq = %f\tlast_move_state=%d\n", last_current_freq, last_move_state);
daapp 2:33a99f65551d 278 #endif
daapp 2:33a99f65551d 279
daapp 2:33a99f65551d 280 position_mm = position / settings.steps_per_mm;
daapp 2:33a99f65551d 281 pc.printf("ok manual stop position %f mm\n", position_mm);
daapp 2:33a99f65551d 282 }
daapp 2:33a99f65551d 283 }
daapp 2:33a99f65551d 284
daapp 2:33a99f65551d 285 void update_position() {
daapp 2:33a99f65551d 286 // position += dir;
daapp 2:33a99f65551d 287 // __disable_irq();
daapp 2:33a99f65551d 288
daapp 2:33a99f65551d 289 switch (move_state) {
daapp 2:33a99f65551d 290
daapp 2:33a99f65551d 291 case ACCELERATION:
daapp 2:33a99f65551d 292 accel_pulses_left--;
daapp 2:33a99f65551d 293 if (accel_pulses_left > 0) {
daapp 2:33a99f65551d 294 current_freq += delta_freq;
daapp 2:33a99f65551d 295 } else {
daapp 2:33a99f65551d 296 current_freq = max_freq;
daapp 2:33a99f65551d 297 move_state = MOVE;
daapp 2:33a99f65551d 298 LED2_ON;
daapp 2:33a99f65551d 299 }
daapp 2:33a99f65551d 300 stepper.period(1.0/current_freq);
daapp 2:33a99f65551d 301
daapp 2:33a99f65551d 302 break;
daapp 2:33a99f65551d 303
daapp 2:33a99f65551d 304 case MOVE:
daapp 2:33a99f65551d 305 move_pulses_left--;
daapp 2:33a99f65551d 306 if (move_pulses_left > 0) {
daapp 2:33a99f65551d 307
daapp 2:33a99f65551d 308 } else {
daapp 2:33a99f65551d 309 move_state = DECELERATION;
daapp 2:33a99f65551d 310 LED3_ON;
daapp 2:33a99f65551d 311 }
daapp 2:33a99f65551d 312 break;
daapp 2:33a99f65551d 313
daapp 2:33a99f65551d 314 case DECELERATION:
daapp 2:33a99f65551d 315 decel_pulses_left--;
daapp 2:33a99f65551d 316
daapp 2:33a99f65551d 317 if (decel_pulses_left > 0) {
daapp 2:33a99f65551d 318 current_freq -= delta_freq;
daapp 2:33a99f65551d 319 } else {
daapp 2:33a99f65551d 320 move_state = STOP;
daapp 2:33a99f65551d 321 current_freq = 0.0;
daapp 2:33a99f65551d 322 }
daapp 2:33a99f65551d 323
daapp 2:33a99f65551d 324 if (fabs(current_freq) > 0.0001) {
daapp 2:33a99f65551d 325 stepper.period(1.0/current_freq);
daapp 2:33a99f65551d 326 } else {
daapp 2:33a99f65551d 327 stepper.period(0.0);
daapp 2:33a99f65551d 328 }
daapp 2:33a99f65551d 329
daapp 2:33a99f65551d 330 last_current_freq = current_freq;
daapp 2:33a99f65551d 331 last_move_state = move_state;
daapp 2:33a99f65551d 332 break;
daapp 2:33a99f65551d 333
daapp 2:33a99f65551d 334 case STOP:
daapp 2:33a99f65551d 335 stepper.period(0.0);
daapp 2:33a99f65551d 336 stepper.write(0.0);
daapp 2:33a99f65551d 337 p27_SET;
daapp 2:33a99f65551d 338 LED1_OFF;
daapp 2:33a99f65551d 339 LED2_OFF;
daapp 2:33a99f65551d 340 LED3_OFF;
daapp 2:33a99f65551d 341 //pc.printf("ok stop position %f mm\n", position_mm);
daapp 2:33a99f65551d 342 break;
daapp 2:33a99f65551d 343 }
daapp 2:33a99f65551d 344
daapp 2:33a99f65551d 345 // __enable_irq();
daapp 2:33a99f65551d 346 }
daapp 2:33a99f65551d 347
daapp 2:33a99f65551d 348
daapp 2:33a99f65551d 349
daapp 2:33a99f65551d 350 void process_command(void) {
daapp 2:33a99f65551d 351
daapp 2:33a99f65551d 352 #ifdef DEBUG
daapp 2:33a99f65551d 353 pc.printf("debug serial buffer <%s>\n", input_buffer);
daapp 2:33a99f65551d 354 #endif
daapp 2:33a99f65551d 355
daapp 2:33a99f65551d 356 if (input_buffer[0] == '\0') {
daapp 2:33a99f65551d 357 // todo: empty command stop the stage
daapp 2:33a99f65551d 358 stop_module();
daapp 2:33a99f65551d 359 } else if (input_buffer[0] == '$') {
daapp 2:33a99f65551d 360 // '$' - print or change settings
daapp 2:33a99f65551d 361 if (input_buffer[1] == '\0') {
daapp 2:33a99f65551d 362 print_settings();
daapp 2:33a99f65551d 363 } else if (input_buffer[1] >= '0' and input_buffer[1] <='9' and input_buffer[2] == '=') {
daapp 2:33a99f65551d 364 // todo: save settings to file
daapp 2:33a99f65551d 365 if (set_param(input_buffer[1], &input_buffer[3])) {
daapp 2:33a99f65551d 366 save_settings(SETTINGS_FILE);
daapp 2:33a99f65551d 367 }
daapp 2:33a99f65551d 368 } else {
daapp 2:33a99f65551d 369 invalid_command();
daapp 2:33a99f65551d 370 }
daapp 2:33a99f65551d 371 } else if (input_buffer[0] == '?' && input_buffer[1] == '\0') {
daapp 2:33a99f65551d 372 pc.printf("ok %f\n", position_mm);
daapp 2:33a99f65551d 373 } else {
daapp 2:33a99f65551d 374 char *p = input_buffer, *rest = input_buffer;
daapp 2:33a99f65551d 375 uint32_t ul_value;
daapp 2:33a99f65551d 376 double dbl_value;
daapp 2:33a99f65551d 377
daapp 2:33a99f65551d 378 bool error = false;
daapp 2:33a99f65551d 379
daapp 2:33a99f65551d 380 bool newabs_movement_mode = abs_movement_mode;
daapp 2:33a99f65551d 381 double newposition_mm = position_mm;
daapp 2:33a99f65551d 382 double new_feed_rate = feed_rate;
daapp 2:33a99f65551d 383
daapp 2:33a99f65551d 384 bool move = false;
daapp 2:33a99f65551d 385
daapp 2:33a99f65551d 386 while (*p != '\0') {
daapp 2:33a99f65551d 387 switch (*p) {
daapp 2:33a99f65551d 388 case 'G':
daapp 2:33a99f65551d 389 p++;
daapp 2:33a99f65551d 390 ul_value = strtoul(p, &rest, 10);
daapp 2:33a99f65551d 391 if (p == rest) {
daapp 2:33a99f65551d 392 pc.printf("err invalid value for command G: %s\n", p);
daapp 2:33a99f65551d 393 error = true;
daapp 2:33a99f65551d 394 } else {
daapp 2:33a99f65551d 395 #ifdef DEBUG
daapp 2:33a99f65551d 396 pc.printf("debug G%u -> %s\n", ul_value, rest);
daapp 2:33a99f65551d 397 #endif
daapp 2:33a99f65551d 398 p = rest;
daapp 2:33a99f65551d 399 switch (ul_value) {
daapp 2:33a99f65551d 400 case 0:
daapp 2:33a99f65551d 401 // todo: implement
daapp 2:33a99f65551d 402 break;
daapp 2:33a99f65551d 403 case 1:
daapp 2:33a99f65551d 404 // todo: implement
daapp 2:33a99f65551d 405 break;
daapp 2:33a99f65551d 406 case 90:
daapp 2:33a99f65551d 407 newabs_movement_mode = true;
daapp 2:33a99f65551d 408 break;
daapp 2:33a99f65551d 409 case 91:
daapp 2:33a99f65551d 410 newabs_movement_mode = false;
daapp 2:33a99f65551d 411 break;
daapp 2:33a99f65551d 412 default:
daapp 2:33a99f65551d 413 pc.printf("err invalid value for command G: %u\n", ul_value);
daapp 2:33a99f65551d 414 error = true;
daapp 2:33a99f65551d 415 break;
daapp 2:33a99f65551d 416 }
daapp 2:33a99f65551d 417 }
daapp 2:33a99f65551d 418 break;
daapp 2:33a99f65551d 419 case 'X':
daapp 2:33a99f65551d 420 p++;
daapp 2:33a99f65551d 421 dbl_value = strtod(p, &rest);
daapp 2:33a99f65551d 422 if (p == rest) {
daapp 2:33a99f65551d 423 pc.printf("err invalid value for command X: %s\n", p);
daapp 2:33a99f65551d 424 error = true;
daapp 2:33a99f65551d 425 } else {
daapp 2:33a99f65551d 426 #ifdef DEBUG
daapp 2:33a99f65551d 427 pc.printf("debug X%f -> %s\n", dbl_value, rest);
daapp 2:33a99f65551d 428 #endif
daapp 2:33a99f65551d 429 p = rest;
daapp 2:33a99f65551d 430 newposition_mm = dbl_value;
daapp 2:33a99f65551d 431
daapp 2:33a99f65551d 432 move = true;
daapp 2:33a99f65551d 433 }
daapp 2:33a99f65551d 434 break;
daapp 2:33a99f65551d 435 case 'F':
daapp 2:33a99f65551d 436 p++;
daapp 2:33a99f65551d 437 dbl_value = strtod(p, &rest);
daapp 2:33a99f65551d 438 if (p == rest || dbl_value < 0.0) {
daapp 2:33a99f65551d 439 pc.printf("err invalid value for command F: %s\n", p);
daapp 2:33a99f65551d 440 error = true;
daapp 2:33a99f65551d 441 } else {
daapp 2:33a99f65551d 442 #ifdef DEBUG
daapp 2:33a99f65551d 443 pc.printf("debug parse F%f => %s\n", dbl_value, rest);
daapp 2:33a99f65551d 444 #endif
daapp 2:33a99f65551d 445 p = rest;
daapp 2:33a99f65551d 446 new_feed_rate = dbl_value;
daapp 2:33a99f65551d 447 }
daapp 2:33a99f65551d 448 break;
daapp 2:33a99f65551d 449 default:
daapp 2:33a99f65551d 450 pc.printf("err invalid command %s\n", p);
daapp 2:33a99f65551d 451 error = true;
daapp 2:33a99f65551d 452 break;
daapp 2:33a99f65551d 453 }
daapp 2:33a99f65551d 454
daapp 2:33a99f65551d 455 if (error) {
daapp 2:33a99f65551d 456 break;
daapp 2:33a99f65551d 457 }
daapp 2:33a99f65551d 458
daapp 2:33a99f65551d 459 }
daapp 2:33a99f65551d 460
daapp 2:33a99f65551d 461 if (!error) {
daapp 2:33a99f65551d 462 abs_movement_mode = newabs_movement_mode;
daapp 2:33a99f65551d 463 feed_rate = new_feed_rate;
daapp 2:33a99f65551d 464
daapp 2:33a99f65551d 465 if (move) {
daapp 2:33a99f65551d 466 move_module(newposition_mm);
daapp 2:33a99f65551d 467 }
daapp 2:33a99f65551d 468 }
daapp 2:33a99f65551d 469 }
daapp 2:33a99f65551d 470 }
daapp 2:33a99f65551d 471
daapp 2:33a99f65551d 472
daapp 2:33a99f65551d 473 void read_char(void) {
daapp 2:33a99f65551d 474 int ch;
daapp 2:33a99f65551d 475
daapp 2:33a99f65551d 476 #ifdef DEBUG
daapp 2:33a99f65551d 477 LED4_ON;
daapp 2:33a99f65551d 478 #endif
daapp 2:33a99f65551d 479
daapp 2:33a99f65551d 480 ch = pc.getc();
daapp 2:33a99f65551d 481 if (input_position < INPUT_BUFFER_SIZE) {
daapp 2:33a99f65551d 482
daapp 2:33a99f65551d 483 } else {
daapp 2:33a99f65551d 484 pc.printf("\nToo long string, should be <= %d characters.\n", INPUT_BUFFER_SIZE);
daapp 2:33a99f65551d 485 input_position = 0;
daapp 2:33a99f65551d 486 }
daapp 2:33a99f65551d 487
daapp 2:33a99f65551d 488 if (ch == ' ' || ch == '\t') {
daapp 2:33a99f65551d 489 // ignore space characters
daapp 2:33a99f65551d 490 } else {
daapp 2:33a99f65551d 491
daapp 2:33a99f65551d 492 if (ch == '\n') {
daapp 2:33a99f65551d 493 input_buffer[input_position] = '\0';
daapp 2:33a99f65551d 494 process_command();
daapp 2:33a99f65551d 495 input_position = 0;
daapp 2:33a99f65551d 496 input_buffer[input_position] = '\0';
daapp 2:33a99f65551d 497 } else {
daapp 2:33a99f65551d 498 if (ch >= 'a' and ch <= 'z') {
daapp 2:33a99f65551d 499 ch = 'A' + (ch - 'a'); // convert to upper case
daapp 2:33a99f65551d 500 }
daapp 2:33a99f65551d 501 input_buffer[input_position++] = ch;
daapp 2:33a99f65551d 502 }
daapp 2:33a99f65551d 503 }
daapp 2:33a99f65551d 504
daapp 2:33a99f65551d 505 #ifdef DEBUG
daapp 2:33a99f65551d 506 LED4_OFF;
daapp 2:33a99f65551d 507 #endif
daapp 2:33a99f65551d 508 }
daapp 2:33a99f65551d 509
daapp 2:33a99f65551d 510
daapp 2:33a99f65551d 511 extern "C" void EINT3_IRQHandler (void) __irq {
daapp 2:33a99f65551d 512 if (LPC_GPIOINT->IntStatus & 0x1) {
daapp 2:33a99f65551d 513 if (LPC_GPIOINT->IO0IntStatF & count_int_mask) {
daapp 2:33a99f65551d 514 update_position();
daapp 2:33a99f65551d 515 }
daapp 2:33a99f65551d 516 }
daapp 2:33a99f65551d 517
daapp 2:33a99f65551d 518 LPC_GPIOINT->IO2IntClr = (LPC_GPIOINT->IO2IntStatR | LPC_GPIOINT->IO2IntStatF);
daapp 2:33a99f65551d 519 LPC_GPIOINT->IO0IntClr = (LPC_GPIOINT->IO0IntStatR | LPC_GPIOINT->IO0IntStatF);
daapp 2:33a99f65551d 520
daapp 2:33a99f65551d 521 }
daapp 2:33a99f65551d 522
daapp 2:33a99f65551d 523 void event_irq_init(void) {
daapp 2:33a99f65551d 524 p30_AS_INPUT;
daapp 2:33a99f65551d 525 // p30_MODE(PIN_REPEAT);
daapp 2:33a99f65551d 526 // Enable p30 is P0_4 for rising edge interrupt generation.
daapp 2:33a99f65551d 527 LPC_GPIOINT->IO0IntEnF |= count_int_mask;
daapp 2:33a99f65551d 528 NVIC_SetPriority(EINT3_IRQn, 1);
daapp 2:33a99f65551d 529 // Enable the interrupt
daapp 2:33a99f65551d 530 NVIC_EnableIRQ(EINT3_IRQn);
daapp 2:33a99f65551d 531 }
daapp 2:33a99f65551d 532
daapp 2:33a99f65551d 533
daapp 2:33a99f65551d 534
daapp 2:33a99f65551d 535 int main() {
daapp 2:33a99f65551d 536 LED1_USE; // acceleration
daapp 2:33a99f65551d 537 LED2_USE; // move
daapp 2:33a99f65551d 538 LED3_USE; // deceleration
daapp 2:33a99f65551d 539 LED4_USE; // serial port receive
daapp 2:33a99f65551d 540
daapp 2:33a99f65551d 541 p22_AS_OUTPUT; // dir pin
daapp 2:33a99f65551d 542
daapp 2:33a99f65551d 543 p27_AS_OUTPUT;
daapp 2:33a99f65551d 544
daapp 2:33a99f65551d 545 position = 0;
daapp 2:33a99f65551d 546
daapp 2:33a99f65551d 547 pc.baud(115200);
daapp 2:33a99f65551d 548 pc.attach(read_char);
daapp 2:33a99f65551d 549
daapp 2:33a99f65551d 550 pc.printf("IGNB-SM %s ['$' for help]\n", VERSION);
daapp 2:33a99f65551d 551
daapp 2:33a99f65551d 552 load_settings(SETTINGS_FILE);
daapp 2:33a99f65551d 553
daapp 2:33a99f65551d 554 event_irq_init();
daapp 2:33a99f65551d 555
daapp 2:33a99f65551d 556 while(1) {}
daapp 2:33a99f65551d 557 }