Diff: CommandProcessor/CommandProcessor.cpp

Revision:

1:df6bbacb7bb9

Child:

2:cfc39b0843ae

diff -r 941e150e78b6 -r df6bbacb7bb9 CommandProcessor/CommandProcessor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CommandProcessor/CommandProcessor.cpp	Thu Mar 02 07:04:47 2017 +0000
@@ -0,0 +1,207 @@
+#include "mbed.h"
+#include "CommandProcessor.h"
+#include "PreferenceWriter.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) {
+    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 processCmd(Serial *pc, PreferenceWriter *pref, char *buf) {
+    char *tokens[10];
+    int len = tokenize(buf, tokens, 10);
+    
+    switch (len) {
+    case 1:
+        if (strcmp(tokens[0], "ls") == 0) cmd_ls(pc);
+        if (strcmp(tokens[0], "defaults") == 0) cmd_defaults(pc);
+        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);
+        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]);
+        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