File content as of revision 3:82c00c8e2cb4:

#include "mbed.h"
#include "math.h"

#include "PreferenceWriter.h"
#include "FlashWriter.h"
#include "CommandProcessor.h"
#include "config.h"

Serial pc(USBTX, USBRX);
PreferenceWriter pref(6);
PwmOut led(LED1);

int index = 0, loop_counter = 0;
char linebuf[128];
float p = 0.0f;
float torque_pct = 0.0f, user_cmd = 0.0f;
float vout = 0.0f;

void rxCallback() {
    while (pc.readable()) {
        char c = pc.getc();
        if (c != 127 && c != '\r' && c != '\t') {
            linebuf[index] = c;
            index++;
            pc.putc(c);
        } else if (c == 127) {
            if (index > 0) {
                index--;
                pc.putc(c);
            }
        } else if (c == '\r') {
            linebuf[index] = 0;
            pc.putc(c);
            processCmd(&pc, &pref, linebuf);
            index = 0;
            pc.putc('>');
        }
    }
}

void slow_loop() {
    switch (BREMS_src) {
    case CMD_SRC_RC:
        //rc throttle code here;
        break;        
    case CMD_SRC_ANALOG:
        //analog throttle code here;
        break;
    case CMD_SRC_SERIAL:
    case CMD_SRC_TERMINAL:
    case CMD_SRC_CAN:
        //we presume these are set by free-running interrupts;
        break;
    default:
        break;
    }
}

void commutate() {
    p += 0.001f;
    if (p >= 6.28318f) p = 0.0f;
    
    loop_counter++;
    if (loop_counter % 50 == 0) {
        loop_counter = 0;
        slow_loop();
    }
    
    switch (BREMS_op) {
    case OP_TORQUE:
        torque_pct = user_cmd;
        break;
    case OP_DRIVING:
        //driving mode here;
        break;
    case OP_SPEED:
        //speed loop here;
        break;
    case OP_POSITION:
        //position loop here;
        break;
    }
    
    switch (BREMS_mode) {
    case MODE_RUN:
        vout = torque_pct * sinf(p);
        break;
    case MODE_CFG:
        vout = 0.0f;
        break;
    case MODE_ZERO:
        //zero'ing code here;
        break;
    case MODE_CHR:
        //wizarding code here;
        break;
    default:
        break;
    }
    
    led = 0.5f + 0.5f * vout;
}

int main() {    
    pc.baud(115200);
    pc.attach(rxCallback);
    pc.printf("%s\n", "FOC'ed in the bot version A");
    cmd_reload(&pc, &pref);
    pc.printf("%s", ">");
    
    Ticker tick;
    tick.attach_us(commutate, 200);
    
    for(;;) {
    }
}