Revision 3:82c00c8e2cb4, committed 2017-03-07

Comitter:

bwang

Date:

Tue Mar 07 08:06:39 2017 +0000

Parent:

2:cfc39b0843ae

Commit message:

lot's more commands

Changed in this revision

--- a/CommandProcessor/CommandProcessor.cpp	Thu Mar 02 07:36:56 2017 +0000
+++ b/CommandProcessor/CommandProcessor.cpp	Tue Mar 07 08:06:39 2017 +0000
@@ -12,140 +12,10 @@
 #include "config_pins.h"
 #include "config_table.h"
 
-void cmd_ls(Serial *pc) {
-    DPRINT(TORQUE_MODE);
-    FPRINT(MAX_TQPCT_PLUS);
-    FPRINT(MAX_TQPCT_MINUS);
-    FPRINT(TORQUE_MAX);
-    FPRINT(W_MAX);
-    FPRINT(BUS_VOLTAGE);
-    FPRINT(F_SW);
-    FPRINT(K_LOOP_D);
-    FPRINT(KI_BASE_D);
-    FPRINT(K_LOOP_Q);
-    FPRINT(KI_BASE_Q);
-    FPRINT(F_SLOW_LOOP);
-    FPRINT(INTEGRAL_MAX);
-    FPRINT(W_FILTER_STRENGTH);
-    FPRINT(DQ_FILTER_STRENGTH);
-    FPRINT(THROTTLE_FILTER_STRENGTH);
-    FPRINT(KP_D);
-    FPRINT(KI_D);
-    FPRINT(KP_Q);
-    FPRINT(KI_Q);
-    FPRINT(SLOW_LOOP_COUNTER);
-    FPRINT(POLE_PAIRS);
-    FPRINT(POS_OFFSET);
-    FPRINT(RESOLVER_LOBES);
-    DPRINT(CPR);
-    FPRINT(Ld);
-    FPRINT(Lq);
-    FPRINT(FLUX_LINKAGE);
-    FPRINT(Rs);
-    FPRINT(KT);
-    FPRINT(W_SAFE);
-    FPRINT(W_CRAZY);
-    DPRINT(TH_LIMIT_LOW);
-    DPRINT(TH_LIMIT_HIGH);
-    DPRINT(TH_LIMIT_CRAZY);
-    DPRINT(ROWS);
-    DPRINT(COLUMNS);
-    FPRINT(W_STEP);
-}
-
-void cmd_ls2(Serial *pc, char *buf) {
-    float *fptr = checkf(buf);
-    if (fptr != NULL) pc->printf("%s: %f\n", buf, *fptr);
-    int *nptr = NULL;
-    if (fptr == NULL) nptr = checkn(buf);
-    if (nptr != NULL) pc->printf("%s: %d\n", buf, *nptr);
-    if (nptr == NULL && fptr == NULL) pc->printf("%s\n", "No Such Parameter");
-}
-
-void cmd_defaults(Serial *pc) {
-    DEFAULT(TORQUE_MODE);
-    DEFAULT(MAX_TQPCT_PLUS);
-    DEFAULT(MAX_TQPCT_MINUS);
-    DEFAULT(TORQUE_MAX);
-    DEFAULT(W_MAX);
-    DEFAULT(BUS_VOLTAGE);
-    DEFAULT(F_SW);
-    DEFAULT(K_LOOP_D);
-    DEFAULT(KI_BASE_D);
-    DEFAULT(K_LOOP_Q);
-    DEFAULT(KI_BASE_Q);
-    DEFAULT(F_SLOW_LOOP);
-    DEFAULT(INTEGRAL_MAX);
-    DEFAULT(W_FILTER_STRENGTH);
-    DEFAULT(DQ_FILTER_STRENGTH);
-    DEFAULT(THROTTLE_FILTER_STRENGTH);
-    DEFAULT(KP_D);
-    DEFAULT(KI_D);
-    DEFAULT(KP_Q);
-    DEFAULT(KI_Q);
-    DEFAULT(SLOW_LOOP_COUNTER);
-    DEFAULT(POLE_PAIRS);
-    DEFAULT(POS_OFFSET);
-    DEFAULT(RESOLVER_LOBES);
-    DEFAULT(CPR);
-    DEFAULT(Ld);
-    DEFAULT(Lq);
-    DEFAULT(FLUX_LINKAGE);
-    DEFAULT(Rs);
-    DEFAULT(KT);
-    DEFAULT(W_SAFE);
-    DEFAULT(W_CRAZY);
-    DEFAULT(TH_LIMIT_LOW);
-    DEFAULT(TH_LIMIT_HIGH);
-    DEFAULT(TH_LIMIT_CRAZY);
-    DEFAULT(ROWS);
-    DEFAULT(COLUMNS);
-    DEFAULT(W_STEP);
-    pc->printf("Defaults Loaded\n");
-}
-
-void cmd_reload(Serial *pc, PreferenceWriter *pref) {
-    pref->load();
-    pc->printf("Flash Values Loaded\n");
-}
-
-void cmd_set(Serial *pc, char *buf, char *val) {
-    float *fptr = checkf(buf);
-    if (fptr != NULL) *fptr = (float) (atof(val));
-    int *nptr = NULL;
-    if (fptr == NULL) nptr = checkn(buf);
-    if (nptr != NULL) *nptr = (int) (atoi(val));
-    cmd_ls2(pc, buf);
-    if (nptr == NULL && fptr == NULL) pc->printf("%s\n", "No Such Parameter");
-}
-
-void cmd_flush(Serial *pc, PreferenceWriter *pref) {
-    if (!pref->ready()) pref->open();
-    pref->flush();
-}
-
-void cmd_cfg(Serial *pc) {
-    mode = MODE_CFG;
-    pc->printf("%s\n", "Config mode ON");
-}
-
-void cmd_exit(Serial *pc) {
-    mode = MODE_RUN;
-    pc->printf("%s\n", "Run mode ON");
-}
-
 void processCmd(Serial *pc, PreferenceWriter *pref, char *buf) {
     char *tokens[10];
     int len = tokenize(buf, tokens, 10);
     
-    if (mode == MODE_RUN) {
-        if (len == 1 && strcmp(tokens[0], "cfg") == 0) {
-            cmd_cfg(pc);
-        } else {
-            return;
-        }
-    }
-    
     switch (len) {
     case 1:
         if (strcmp(tokens[0], "ls") == 0) cmd_ls(pc);
@@ -153,75 +23,25 @@
         if (strcmp(tokens[0], "reload") == 0) cmd_reload(pc, pref);
         if (strcmp(tokens[0], "load") == 0) cmd_reload(pc, pref);
         if (strcmp(tokens[0], "flush") == 0) cmd_flush(pc, pref);
+        if (strcmp(tokens[0], "mode") == 0) cmd_ls2(pc, tokens[0]);
+        if (strcmp(tokens[0], "src") == 0) cmd_ls2(pc, tokens[0]);
+        if (strcmp(tokens[0], "op") == 0) cmd_ls2(pc, tokens[0]);
+        if (strcmp(tokens[0], "cfg") == 0) cmd_mode(pc, tokens[0]);
+        if (strcmp(tokens[0], "zero") == 0) cmd_mode(pc, tokens[0]);
+        if (strcmp(tokens[0], "wizard") == 0) cmd_mode(pc, tokens[0]);
         if (strcmp(tokens[0], "exit") == 0) cmd_exit(pc);
         break;
     case 2:
         if (strcmp(tokens[0], "ls") == 0) cmd_ls2(pc, tokens[1]);
         if (strcmp(tokens[0], "get") == 0) cmd_ls2(pc, tokens[1]);
+        if (strcmp(tokens[0], "setp") == 0) cmd_setp(pc, tokens[1]);
+        if (strcmp(tokens[0], "mode") == 0) cmd_mode(pc, tokens[1]);
+        if (strcmp(tokens[0], "src") == 0) cmd_src(pc, tokens[1]);
+        if (strcmp(tokens[0], "op") == 0) cmd_op(pc, tokens[1]);
         break;
     case 3:
         if (strcmp(tokens[0], "set") == 0) cmd_set(pc, tokens[1], tokens[2]);
     default:
         break;
     }
-}
-
-int tokenize(char *buf, char **out, int max) {
-    char* tok;
-    int k = 0;
-    
-    tok = strtok(buf, " ");
-    
-    while(tok != NULL && k < max) {
-        out[k] = tok;
-        k++;
-        tok = strtok(NULL, " ");
-    }
-    return k;
-}
-
-#define __check(x) if(strcmp(s, #x) == 0) return &_##x
-float* checkf(char *s) {
-    __check(MAX_TQPCT_PLUS);
-    __check(MAX_TQPCT_MINUS);
-    __check(TORQUE_MAX);
-    __check(W_MAX);
-    __check(BUS_VOLTAGE);
-    __check(F_SW);
-    __check(K_LOOP_D);
-    __check(KI_BASE_D);
-    __check(K_LOOP_Q);
-    __check(KI_BASE_Q);
-    __check(F_SLOW_LOOP);
-    __check(INTEGRAL_MAX);
-    __check(W_FILTER_STRENGTH);
-    __check(DQ_FILTER_STRENGTH);
-    __check(THROTTLE_FILTER_STRENGTH);
-    __check(KP_D);
-    __check(KI_D);
-    __check(KP_Q);
-    __check(KI_Q);
-    __check(POLE_PAIRS);
-    __check(POS_OFFSET);
-    __check(RESOLVER_LOBES);
-    __check(Ld);
-    __check(Lq);
-    __check(FLUX_LINKAGE);
-    __check(Rs);
-    __check(KT);
-    __check(W_SAFE);
-    __check(W_CRAZY);
-    __check(W_STEP);
-    return NULL;
-}
-
-int* checkn(char *s) {
-    __check(TORQUE_MODE);
-    __check(CPR);
-    __check(TH_LIMIT_LOW);
-    __check(TH_LIMIT_HIGH);
-    __check(TH_LIMIT_CRAZY);
-    __check(ROWS);
-    __check(COLUMNS);
-    return NULL;
 }
