main.cpp

00001 /* picocom -b 115200 -c --imap lfcrlf --omap crlf /dev/ttyACM0 */
00002 
00003 
00004 /*
00005     todo: when command complete it should print "ok"
00006     todo: when value of parameter changed print "ok saved"
00007     todo: when value of parameter is invalid print "err invalid value"
00008     todo: add G command for set zero here
00009 */
00010 
00011 #include "mbed.h"
00012 #include "FastPWM.h"
00013 #include "IOMacros.h"
00014 
00015 #define VERSION "0.1"
00016 
00017 #define DEBUG
00018 
00019 #define FILESYSTEM_ROOT "local"
00020 #define SETTINGS_FILE ("/" FILESYSTEM_ROOT "/settings.txt")
00021 #define SETTINGS_BACKUP_FILE ("/" FILESYSTEM_ROOT "/settings.bak")
00022 
00023 LocalFileSystem local(FILESYSTEM_ROOT);
00024 
00025 
00026 /* p30 - P0_4 */
00027 // const uint32_t counterPin = 4;
00028 const uint32_t count_int_mask = p30_SET_MASK;
00029 
00030 volatile int32_t position;
00031 
00032 FastPWM stepper(p21);
00033 
00034 // Dir pin is p22 - P2.4
00035 
00036 
00037 /* G code interpreter state*/
00038 bool abs_movement_mode = true;
00039 double position_mm = 0.0; // mm
00040 double feed_rate = 1.0; // mm/sec^2
00041 
00042 /* *** Serial port *** */
00043 #define INPUT_BUFFER_SIZE 40
00044 
00045 Serial pc(USBTX, USBRX);
00046 
00047 char input_buffer[INPUT_BUFFER_SIZE+1];
00048 int input_position = 0;
00049 
00050 
00051 /* *** Settings *** */
00052 
00053 typedef struct {
00054     double steps_per_mm;
00055     double acceleration; // mm/sec^2
00056     double start_speed; // mm/sec^2
00057 } settings_t;
00058 
00059 settings_t settings;
00060 
00061 // return true - if parsed succesfully
00062 // return false - if error during parsing
00063 bool parse_settings(FILE *fp, settings_t *settings) {
00064     // todo: write parser
00065     return true;
00066 }
00067 
00068 void load_settings(char *filename) {
00069     //FILE *fp;
00070 
00071 #ifdef DEBUG
00072     pc.printf("debug load settings from file \"%s\"\n", filename);
00073 #endif
00074 
00075     //fp = fopen(filename, "r");
00076     //if (fp == NULL) {
00077         settings.steps_per_mm = 12800.0 / 150.0; // 170.666667;
00078         settings.acceleration = 2000.000; // mm/sec^2
00079         settings.start_speed = 100.0;
00080     //} else {
00081     //    parse_settings(fp, &settings);
00082     //    fclose(fp);
00083     //}
00084 }
00085 
00086 void save_settings(char *filename) {
00087     FILE *fp;
00088     
00089     fp = fopen(SETTINGS_FILE, "w");
00090     if (fp != NULL) {
00091         fprintf(fp, "$0=%f\n", settings.steps_per_mm);
00092         fprintf(fp, "$1=%f\n", settings.acceleration);
00093         fprintf(fp, "$2=%f\n", settings.start_speed);
00094         fclose(fp);
00095 #ifdef DEBUG
00096         pc.printf("ok settings saved to %s\n", SETTINGS_FILE);
00097 #endif
00098     } else {
00099         pc.printf("err unable to open %s for writing\n", SETTINGS_FILE);
00100     }
00101 }
00102 
00103 void print_settings() {
00104     pc.printf("$0=%f (x, steps/mm)\n", settings.steps_per_mm);
00105     pc.printf("$1=%f (x accel, mm/sec^2)\n", settings.acceleration);
00106     pc.printf("$2=%f (x start acceleration, mm/sec)\n", settings.start_speed);
00107     pc.printf("ok\n");
00108 }
00109 
00110 // return true if no error
00111 // return false if error (invalid parameter or value)
00112 bool set_param(char name, char *value) {
00113     if (name >= '0' && name <= '2') {
00114         char *rest = value;
00115         double dbl_value;
00116         bool error = false;
00117 
00118         dbl_value = strtod(value, &rest);
00119         if (value == rest || *rest != '\0') {
00120             pc.printf("err invalid value for command $%c: %s\n", name, value);
00121             error = true;
00122         } else {
00123 
00124 #ifdef DEBUG
00125             pc.printf("debug $%c=%f -> %s\n", name, dbl_value, rest);
00126 #endif
00127                                     
00128             switch (name) {
00129                 case '0':
00130                     if (dbl_value > 0.0) {
00131                         settings.steps_per_mm = dbl_value;
00132                     } else {
00133                         pc.printf("err value should be > 0.0, but %f\n", dbl_value);
00134                         error = true;
00135                     }
00136                     break;
00137                 case '1':
00138                     if (dbl_value > 0.0) {
00139                         settings.acceleration = dbl_value;
00140                     } else {
00141                         pc.printf("err value should be > 0.0, but %f\n", dbl_value);
00142                         error = true;
00143                     }
00144                     break;
00145                 case '2':
00146                     if (dbl_value >= 0.0) {
00147                         settings.start_speed = dbl_value;
00148                     } else {
00149                         pc.printf("err value should be >= 0.0, but %f\n", dbl_value);
00150                         error = true;
00151                     }
00152                     break;
00153                 default:
00154                     pc.printf("err parameter %c unrecognized\n", name);
00155                     error = true;
00156                     break;
00157             }
00158         }
00159         return !error;
00160     } else {
00161         pc.printf("err invalid parameter %c\n", name);
00162         return false;
00163     }
00164 }
00165 
00166 
00167 void invalid_command() {
00168     pc.printf("err invalid command %s\n", input_buffer);
00169 }
00170 
00171 enum MOVE_STATE {
00172     STOP,
00173     ACCELERATION,
00174     MOVE,
00175     DECELERATION
00176 };
00177 
00178 enum MOVE_DIR {
00179     CW = 1,
00180     CCW = -1
00181 };
00182 
00183 volatile double current_freq, max_freq;
00184 volatile double delta_freq;
00185 volatile uint64_t accel_pulses = 0;
00186 volatile uint32_t accel_pulses_left, decel_pulses_left, move_pulses_left; // pulses left until end of movement
00187 volatile MOVE_DIR dir;
00188 
00189 // todo: remove, because useless
00190 volatile double last_current_freq;
00191 volatile MOVE_STATE last_move_state;
00192 
00193 volatile MOVE_STATE move_state = STOP;
00194 
00195 void move_module(double new_position_mm) {
00196     uint32_t distance_pulses;
00197 
00198     LED1_OFF;
00199     LED2_OFF;
00200     LED3_OFF;
00201     
00202     new_position_mm = abs_movement_mode ? new_position_mm : position_mm + new_position_mm;
00203     
00204     distance_pulses = (uint32_t) ceil(fabs(new_position_mm - position_mm) * settings.steps_per_mm);
00205 
00206 #ifdef DEBUG    
00207     pc.printf("debug move from %f to %f mm\n", position_mm, new_position_mm);
00208     pc.printf("debug move from %f to %f pulses\n", position_mm * settings.steps_per_mm, new_position_mm * settings.steps_per_mm);
00209 #endif
00210 
00211     if (new_position_mm > position_mm) {
00212         p22_CLR; // positive dir
00213         dir = CW;
00214     } else {
00215         p22_SET; // negative dir
00216         dir = CCW;
00217     }
00218     
00219     // todo: remove
00220     //position = 0;
00221     
00222     double start_freq = settings.start_speed * settings.steps_per_mm ;
00223     
00224     // = freeRate / 60sec / acc
00225     double accTime = feed_rate / 60.0 / settings.acceleration;
00226     // = pulse/mm mm/sec
00227     max_freq = settings.steps_per_mm * (feed_rate/60.0);
00228     
00229     
00230     if (max_freq < start_freq) {
00231         delta_freq = 0.0;
00232     } else {
00233         delta_freq = (max_freq - start_freq) / (((start_freq + max_freq) / 2.0) * accTime);
00234     }
00235     
00236     accel_pulses = (uint32_t) ceil((max_freq - start_freq) / delta_freq);
00237     
00238     accel_pulses_left = accel_pulses;
00239     decel_pulses_left = accel_pulses;
00240     move_pulses_left  = distance_pulses - accel_pulses_left - decel_pulses_left;
00241     
00242     current_freq = start_freq > 1.0 ? start_freq : 1.0;
00243 
00244 #ifdef DEBUG
00245     pc.printf("debug frequency from %f to %f step %f time %f\n", start_freq, max_freq, delta_freq, accTime);
00246     pc.printf("debug distance_pulses %u\n", distance_pulses);
00247     pc.printf("debug %u + %u + %u\n", accel_pulses_left, move_pulses_left, decel_pulses_left);
00248     pc.printf("debug current_freq %f Hz\n", current_freq);
00249 #endif
00250 
00251 
00252 
00253     LED1_ON;
00254 
00255     __disable_irq();
00256 
00257     move_state = ACCELERATION;
00258 
00259     stepper.period(1.0/current_freq); //1.0/ (settings.steps_per_mm * 1000));
00260     stepper.write(0.50);
00261 
00262     __enable_irq();
00263 
00264     
00265     //position_mm = newposition_mm;
00266 }
00267 
00268 
00269 
00270 void stop_module() {
00271     if (move_state != STOP) {
00272         move_state = DECELERATION;
00273 
00274 #ifdef DEBUG
00275         pc.printf("position = %d steps\n", position);
00276         pc.printf("debug STOP: %u %u %u %f\n", accel_pulses_left, move_pulses_left, decel_pulses_left,current_freq);
00277         pc.printf("debug last_current_freq = %f\tlast_move_state=%d\n", last_current_freq, last_move_state);
00278 #endif
00279 
00280         position_mm = position / settings.steps_per_mm;
00281         pc.printf("ok manual stop position %f mm\n", position_mm);
00282     }
00283 }
00284 
00285 void update_position() {
00286    // position += dir;
00287 //    __disable_irq();
00288     
00289     switch (move_state) {
00290 
00291         case ACCELERATION:
00292             accel_pulses_left--;
00293             if (accel_pulses_left > 0) {
00294                 current_freq += delta_freq;
00295             } else {
00296                 current_freq = max_freq;
00297                 move_state = MOVE;
00298                 LED2_ON;
00299             }
00300             stepper.period(1.0/current_freq);
00301             
00302             break;
00303         
00304         case MOVE:
00305             move_pulses_left--;
00306             if (move_pulses_left > 0) {
00307 
00308             } else {
00309                 move_state = DECELERATION;
00310                 LED3_ON;
00311             }
00312             break;
00313         
00314         case DECELERATION:
00315             decel_pulses_left--;
00316 
00317             if (decel_pulses_left > 0) {
00318                 current_freq -= delta_freq;
00319             } else {
00320                 move_state = STOP;
00321                 current_freq = 0.0;
00322             }
00323 
00324             if (fabs(current_freq) > 0.0001) {
00325                 stepper.period(1.0/current_freq);
00326             } else {
00327                 stepper.period(0.0);
00328             }
00329 
00330             last_current_freq = current_freq;
00331             last_move_state = move_state;
00332             break;
00333 
00334         case STOP:
00335             stepper.period(0.0);
00336             stepper.write(0.0);
00337             p27_SET;
00338             LED1_OFF;
00339             LED2_OFF;
00340             LED3_OFF;
00341             //pc.printf("ok stop position %f mm\n", position_mm);
00342             break;
00343     }
00344     
00345 //    __enable_irq();
00346 }
00347 
00348 
00349 
00350 void process_command(void) {
00351 
00352 #ifdef DEBUG
00353     pc.printf("debug serial buffer <%s>\n", input_buffer);
00354 #endif
00355 
00356     if (input_buffer[0] == '\0') {
00357         // todo: empty command stop the stage
00358         stop_module();
00359     } else if (input_buffer[0] == '$') {
00360         // '$' - print or change settings
00361         if (input_buffer[1] == '\0') {
00362             print_settings();
00363         } else if (input_buffer[1] >= '0' and input_buffer[1] <='9' and input_buffer[2] == '=') {
00364             // todo: save settings to file
00365             if (set_param(input_buffer[1], &input_buffer[3])) {
00366                 save_settings(SETTINGS_FILE);
00367             }
00368         } else {
00369             invalid_command();
00370         }
00371     } else if (input_buffer[0] == '?' && input_buffer[1] == '\0') {
00372         pc.printf("ok %f\n", position_mm);
00373     } else {
00374         char *p = input_buffer, *rest = input_buffer;
00375         uint32_t ul_value;
00376         double dbl_value;
00377         
00378         bool error = false;
00379         
00380         bool newabs_movement_mode = abs_movement_mode;
00381         double newposition_mm = position_mm;
00382         double new_feed_rate = feed_rate;
00383         
00384         bool move = false;
00385         
00386         while (*p != '\0') {
00387             switch (*p) {
00388                 case 'G':
00389                     p++;
00390                     ul_value = strtoul(p, &rest, 10);
00391                     if (p == rest) {
00392                         pc.printf("err invalid value for command G: %s\n", p);
00393                         error = true;
00394                     } else {
00395 #ifdef DEBUG
00396                         pc.printf("debug G%u -> %s\n", ul_value, rest);
00397 #endif
00398                         p = rest;
00399                         switch (ul_value) {
00400                             case 0:
00401                                 // todo: implement
00402                                 break;
00403                             case 1:
00404                                 // todo: implement
00405                                 break;
00406                             case 90:
00407                                 newabs_movement_mode = true;
00408                                 break;
00409                             case 91:
00410                                 newabs_movement_mode = false;
00411                                 break;
00412                             default:
00413                                 pc.printf("err invalid value for command G: %u\n", ul_value);
00414                                 error = true;
00415                                 break;
00416                         }
00417                     }
00418                     break;
00419                 case 'X':
00420                     p++;
00421                     dbl_value = strtod(p, &rest);
00422                     if (p == rest) {
00423                         pc.printf("err invalid value for command X: %s\n", p);
00424                         error = true;
00425                     } else {
00426 #ifdef DEBUG
00427                         pc.printf("debug X%f -> %s\n", dbl_value, rest);
00428 #endif
00429                         p = rest;
00430                         newposition_mm = dbl_value;
00431                         
00432                         move = true;
00433                     }
00434                     break;
00435                 case 'F':
00436                     p++;
00437                     dbl_value = strtod(p, &rest);
00438                     if (p == rest || dbl_value < 0.0) {
00439                         pc.printf("err invalid value for command F: %s\n", p);
00440                         error = true;
00441                     } else {
00442 #ifdef DEBUG
00443                         pc.printf("debug parse F%f => %s\n", dbl_value, rest);
00444 #endif
00445                         p = rest;
00446                         new_feed_rate = dbl_value;
00447                     }
00448                     break;
00449                 default:
00450                     pc.printf("err invalid command %s\n", p);
00451                     error = true;
00452                     break;
00453             }
00454             
00455             if (error) {
00456                 break;
00457             }
00458             
00459         }
00460         
00461         if (!error) {
00462             abs_movement_mode = newabs_movement_mode;
00463             feed_rate = new_feed_rate;
00464             
00465             if (move) {
00466                 move_module(newposition_mm);
00467             }
00468         }
00469     }
00470 }
00471 
00472 
00473 void read_char(void) {
00474     int ch;
00475     
00476 #ifdef DEBUG
00477     LED4_ON;
00478 #endif
00479     
00480     ch = pc.getc();
00481     if (input_position < INPUT_BUFFER_SIZE) {
00482         
00483     } else {
00484         pc.printf("\nToo long string, should be <= %d characters.\n", INPUT_BUFFER_SIZE);
00485         input_position = 0;
00486     }
00487 
00488     if (ch == ' ' || ch == '\t') {
00489         // ignore space characters
00490     } else {
00491     
00492         if (ch == '\n') {
00493             input_buffer[input_position] = '\0';
00494             process_command();
00495             input_position = 0;
00496             input_buffer[input_position] = '\0';
00497         } else {
00498             if (ch >= 'a' and ch <= 'z') {
00499                 ch = 'A' + (ch - 'a'); // convert to upper case
00500             }
00501             input_buffer[input_position++] = ch;
00502         }
00503     }
00504 
00505 #ifdef DEBUG
00506     LED4_OFF;
00507 #endif
00508 }
00509 
00510 
00511 extern "C" void EINT3_IRQHandler (void) __irq {
00512     if (LPC_GPIOINT->IntStatus & 0x1) {
00513         if (LPC_GPIOINT->IO0IntStatF & count_int_mask) {
00514             update_position();
00515         }
00516     }
00517     
00518     LPC_GPIOINT->IO2IntClr = (LPC_GPIOINT->IO2IntStatR | LPC_GPIOINT->IO2IntStatF);
00519     LPC_GPIOINT->IO0IntClr = (LPC_GPIOINT->IO0IntStatR | LPC_GPIOINT->IO0IntStatF);
00520 
00521 }
00522 
00523 void event_irq_init(void) {
00524     p30_AS_INPUT;
00525     // p30_MODE(PIN_REPEAT);
00526     // Enable p30 is P0_4 for rising edge interrupt generation.
00527     LPC_GPIOINT->IO0IntEnF |= count_int_mask;
00528     NVIC_SetPriority(EINT3_IRQn, 1);
00529     // Enable the interrupt
00530     NVIC_EnableIRQ(EINT3_IRQn);
00531 }
00532 
00533 
00534 
00535 int main() {
00536     LED1_USE; // acceleration
00537     LED2_USE; // move
00538     LED3_USE; // deceleration
00539     LED4_USE; // serial port receive
00540     
00541     p22_AS_OUTPUT; // dir pin
00542     
00543     p27_AS_OUTPUT;
00544     
00545     position = 0;
00546     
00547     pc.baud(115200);
00548     pc.attach(read_char);
00549 
00550     pc.printf("IGNB-SM %s ['$' for help]\n", VERSION);
00551     
00552     load_settings(SETTINGS_FILE);
00553     
00554     event_irq_init();
00555 
00556     while(1) {}
00557 }