Bayley Wang
/
foc-ed_in_the_bot_compact
robot
CommandProcessor/cmd_helpers.cpp@82:5e741c5ffd9f, 2017-03-10 (annotated)
- Committer:
- bwang
- Date:
- Fri Mar 10 08:29:13 2017 +0000
- Revision:
- 82:5e741c5ffd9f
- Child:
- 84:dd32640942a4
things somewhat work?
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 82:5e741c5ffd9f | 1 | #include "mbed.h" |
bwang | 82:5e741c5ffd9f | 2 | #include "CommandProcessor.h" |
bwang | 82:5e741c5ffd9f | 3 | #include "PreferenceWriter.h" |
bwang | 82:5e741c5ffd9f | 4 | #include "globals.h" |
bwang | 82:5e741c5ffd9f | 5 | |
bwang | 82:5e741c5ffd9f | 6 | #include "config.h" |
bwang | 82:5e741c5ffd9f | 7 | #include "config_driving.h" |
bwang | 82:5e741c5ffd9f | 8 | #include "config_inverter.h" |
bwang | 82:5e741c5ffd9f | 9 | #include "config_logging.h" |
bwang | 82:5e741c5ffd9f | 10 | #include "config_loop.h" |
bwang | 82:5e741c5ffd9f | 11 | #include "config_motor.h" |
bwang | 82:5e741c5ffd9f | 12 | #include "config_pins.h" |
bwang | 82:5e741c5ffd9f | 13 | #include "config_table.h" |
bwang | 82:5e741c5ffd9f | 14 | |
bwang | 82:5e741c5ffd9f | 15 | int tokenize(char *buf, char **out, int max) { |
bwang | 82:5e741c5ffd9f | 16 | char* tok; |
bwang | 82:5e741c5ffd9f | 17 | int k = 0; |
bwang | 82:5e741c5ffd9f | 18 | |
bwang | 82:5e741c5ffd9f | 19 | tok = strtok(buf, " "); |
bwang | 82:5e741c5ffd9f | 20 | |
bwang | 82:5e741c5ffd9f | 21 | while(tok != NULL && k < max) { |
bwang | 82:5e741c5ffd9f | 22 | out[k] = tok; |
bwang | 82:5e741c5ffd9f | 23 | k++; |
bwang | 82:5e741c5ffd9f | 24 | tok = strtok(NULL, " "); |
bwang | 82:5e741c5ffd9f | 25 | } |
bwang | 82:5e741c5ffd9f | 26 | return k; |
bwang | 82:5e741c5ffd9f | 27 | } |
bwang | 82:5e741c5ffd9f | 28 | |
bwang | 82:5e741c5ffd9f | 29 | #define __check(x) if(strcmp(s, #x) == 0) return &_##x |
bwang | 82:5e741c5ffd9f | 30 | #define __check2(x) if (strcmp(s, #x) == 0) return &x |
bwang | 82:5e741c5ffd9f | 31 | |
bwang | 82:5e741c5ffd9f | 32 | float* checkf(char *s) { |
bwang | 82:5e741c5ffd9f | 33 | __check(MAX_TQPCT_PLUS); |
bwang | 82:5e741c5ffd9f | 34 | __check(MAX_TQPCT_MINUS); |
bwang | 82:5e741c5ffd9f | 35 | __check(TORQUE_MAX); |
bwang | 82:5e741c5ffd9f | 36 | __check(W_MAX); |
bwang | 82:5e741c5ffd9f | 37 | __check(BUS_VOLTAGE); |
bwang | 82:5e741c5ffd9f | 38 | __check(F_SW); |
bwang | 82:5e741c5ffd9f | 39 | __check(K_LOOP_D); |
bwang | 82:5e741c5ffd9f | 40 | __check(KI_BASE_D); |
bwang | 82:5e741c5ffd9f | 41 | __check(K_LOOP_Q); |
bwang | 82:5e741c5ffd9f | 42 | __check(KI_BASE_Q); |
bwang | 82:5e741c5ffd9f | 43 | __check(F_SLOW_LOOP); |
bwang | 82:5e741c5ffd9f | 44 | __check(INTEGRAL_MAX); |
bwang | 82:5e741c5ffd9f | 45 | __check(W_FILTER_STRENGTH); |
bwang | 82:5e741c5ffd9f | 46 | __check(DQ_FILTER_STRENGTH); |
bwang | 82:5e741c5ffd9f | 47 | __check(THROTTLE_FILTER_STRENGTH); |
bwang | 82:5e741c5ffd9f | 48 | __check(KP_D); |
bwang | 82:5e741c5ffd9f | 49 | __check(KI_D); |
bwang | 82:5e741c5ffd9f | 50 | __check(KP_Q); |
bwang | 82:5e741c5ffd9f | 51 | __check(KI_Q); |
bwang | 82:5e741c5ffd9f | 52 | __check(POLE_PAIRS); |
bwang | 82:5e741c5ffd9f | 53 | __check(POS_OFFSET); |
bwang | 82:5e741c5ffd9f | 54 | __check(RESOLVER_LOBES); |
bwang | 82:5e741c5ffd9f | 55 | __check(Ld); |
bwang | 82:5e741c5ffd9f | 56 | __check(Lq); |
bwang | 82:5e741c5ffd9f | 57 | __check(FLUX_LINKAGE); |
bwang | 82:5e741c5ffd9f | 58 | __check(Rs); |
bwang | 82:5e741c5ffd9f | 59 | __check(KT); |
bwang | 82:5e741c5ffd9f | 60 | __check(W_SAFE); |
bwang | 82:5e741c5ffd9f | 61 | __check(W_CRAZY); |
bwang | 82:5e741c5ffd9f | 62 | __check(W_STEP); |
bwang | 82:5e741c5ffd9f | 63 | return NULL; |
bwang | 82:5e741c5ffd9f | 64 | } |
bwang | 82:5e741c5ffd9f | 65 | |
bwang | 82:5e741c5ffd9f | 66 | int* checkn(char *s) { |
bwang | 82:5e741c5ffd9f | 67 | __check(TORQUE_MODE); |
bwang | 82:5e741c5ffd9f | 68 | __check(CPR); |
bwang | 82:5e741c5ffd9f | 69 | __check(TH_LIMIT_LOW); |
bwang | 82:5e741c5ffd9f | 70 | __check(TH_LIMIT_HIGH); |
bwang | 82:5e741c5ffd9f | 71 | __check(TH_LIMIT_CRAZY); |
bwang | 82:5e741c5ffd9f | 72 | __check(ROWS); |
bwang | 82:5e741c5ffd9f | 73 | __check(COLUMNS); |
bwang | 82:5e741c5ffd9f | 74 | __check2(BREMS_mode); |
bwang | 82:5e741c5ffd9f | 75 | __check2(BREMS_src); |
bwang | 82:5e741c5ffd9f | 76 | __check2(BREMS_op); |
bwang | 82:5e741c5ffd9f | 77 | return NULL; |
bwang | 82:5e741c5ffd9f | 78 | } |
bwang | 82:5e741c5ffd9f | 79 | |
bwang | 82:5e741c5ffd9f | 80 | #define __strcase(in, out) case in:strcpy(result, out);break |
bwang | 82:5e741c5ffd9f | 81 | #define __intcase(in, out) if (strcmp(buf, in) == 0) return out |
bwang | 82:5e741c5ffd9f | 82 | |
bwang | 82:5e741c5ffd9f | 83 | char* mode_to_str(int n) { |
bwang | 82:5e741c5ffd9f | 84 | static char result[12]; |
bwang | 82:5e741c5ffd9f | 85 | switch (n) { |
bwang | 82:5e741c5ffd9f | 86 | __strcase(MODE_RUN, "Run"); |
bwang | 82:5e741c5ffd9f | 87 | __strcase(MODE_CFG, "Config"); |
bwang | 82:5e741c5ffd9f | 88 | __strcase(MODE_ZERO, "Zero"); |
bwang | 82:5e741c5ffd9f | 89 | __strcase(MODE_CHR, "Wizard"); |
bwang | 82:5e741c5ffd9f | 90 | default: |
bwang | 82:5e741c5ffd9f | 91 | strcpy(result, "Invalid"); |
bwang | 82:5e741c5ffd9f | 92 | break; |
bwang | 82:5e741c5ffd9f | 93 | } |
bwang | 82:5e741c5ffd9f | 94 | return result; |
bwang | 82:5e741c5ffd9f | 95 | } |
bwang | 82:5e741c5ffd9f | 96 | |
bwang | 82:5e741c5ffd9f | 97 | int str_to_mode(char *buf) { |
bwang | 82:5e741c5ffd9f | 98 | __intcase("run", MODE_RUN); |
bwang | 82:5e741c5ffd9f | 99 | __intcase("cfg", MODE_CFG); |
bwang | 82:5e741c5ffd9f | 100 | __intcase("zero", MODE_ZERO); |
bwang | 82:5e741c5ffd9f | 101 | __intcase("char", MODE_CHR); |
bwang | 82:5e741c5ffd9f | 102 | __intcase("wizard", MODE_CHR); |
bwang | 82:5e741c5ffd9f | 103 | return -1; |
bwang | 82:5e741c5ffd9f | 104 | } |
bwang | 82:5e741c5ffd9f | 105 | |
bwang | 82:5e741c5ffd9f | 106 | char* src_to_str(int n) { |
bwang | 82:5e741c5ffd9f | 107 | static char result[12]; |
bwang | 82:5e741c5ffd9f | 108 | switch (n) { |
bwang | 82:5e741c5ffd9f | 109 | __strcase(CMD_SRC_RC, "RC"); |
bwang | 82:5e741c5ffd9f | 110 | __strcase(CMD_SRC_ANALOG, "Analog"); |
bwang | 82:5e741c5ffd9f | 111 | __strcase(CMD_SRC_TERMINAL, "Terminal"); |
bwang | 82:5e741c5ffd9f | 112 | __strcase(CMD_SRC_SERIAL, "Serial"); |
bwang | 82:5e741c5ffd9f | 113 | __strcase(CMD_SRC_CAN, "CAN"); |
bwang | 82:5e741c5ffd9f | 114 | __strcase(CMD_SRC_INTERNAL, "Internal"); |
bwang | 82:5e741c5ffd9f | 115 | default: |
bwang | 82:5e741c5ffd9f | 116 | strcpy(result, "Invalid"); |
bwang | 82:5e741c5ffd9f | 117 | break; |
bwang | 82:5e741c5ffd9f | 118 | } |
bwang | 82:5e741c5ffd9f | 119 | return result; |
bwang | 82:5e741c5ffd9f | 120 | } |
bwang | 82:5e741c5ffd9f | 121 | |
bwang | 82:5e741c5ffd9f | 122 | int str_to_src(char *buf) { |
bwang | 82:5e741c5ffd9f | 123 | __intcase("rc", CMD_SRC_RC); |
bwang | 82:5e741c5ffd9f | 124 | __intcase("analog", CMD_SRC_ANALOG); |
bwang | 82:5e741c5ffd9f | 125 | __intcase("terminal", CMD_SRC_TERMINAL); |
bwang | 82:5e741c5ffd9f | 126 | __intcase("serial", CMD_SRC_SERIAL); |
bwang | 82:5e741c5ffd9f | 127 | __intcase("can", CMD_SRC_CAN); |
bwang | 82:5e741c5ffd9f | 128 | __intcase("internal", CMD_SRC_INTERNAL); |
bwang | 82:5e741c5ffd9f | 129 | return -1; |
bwang | 82:5e741c5ffd9f | 130 | } |
bwang | 82:5e741c5ffd9f | 131 | |
bwang | 82:5e741c5ffd9f | 132 | char* op_to_str(int n) { |
bwang | 82:5e741c5ffd9f | 133 | static char result[24]; |
bwang | 82:5e741c5ffd9f | 134 | switch(n) { |
bwang | 82:5e741c5ffd9f | 135 | __strcase(OP_TORQUE, "Torque loop"); |
bwang | 82:5e741c5ffd9f | 136 | __strcase(OP_DRIVING, "Driving map"); |
bwang | 82:5e741c5ffd9f | 137 | __strcase(OP_SPEED, "Speed loop"); |
bwang | 82:5e741c5ffd9f | 138 | __strcase(OP_POSITION, "Position loop"); |
bwang | 82:5e741c5ffd9f | 139 | default: |
bwang | 82:5e741c5ffd9f | 140 | strcpy(result, "Invalid"); |
bwang | 82:5e741c5ffd9f | 141 | break; |
bwang | 82:5e741c5ffd9f | 142 | } |
bwang | 82:5e741c5ffd9f | 143 | return result; |
bwang | 82:5e741c5ffd9f | 144 | } |
bwang | 82:5e741c5ffd9f | 145 | |
bwang | 82:5e741c5ffd9f | 146 | int str_to_op(char *buf) { |
bwang | 82:5e741c5ffd9f | 147 | __intcase("torque", OP_TORQUE); |
bwang | 82:5e741c5ffd9f | 148 | __intcase("driving", OP_DRIVING); |
bwang | 82:5e741c5ffd9f | 149 | __intcase("speed", OP_SPEED); |
bwang | 82:5e741c5ffd9f | 150 | __intcase("pos", OP_POSITION); |
bwang | 82:5e741c5ffd9f | 151 | __intcase("position", OP_POSITION); |
bwang | 82:5e741c5ffd9f | 152 | return -1; |
bwang | 82:5e741c5ffd9f | 153 | } |
bwang | 82:5e741c5ffd9f | 154 |