\ No newline at end of file

--- a/CommandProcessor/CommandProcessor.h	Thu Mar 02 07:36:56 2017 +0000
+++ b/CommandProcessor/CommandProcessor.h	Tue Mar 07 08:06:39 2017 +0000
@@ -4,18 +4,34 @@
 #include "mbed.h"
 #include "PreferenceWriter.h"
 
-float *checkf(char *s);
-int *checkn(char *s);
+void processCmd(Serial *pc, PreferenceWriter *pref, char *buf);
 
-void processCmd(Serial *pc, PreferenceWriter *pref, char *buf);
+/*---variable loading, setting, and flashing---*/
 void cmd_ls(Serial *pc);
 void cmd_ls2(Serial *pc, char *buf);
+void cmd_set(Serial *pc, char *buf, char *val);
 void cmd_defaults(Serial *pc);
 void cmd_reload(Serial *pc, PreferenceWriter *pref);
 void cmd_flush(Serial *pc, PreferenceWriter *pref);
-void cmd_cfg(Serial *pc);
+
+/*---mode switching---*/
+void cmd_setp(Serial *pc, char *buf);
+void cmd_mode(Serial *pc, char *buf);
+void cmd_src(Serial *pc, char *buf);
+void cmd_op(Serial *pc, char *buf);
+
+/*---variable commands---*/
 void cmd_exit(Serial *pc);
 
