File content as of revision 2:33a99f65551d:

/* 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) {}
}