+/*---internal functions---*/
 int tokenize(char *buf, char **out, int max);
+float *checkf(char *s);
+int *checkn(char *s);
+char *mode_to_str(int n);
+int str_to_mode(char *buf);
+char *src_to_str(int n);
+int str_to_src(char *buf);
+char *op_to_str(int n);
+int str_to_op(char *buf);
 
 #endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CommandProcessor/cmd_helpers.cpp	Tue Mar 07 08:06:39 2017 +0000
@@ -0,0 +1,154 @@
+#include "mbed.h"
+#include "CommandProcessor.h"
+#include "PreferenceWriter.h"
+#include "globals.h"
+
+#include "config.h"
+#include "config_driving.h"
+#include "config_inverter.h"
+#include "config_logging.h"
+#include "config_loop.h"
+#include "config_motor.h"
+#include "config_pins.h"
+#include "config_table.h"
+
+int tokenize(char *buf, char **out, int max) {
+    char* tok;
+    int k = 0;
+    
+    tok = strtok(buf, " ");
+    
+    while(tok != NULL && k < max) {
+        out[k] = tok;
+        k++;
+        tok = strtok(NULL, " ");
+    }
+    return k;
+}
+
+#define __check(x) if(strcmp(s, #x) == 0) return &_##x
+#define __check2(x) if (strcmp(s, #x) == 0) return &x
+
+float* checkf(char *s) {
+    __check(MAX_TQPCT_PLUS);
+    __check(MAX_TQPCT_MINUS);
+    __check(TORQUE_MAX);
+    __check(W_MAX);
+    __check(BUS_VOLTAGE);
+    __check(F_SW);
+    __check(K_LOOP_D);
+    __check(KI_BASE_D);
+    __check(K_LOOP_Q);
+    __check(KI_BASE_Q);
+    __check(F_SLOW_LOOP);
+    __check(INTEGRAL_MAX);
+    __check(W_FILTER_STRENGTH);
+    __check(DQ_FILTER_STRENGTH);
+    __check(THROTTLE_FILTER_STRENGTH);
+    __check(KP_D);
+    __check(KI_D);
+    __check(KP_Q);
+    __check(KI_Q);
+    __check(POLE_PAIRS);
+    __check(POS_OFFSET);
+    __check(RESOLVER_LOBES);
+    __check(Ld);
+    __check(Lq);
+    __check(FLUX_LINKAGE);
+    __check(Rs);
+    __check(KT);
+    __check(W_SAFE);
+    __check(W_CRAZY);
+    __check(W_STEP);
+    return NULL;
+}
+
+int* checkn(char *s) {
+    __check(TORQUE_MODE);
+    __check(CPR);
+    __check(TH_LIMIT_LOW);
+    __check(TH_LIMIT_HIGH);
+    __check(TH_LIMIT_CRAZY);
+    __check(ROWS);
+    __check(COLUMNS);
+    __check2(BREMS_mode);
+    __check2(BREMS_src);
+    __check2(BREMS_op);
+    return NULL;
+}
+
+#define __strcase(in, out) case in:strcpy(result, out);break
+#define __intcase(in, out) if (strcmp(buf, in) == 0) return out
+
+char* mode_to_str(int n) {
+    static char result[12];
+    switch (n) {
+        __strcase(MODE_RUN, "Run");
+        __strcase(MODE_CFG, "Config");
+        __strcase(MODE_ZERO, "Zero");
+        __strcase(MODE_CHR, "Wizard");
+    default:
+        strcpy(result, "Invalid");
+        break;
+    }
+    return result;
+}
+
+int str_to_mode(char *buf) {
+    __intcase("run", MODE_RUN);
+    __intcase("cfg", MODE_CFG);
+    __intcase("zero", MODE_ZERO);
+    __intcase("char", MODE_CHR);
+    __intcase("wizard", MODE_CHR);
+    return -1;
+}
+ 
+char* src_to_str(int n) {
+    static char result[12];
+    switch (n) {
+        __strcase(CMD_SRC_RC, "RC");
+        __strcase(CMD_SRC_ANALOG, "Analog");
+        __strcase(CMD_SRC_TERMINAL, "Terminal");
+        __strcase(CMD_SRC_SERIAL, "Serial");
+        __strcase(CMD_SRC_CAN, "CAN");
+        __strcase(CMD_SRC_INTERNAL, "Internal");
+    default:
+        strcpy(result, "Invalid");
+        break;
+    }
+    return result;
+}
+
+int str_to_src(char *buf) {
+    __intcase("rc", CMD_SRC_RC);
+    __intcase("analog", CMD_SRC_ANALOG);
+    __intcase("terminal", CMD_SRC_TERMINAL);
+    __intcase("serial", CMD_SRC_SERIAL);
+    __intcase("can", CMD_SRC_CAN);
+    __intcase("internal", CMD_SRC_INTERNAL);
+    return -1;
+}
+
+char* op_to_str(int n) {
+    static char result[24];
+    switch(n) {
+        __strcase(OP_TORQUE, "Torque loop");
+        __strcase(OP_DRIVING, "Driving map");
+        __strcase(OP_SPEED, "Speed loop");
+        __strcase(OP_POSITION, "Position loop");
+    default:
+        strcpy(result, "Invalid");
+        break;
+    }
+    return result;
+}
+
+int str_to_op(char *buf) {
+    __intcase("torque", OP_TORQUE);
+    __intcase("driving", OP_DRIVING);
+    __intcase("speed", OP_SPEED);
+    __intcase("pos", OP_POSITION);
+    __intcase("position", OP_POSITION);
+    return -1;
+}
+    
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CommandProcessor/cmd_mode.cpp	Tue Mar 07 08:06:39 2017 +0000
@@ -0,0 +1,52 @@
+#include "mbed.h"
+#include "CommandProcessor.h"
+#include "PreferenceWriter.h"
+#include "globals.h"
+
+#include "config.h"
+#include "config_driving.h"
+#include "config_inverter.h"
+#include "config_logging.h"
+#include "config_loop.h"
+#include "config_motor.h"
+#include "config_pins.h"
+#include "config_table.h"
+
+void cmd_setp(Serial *pc, char *buf) {
+    if (BREMS_src == CMD_SRC_TERMINAL) user_cmd = atof(buf);
+}
+
+void cmd_mode(Serial *pc, char *buf) {
+    int n = str_to_mode(buf);
+    if (n < 0) {
+        pc->printf("%s\n", "Invalid Mode");
+        return;
+    }
+    BREMS_mode = n;
+    pc->printf("Set mode to %s\n", mode_to_str(n));
+}
+
+void cmd_src(Serial *pc, char *buf) {
+    int n = str_to_src(buf);
+    if (n < 0) {
+        pc->printf("%s\n", "Invalid Source");
+        return;
+    }
+    BREMS_src = n;
+    pc->printf("Set source to %s\n", src_to_str(n));
+}
+
+void cmd_op(Serial *pc, char *buf) {
+    int n = str_to_op(buf);
+    if (n < 0) {
+        pc->printf("%s\n", "Invalid Source");
+        return;
+    }
+    BREMS_op = n;
+    pc->printf("Set operation to %s\n", op_to_str(n));
+}
+
+void cmd_exit(Serial *pc) {
+    BREMS_mode = MODE_RUN;
+    pc->printf("%s\n", "Run mode ON");
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CommandProcessor/cmd_set_get.cpp	Tue Mar 07 08:06:39 2017 +0000
@@ -0,0 +1,138 @@
+#include "mbed.h"
+#include "CommandProcessor.h"
+#include "PreferenceWriter.h"
+#include "globals.h"
+
+#include "config.h"
+#include "config_driving.h"
+#include "config_inverter.h"
+#include "config_logging.h"
+#include "config_loop.h"
+#include "config_motor.h"
+#include "config_pins.h"
+#include "config_table.h"
+
+void cmd_ls(Serial *pc) {
+    DPRINT(TORQUE_MODE);
+    FPRINT(MAX_TQPCT_PLUS);
+    FPRINT(MAX_TQPCT_MINUS);
+    FPRINT(TORQUE_MAX);
+    FPRINT(W_MAX);
+    FPRINT(BUS_VOLTAGE);
+    FPRINT(F_SW);
+    FPRINT(K_LOOP_D);
+    FPRINT(KI_BASE_D);
+    FPRINT(K_LOOP_Q);
+    FPRINT(KI_BASE_Q);
+    FPRINT(F_SLOW_LOOP);
+    FPRINT(INTEGRAL_MAX);
+    FPRINT(W_FILTER_STRENGTH);
+    FPRINT(DQ_FILTER_STRENGTH);
+    FPRINT(THROTTLE_FILTER_STRENGTH);
+    FPRINT(KP_D);
+    FPRINT(KI_D);
+    FPRINT(KP_Q);
+    FPRINT(KI_Q);
+    FPRINT(SLOW_LOOP_COUNTER);
+    FPRINT(POLE_PAIRS);
+    FPRINT(POS_OFFSET);
+    FPRINT(RESOLVER_LOBES);
+    DPRINT(CPR);
+    FPRINT(Ld);
+    FPRINT(Lq);
+    FPRINT(FLUX_LINKAGE);
+    FPRINT(Rs);
+    FPRINT(KT);
+    FPRINT(W_SAFE);
+    FPRINT(W_CRAZY);
+    DPRINT(TH_LIMIT_LOW);
+    DPRINT(TH_LIMIT_HIGH);
+    DPRINT(TH_LIMIT_CRAZY);
+    DPRINT(ROWS);
+    DPRINT(COLUMNS);
+    FPRINT(W_STEP);
+}
+
+void cmd_ls2(Serial *pc, char *buf) {
+    if (strcmp(buf, "mode") == 0) {
+        pc->printf("%s\n", mode_to_str(BREMS_mode));
+        return;
+    }
+    if (strcmp(buf, "src") == 0) {
+        pc->printf("%s\n", src_to_str(BREMS_src));
+        return;
+    }
+    if (strcmp(buf, "op") == 0) {
+        pc->printf("%s\n", op_to_str(BREMS_op));
+        return;
+    }
+    
+    float *fptr = checkf(buf);
+    if (fptr != NULL) pc->printf("%s: %f\n", buf, *fptr);
+    int *nptr = NULL;
+    if (fptr == NULL) nptr = checkn(buf);
+    if (nptr != NULL) pc->printf("%s: %d\n", buf, *nptr);
+    if (nptr == NULL && fptr == NULL) pc->printf("%s\n", "No Such Parameter");
+}
+
+void cmd_defaults(Serial *pc) {
+    DEFAULT(TORQUE_MODE);
+    DEFAULT(MAX_TQPCT_PLUS);
+    DEFAULT(MAX_TQPCT_MINUS);
+    DEFAULT(TORQUE_MAX);
+    DEFAULT(W_MAX);
+    DEFAULT(BUS_VOLTAGE);
+    DEFAULT(F_SW);
+    DEFAULT(K_LOOP_D);
+    DEFAULT(KI_BASE_D);
+    DEFAULT(K_LOOP_Q);
+    DEFAULT(KI_BASE_Q);
+    DEFAULT(F_SLOW_LOOP);
+    DEFAULT(INTEGRAL_MAX);
+    DEFAULT(W_FILTER_STRENGTH);
+    DEFAULT(DQ_FILTER_STRENGTH);
+    DEFAULT(THROTTLE_FILTER_STRENGTH);
+    DEFAULT(KP_D);
+    DEFAULT(KI_D);
+    DEFAULT(KP_Q);
+    DEFAULT(KI_Q);
+    DEFAULT(SLOW_LOOP_COUNTER);
+    DEFAULT(POLE_PAIRS);
+    DEFAULT(POS_OFFSET);
+    DEFAULT(RESOLVER_LOBES);
+    DEFAULT(CPR);
+    DEFAULT(Ld);
+    DEFAULT(Lq);
+    DEFAULT(FLUX_LINKAGE);
+    DEFAULT(Rs);
+    DEFAULT(KT);
+    DEFAULT(W_SAFE);
+    DEFAULT(W_CRAZY);
+    DEFAULT(TH_LIMIT_LOW);
+    DEFAULT(TH_LIMIT_HIGH);
+    DEFAULT(TH_LIMIT_CRAZY);
+    DEFAULT(ROWS);
+    DEFAULT(COLUMNS);
+    DEFAULT(W_STEP);
+    pc->printf("Defaults Loaded\n");
+}
+
+void cmd_reload(Serial *pc, PreferenceWriter *pref) {
+    pref->load();
+    pc->printf("Flash Values Loaded\n");
+}
+
+void cmd_set(Serial *pc, char *buf, char *val) {
+    float *fptr = checkf(buf);
+    if (fptr != NULL) *fptr = (float) (atof(val));
+    int *nptr = NULL;
+    if (fptr == NULL) nptr = checkn(buf);
+    if (nptr != NULL) *nptr = (int) (atoi(val));
+    cmd_ls2(pc, buf);
+    if (nptr == NULL && fptr == NULL) pc->printf("%s\n", "No Such Parameter");
+}
+
+void cmd_flush(Serial *pc, PreferenceWriter *pref) {
+    if (!pref->ready()) pref->open();
+    pref->flush();
+}
\ No newline at end of file

--- a/config.h	Thu Mar 02 07:36:56 2017 +0000
+++ b/config.h	Tue Mar 07 08:06:39 2017 +0000
@@ -64,6 +64,12 @@
 #define _COLUMNS         __int_reg[7]
 #define _W_STEP          __float_reg[8]
 
+/*---operating mode---*/
+
+#define BREMS_mode             __int_reg[8]
+#define BREMS_src              __int_reg[9]
+#define BREMS_op               __int_reg[10]
+
 /*internal variables and macros*/
 
 extern float __float_reg[];
@@ -73,7 +79,27 @@
 #define FPRINT(a) pc->printf("%s: %f\n", #a, _##a)
 #define DPRINT(a) pc->printf("%s: %d\n", #a, _##a)
 
-#define MODE_RUN 0
-#define MODE_CFG 1
+enum {
+    MODE_RUN,
+    MODE_CFG,
+    MODE_ZERO,
+    MODE_CHR,
+};
+
+enum {
+    OP_TORQUE,
+    OP_DRIVING,
+    OP_SPEED,
+    OP_POSITION,
+};
+
+enum {
+    CMD_SRC_RC,
+    CMD_SRC_ANALOG,
+    CMD_SRC_TERMINAL,
+    CMD_SRC_SERIAL,
+    CMD_SRC_CAN,
+    CMD_SRC_INTERNAL
+};
 
 #endif
\ No newline at end of file

--- a/globals.h	Thu Mar 02 07:36:56 2017 +0000
+++ b/globals.h	Tue Mar 07 08:06:39 2017 +0000
@@ -1,6 +1,6 @@
 #ifndef __GLOBALS_H
 #define __GLOBALS_H
 
-extern int mode;
+extern float p, torque_pct, user_cmd, vout;
 
 #endif
\ No newline at end of file

--- a/main.cpp	Thu Mar 02 07:36:56 2017 +0000
+++ b/main.cpp	Tue Mar 07 08:06:39 2017 +0000
@@ -1,4 +1,5 @@
 #include "mbed.h"
+#include "math.h"
 
 #include "PreferenceWriter.h"
 #include "FlashWriter.h"
@@ -7,10 +8,13 @@
 
 Serial pc(USBTX, USBRX);
 PreferenceWriter pref(6);
-DigitalOut led(LED1);
+PwmOut led(LED1);
 
-int index = 0, mode = MODE_RUN;
+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()) {
@@ -34,18 +38,74 @@
     }
 }
 
-void commutate() {
-    if (mode != MODE_RUN) {
-        led = 0;
-    } else {
-        led = 1;
+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;
     }
 }
 
-int main() {
+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", "Serial Configuration Test");
+    pc.printf("%s\n", "FOC'ed in the bot version A");
+    cmd_reload(&pc, &pref);
     pc.printf("%s", ">");
     
     Ticker tick;