STM3 Twin Brushless Motor Electronic Speed Controller

Dependencies:   BufferedSerial FastPWM Servo mbed

Committer:
JonFreeman
Date:
Sun Jun 17 06:59:37 2018 +0000
Revision:
7:6deaeace9a3e
Parent:
6:f289a49c1eae
Firmware for STM3 Twin Brushless Motor Electronic Speed Controller; Snapshot at 17th June 2018

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JonFreeman 6:f289a49c1eae 1 // DualBLS2018_03
JonFreeman 6:f289a49c1eae 2 #include "mbed.h"
JonFreeman 6:f289a49c1eae 3 #include "BufferedSerial.h"
JonFreeman 6:f289a49c1eae 4
JonFreeman 6:f289a49c1eae 5 #include <cctype>
JonFreeman 6:f289a49c1eae 6 #include "DualBLS.h"
JonFreeman 6:f289a49c1eae 7 using namespace std;
JonFreeman 6:f289a49c1eae 8
JonFreeman 6:f289a49c1eae 9 extern int I_Am () ; // Returns boards id number as ASCII char '0', '1' etc. Code for Broadcast = '\r'
JonFreeman 6:f289a49c1eae 10 extern int WatchDog;
JonFreeman 6:f289a49c1eae 11
JonFreeman 6:f289a49c1eae 12 const int BROADCAST = '\r';
JonFreeman 6:f289a49c1eae 13 const int MAX_PARAMS = 20;
JonFreeman 6:f289a49c1eae 14 struct parameters {
JonFreeman 7:6deaeace9a3e 15 struct kb_command const * command_list;
JonFreeman 7:6deaeace9a3e 16 BufferedSerial * com; // pc or com2
JonFreeman 7:6deaeace9a3e 17 char cmd_line[120];
JonFreeman 7:6deaeace9a3e 18 char * cmd_line_ptr;
JonFreeman 7:6deaeace9a3e 19 int32_t position_in_list, numof_dbls, target_unit, numof_menu_items, cl_index, gp_i;
JonFreeman 6:f289a49c1eae 20 double dbl[MAX_PARAMS];
JonFreeman 7:6deaeace9a3e 21 bool respond, resp_always;
JonFreeman 6:f289a49c1eae 22 } ;
JonFreeman 6:f289a49c1eae 23
JonFreeman 7:6deaeace9a3e 24 struct parameters pccom, lococom;
JonFreeman 6:f289a49c1eae 25 // WithOUT RTOS
JonFreeman 6:f289a49c1eae 26 extern BufferedSerial com2, pc;
JonFreeman 6:f289a49c1eae 27 extern void send_test () ;
JonFreeman 6:f289a49c1eae 28 extern void setVI (double v, double i) ;
JonFreeman 6:f289a49c1eae 29 extern void setV (double v) ;
JonFreeman 6:f289a49c1eae 30 extern void setI (double i) ;
JonFreeman 6:f289a49c1eae 31 extern void read_last_VI (double * val) ; // only for test from cli
JonFreeman 6:f289a49c1eae 32
JonFreeman 7:6deaeace9a3e 33 //BufferedSerial * com;
JonFreeman 6:f289a49c1eae 34
JonFreeman 6:f289a49c1eae 35 void null_cmd (struct parameters & a)
JonFreeman 6:f289a49c1eae 36 {
JonFreeman 6:f289a49c1eae 37 if (a.respond)
JonFreeman 7:6deaeace9a3e 38 a.com->printf ("At null_cmd, board ID %c, parameters : First %.3f, second %.3f\r\n", I_Am(), a.dbl[0], a.dbl[1]);
JonFreeman 6:f289a49c1eae 39 }
JonFreeman 6:f289a49c1eae 40
JonFreeman 6:f289a49c1eae 41 extern void mode_set (int mode, double val) ; // called from cli to set fw, re, rb, hb
JonFreeman 6:f289a49c1eae 42 extern void read_supply_vi (double * val) ;
JonFreeman 6:f289a49c1eae 43
JonFreeman 6:f289a49c1eae 44 void rdi_cmd (struct parameters & a) // read motor currents
JonFreeman 6:f289a49c1eae 45 {
JonFreeman 6:f289a49c1eae 46 if (a.respond) {
JonFreeman 6:f289a49c1eae 47 double r[4];
JonFreeman 6:f289a49c1eae 48 read_supply_vi (r); // get MotorA.I.ave, MotorB.I.ave, Battery volts
JonFreeman 7:6deaeace9a3e 49 a.com->printf ("rdi%.0f %.0f %.1f\r", r[0], r[1], r[2]); // Format good to be unpicked by cli in touch screen controller
JonFreeman 6:f289a49c1eae 50 }
JonFreeman 6:f289a49c1eae 51 }
JonFreeman 6:f289a49c1eae 52
JonFreeman 6:f289a49c1eae 53 void rvi_cmd (struct parameters & a) // read last normalised values sent to pwms
JonFreeman 6:f289a49c1eae 54 {
JonFreeman 6:f289a49c1eae 55 if (a.respond) {
JonFreeman 6:f289a49c1eae 56 double r[6];
JonFreeman 6:f289a49c1eae 57 read_last_VI (r);
JonFreeman 7:6deaeace9a3e 58 a.com->printf ("rvi%.2f %.2f %.2f %.2f\r", r[0], r[1], r[2], r[3]);
JonFreeman 6:f289a49c1eae 59 }
JonFreeman 6:f289a49c1eae 60 }
JonFreeman 6:f289a49c1eae 61
JonFreeman 6:f289a49c1eae 62 void fw_cmd (struct parameters & a) // Forward command
JonFreeman 6:f289a49c1eae 63 {
JonFreeman 6:f289a49c1eae 64 mode_set (FORWARD, 0.0);
JonFreeman 6:f289a49c1eae 65 }
JonFreeman 6:f289a49c1eae 66
JonFreeman 6:f289a49c1eae 67 void re_cmd (struct parameters & a) // Reverse command
JonFreeman 6:f289a49c1eae 68 {
JonFreeman 6:f289a49c1eae 69 mode_set (REVERSE, 0.0);
JonFreeman 6:f289a49c1eae 70 }
JonFreeman 6:f289a49c1eae 71
JonFreeman 6:f289a49c1eae 72 void rb_cmd (struct parameters & a) // Regen brake command
JonFreeman 6:f289a49c1eae 73 {
JonFreeman 6:f289a49c1eae 74 double b = a.dbl[0] / 100.0;
JonFreeman 7:6deaeace9a3e 75 // a.com->printf ("Applying brake %.3f\r\n", b);
JonFreeman 6:f289a49c1eae 76 mode_set (REGENBRAKE, b);
JonFreeman 6:f289a49c1eae 77 // apply_brake (b);
JonFreeman 6:f289a49c1eae 78 }
JonFreeman 6:f289a49c1eae 79
JonFreeman 6:f289a49c1eae 80 extern bool wr_24LC64 (int mem_start_addr, char * source, int length) ;
JonFreeman 6:f289a49c1eae 81 extern bool rd_24LC64 (int mem_start_addr, char * dest, int length) ;
JonFreeman 6:f289a49c1eae 82
JonFreeman 6:f289a49c1eae 83 void erase_cmd (struct parameters & a) // Sets eeprom contents to all 0xff. 256 pages of 32 bytes to do
JonFreeman 6:f289a49c1eae 84 {
JonFreeman 6:f289a49c1eae 85 char t[36];
JonFreeman 6:f289a49c1eae 86 for (int i = 0; i < 32; i++)
JonFreeman 6:f289a49c1eae 87 t[i] = 0xff;
JonFreeman 6:f289a49c1eae 88 for (int i = 0; i < 8191; i += 32) {
JonFreeman 7:6deaeace9a3e 89 a.com->printf (".");
JonFreeman 6:f289a49c1eae 90 if (!wr_24LC64 (i, t, 32))
JonFreeman 7:6deaeace9a3e 91 a.com->printf ("eeprom write prob\r\n");
JonFreeman 6:f289a49c1eae 92 }
JonFreeman 6:f289a49c1eae 93 }
JonFreeman 6:f289a49c1eae 94 /*struct motorpairoptions { // This to be user settable in eeprom, 32 bytes
JonFreeman 6:f289a49c1eae 95 uint8_t MotA_dir, // 0 or 1
JonFreeman 6:f289a49c1eae 96 MotB_dir, // 0 or 1
JonFreeman 6:f289a49c1eae 97 gang, // 0 for separate control (robot mode), 1 for ganged loco bogie mode
JonFreeman 6:f289a49c1eae 98 serv1, // 0, 1, 2 = Not used, Input, Output
JonFreeman 6:f289a49c1eae 99 serv2, // 0, 1, 2 = Not used, Input, Output
JonFreeman 6:f289a49c1eae 100 cmd_source, // 0 Invalid, 1 COM1, 2 COM2, 3 Pot, 4 Servo1, 5 Servo2
JonFreeman 6:f289a49c1eae 101 {'1', '9', '0', "Alternative ID ascii '1' to '9'"}, // defaults to '0' before eerom setup for first time
JonFreeman 6:f289a49c1eae 102 {50, 250, 98, "Wheel diameter mm"}, // New 01/06/2018
JonFreeman 6:f289a49c1eae 103 {10, 250, 27, "Motor pinion"}, // New 01/06/2018
JonFreeman 6:f289a49c1eae 104 {50, 250, 85, "Wheel gear"}, // New 01/06/2018
JonFreeman 6:f289a49c1eae 105 // last;
JonFreeman 6:f289a49c1eae 106 } ;
JonFreeman 6:f289a49c1eae 107 */
JonFreeman 6:f289a49c1eae 108 extern char mode_bytes[];
JonFreeman 6:f289a49c1eae 109
JonFreeman 6:f289a49c1eae 110 void mode_cmd (struct parameters & a) // With no params, reads eeprom contents. With params sets eeprom contents
JonFreeman 6:f289a49c1eae 111 {
JonFreeman 7:6deaeace9a3e 112 if (a.target_unit == BROADCAST || !a.resp_always) {
JonFreeman 7:6deaeace9a3e 113 // a.com->printf ("At mode_cmd, can not use BROADCAST with mode_cmd\r\n");
JonFreeman 6:f289a49c1eae 114 } else {
JonFreeman 6:f289a49c1eae 115 char t[36];
JonFreeman 7:6deaeace9a3e 116 a.com->printf ("At mode_cmd with node %d\r\n", a.target_unit);
JonFreeman 6:f289a49c1eae 117 rd_24LC64 (0, t, 32);
JonFreeman 7:6deaeace9a3e 118 a.com->printf ("Numof params=%d\r\n", a.numof_dbls);
JonFreeman 6:f289a49c1eae 119 for (int i = 0; i < numof_eeprom_options; i++)
JonFreeman 7:6deaeace9a3e 120 a.com->printf ("%2x\t%s\r\n", t[i], option_list[i].t);
JonFreeman 6:f289a49c1eae 121 if (a.numof_dbls == 0) { // Read present contents, do not write
JonFreeman 7:6deaeace9a3e 122 a.com->printf ("That's it\r\n");
JonFreeman 6:f289a49c1eae 123 } else { // Write new shit to eeprom
JonFreeman 7:6deaeace9a3e 124 a.com->printf ("\r\n");
JonFreeman 6:f289a49c1eae 125 if (a.numof_dbls != numof_eeprom_options) {
JonFreeman 7:6deaeace9a3e 126 a.com->printf ("params required = %d, you offered %d\r\n", numof_eeprom_options, a.numof_dbls);
JonFreeman 6:f289a49c1eae 127 } else { // Have been passed correct number of parameters
JonFreeman 6:f289a49c1eae 128 int b;
JonFreeman 7:6deaeace9a3e 129 a.com->printf("Ready to write params to eeprom\r\n");
JonFreeman 6:f289a49c1eae 130 for (int i = 0; i < numof_eeprom_options; i++) {
JonFreeman 6:f289a49c1eae 131 b = (int)a.dbl[i]; // parameter value to check against limits
JonFreeman 6:f289a49c1eae 132 if (i == 6) // Alternative ID must be turned to ascii
JonFreeman 6:f289a49c1eae 133 b |= '0';
JonFreeman 6:f289a49c1eae 134 if ((b < option_list[i].min) || (b > option_list[i].max)) { // if parameter out of range
JonFreeman 7:6deaeace9a3e 135 a.com->printf("Warning - Parameter = %d, out of range, setting to default %d\r\n", b, option_list[i].def);
JonFreeman 6:f289a49c1eae 136 b = option_list[i].def;
JonFreeman 6:f289a49c1eae 137 }
JonFreeman 7:6deaeace9a3e 138 a.com->printf ("0x%2x\t%s\r\n", (t[i] = b), option_list[i].t);
JonFreeman 6:f289a49c1eae 139 }
JonFreeman 6:f289a49c1eae 140 wr_24LC64 (0, t, numof_eeprom_options);
JonFreeman 6:f289a49c1eae 141 memcpy (mode_bytes,t,32);
JonFreeman 7:6deaeace9a3e 142 a.com->printf("Parameters set in eeprom\r\n");
JonFreeman 6:f289a49c1eae 143 }
JonFreeman 6:f289a49c1eae 144 }
JonFreeman 6:f289a49c1eae 145 }
JonFreeman 6:f289a49c1eae 146 }
JonFreeman 6:f289a49c1eae 147 /*void coast_cmd (struct parameters & a) { // Coast
JonFreeman 6:f289a49c1eae 148
JonFreeman 6:f289a49c1eae 149 }
JonFreeman 6:f289a49c1eae 150 */
JonFreeman 6:f289a49c1eae 151 void hb_cmd (struct parameters & a)
JonFreeman 6:f289a49c1eae 152 {
JonFreeman 6:f289a49c1eae 153 if (a.respond) {
JonFreeman 7:6deaeace9a3e 154 a.com->printf ("numof params = %d\r\n", a.numof_dbls);
JonFreeman 7:6deaeace9a3e 155 a.com->printf ("Hand Brake : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]);
JonFreeman 6:f289a49c1eae 156 }
JonFreeman 6:f289a49c1eae 157 mode_set (HANDBRAKE, 0.0);
JonFreeman 6:f289a49c1eae 158 }
JonFreeman 6:f289a49c1eae 159
JonFreeman 6:f289a49c1eae 160 extern uint32_t last_temp_count;
JonFreeman 6:f289a49c1eae 161 void temperature_cmd (struct parameters & a) {
JonFreeman 6:f289a49c1eae 162 if (a.respond) {
JonFreeman 7:6deaeace9a3e 163 a.com->printf ("tem%c %d\r\n", mode_bytes[ID], (last_temp_count / 16) - 50);
JonFreeman 6:f289a49c1eae 164 }
JonFreeman 6:f289a49c1eae 165 }
JonFreeman 6:f289a49c1eae 166
JonFreeman 6:f289a49c1eae 167 void bogie_constants_report_cmd (struct parameters & a) {
JonFreeman 6:f289a49c1eae 168 if (a.respond) {
JonFreeman 7:6deaeace9a3e 169 a.com->printf ("bc%c %d %d %d\r\n", mode_bytes[ID], mode_bytes[WHEELDIA], mode_bytes[MOTPIN], mode_bytes[WHEELGEAR]);
JonFreeman 6:f289a49c1eae 170 }
JonFreeman 6:f289a49c1eae 171 }
JonFreeman 6:f289a49c1eae 172
JonFreeman 6:f289a49c1eae 173 extern void read_RPM (uint32_t * dest) ;
JonFreeman 6:f289a49c1eae 174 void rpm_cmd (struct parameters & a) // to report e.g. RPM 1000 1000 ; speed for both motors
JonFreeman 6:f289a49c1eae 175 {
JonFreeman 6:f289a49c1eae 176 if (a.respond) {
JonFreeman 6:f289a49c1eae 177 uint32_t dest[3];
JonFreeman 7:6deaeace9a3e 178 read_RPM (dest); // gets rpm for each motor
JonFreeman 7:6deaeace9a3e 179 a.com->printf ("rpm%d %d\r", dest[0], dest[1]);
JonFreeman 6:f289a49c1eae 180 }
JonFreeman 6:f289a49c1eae 181 }
JonFreeman 6:f289a49c1eae 182
JonFreeman 7:6deaeace9a3e 183 extern double rpm2mph ;
JonFreeman 7:6deaeace9a3e 184 void mph_cmd (struct parameters & a) // to report miles per hour
JonFreeman 7:6deaeace9a3e 185 {
JonFreeman 7:6deaeace9a3e 186 uint32_t dest[3];
JonFreeman 7:6deaeace9a3e 187 read_RPM (dest); // gets rpm for each motor
JonFreeman 7:6deaeace9a3e 188 a.com->printf ("mph%c %.3f\r\n", mode_bytes[ID], (double)(dest[0] + dest[1]) * rpm2mph / 2.0);
JonFreeman 7:6deaeace9a3e 189 }
JonFreeman 7:6deaeace9a3e 190
JonFreeman 6:f289a49c1eae 191 void menucmd (struct parameters & a);
JonFreeman 6:f289a49c1eae 192
JonFreeman 6:f289a49c1eae 193 void vi_cmd (struct parameters & a)
JonFreeman 6:f289a49c1eae 194 {
JonFreeman 6:f289a49c1eae 195 // if (a.respond)
JonFreeman 6:f289a49c1eae 196 // com->printf ("In setVI, setting V to %.2f, I %.2f\r\n", a.dbl[0], a.dbl[1]);
JonFreeman 6:f289a49c1eae 197 setVI (a.dbl[0] / 100.0, a.dbl[1] / 100.0);
JonFreeman 6:f289a49c1eae 198 }
JonFreeman 6:f289a49c1eae 199
JonFreeman 6:f289a49c1eae 200 void v_cmd (struct parameters & a)
JonFreeman 6:f289a49c1eae 201 {
JonFreeman 6:f289a49c1eae 202 // if (a.respond)
JonFreeman 6:f289a49c1eae 203 // com->printf ("In setV, setting V to %.2f\r\n", a.dbl[0]);
JonFreeman 6:f289a49c1eae 204 setV (a.dbl[0] / 100.0);
JonFreeman 6:f289a49c1eae 205 }
JonFreeman 6:f289a49c1eae 206
JonFreeman 6:f289a49c1eae 207 void i_cmd (struct parameters & a)
JonFreeman 6:f289a49c1eae 208 {
JonFreeman 6:f289a49c1eae 209 // if (a.respond)
JonFreeman 7:6deaeace9a3e 210 // a.com->printf ("In setI, setting I to %.2f\r\n", a.dbl[0]);
JonFreeman 6:f289a49c1eae 211 setI (a.dbl[0] / 100.0);
JonFreeman 6:f289a49c1eae 212 }
JonFreeman 6:f289a49c1eae 213
JonFreeman 6:f289a49c1eae 214 void kd_cmd (struct parameters & a) // kick the watchdog
JonFreeman 6:f289a49c1eae 215 {
JonFreeman 6:f289a49c1eae 216 WatchDog = WATCHDOG_RELOAD + (I_Am() & 0x0f);
JonFreeman 7:6deaeace9a3e 217 // a.com->printf ("Poked %d up Dog\r\n", WatchDog);
JonFreeman 6:f289a49c1eae 218 }
JonFreeman 6:f289a49c1eae 219
JonFreeman 6:f289a49c1eae 220 void who_cmd (struct parameters & a)
JonFreeman 6:f289a49c1eae 221 {
JonFreeman 6:f289a49c1eae 222 int i = I_Am ();
JonFreeman 6:f289a49c1eae 223 if (I_Am() == a.target_unit)
JonFreeman 7:6deaeace9a3e 224 a.com->printf ("who%c\r\n", a.target_unit);
JonFreeman 6:f289a49c1eae 225 }
JonFreeman 6:f289a49c1eae 226
JonFreeman 6:f289a49c1eae 227 struct kb_command {
JonFreeman 6:f289a49c1eae 228 const char * cmd_word; // points to text e.g. "menu"
JonFreeman 6:f289a49c1eae 229 const char * explan;
JonFreeman 6:f289a49c1eae 230 void (*f)(struct parameters &); // points to function
JonFreeman 6:f289a49c1eae 231 } ;
JonFreeman 6:f289a49c1eae 232
JonFreeman 7:6deaeace9a3e 233 struct kb_command const loco_command_list[] = {
JonFreeman 6:f289a49c1eae 234 {"ls", "Lists available commands", menucmd},
JonFreeman 6:f289a49c1eae 235 {"?", "Lists available commands, same as ls", menucmd},
JonFreeman 6:f289a49c1eae 236 {"fw", "forward", fw_cmd},
JonFreeman 6:f289a49c1eae 237 {"re", "reverse", re_cmd},
JonFreeman 6:f289a49c1eae 238 {"rb", "regen brake 0 to 99 %", rb_cmd},
JonFreeman 6:f289a49c1eae 239 {"hb", "hand brake", hb_cmd},
JonFreeman 6:f289a49c1eae 240 {"v", "set motors V percent RANGE 0 to 100", v_cmd},
JonFreeman 6:f289a49c1eae 241 {"i", "set motors I percent RANGE 0 to 100", i_cmd},
JonFreeman 6:f289a49c1eae 242 {"vi", "set motors V and I percent RANGE 0 to 100", vi_cmd},
JonFreeman 6:f289a49c1eae 243 {"who", "search for connected units, e.g. 3who returs 'Hi there' if found", who_cmd},
JonFreeman 6:f289a49c1eae 244 {"mode", "read or set params in eeprom", mode_cmd},
JonFreeman 6:f289a49c1eae 245 {"erase", "set eeprom contents to all 0xff", erase_cmd},
JonFreeman 6:f289a49c1eae 246 {"tem", "report temperature", temperature_cmd},
JonFreeman 6:f289a49c1eae 247 {"kd", "kick the dog, reloads WatchDog", kd_cmd},
JonFreeman 6:f289a49c1eae 248 {"rpm", "read motor pair speeds", rpm_cmd},
JonFreeman 6:f289a49c1eae 249 {"rvi", "read most recent values sent to pwms", rvi_cmd},
JonFreeman 6:f289a49c1eae 250 {"rdi", "read motor currents and power voltage", rdi_cmd},
JonFreeman 6:f289a49c1eae 251 {"bc", "bogie constants - wheel dia, motor pinion, wheel gear", bogie_constants_report_cmd},
JonFreeman 6:f289a49c1eae 252 {"nu", "do nothing", null_cmd},
JonFreeman 6:f289a49c1eae 253 };
JonFreeman 6:f289a49c1eae 254
JonFreeman 7:6deaeace9a3e 255 //const int numof_loco_menu_items = sizeof(loco_command_list) / sizeof(kb_command);
JonFreeman 7:6deaeace9a3e 256
JonFreeman 7:6deaeace9a3e 257
JonFreeman 7:6deaeace9a3e 258 struct kb_command const pc_command_list[] = {
JonFreeman 7:6deaeace9a3e 259 {"ls", "Lists available commands", menucmd},
JonFreeman 7:6deaeace9a3e 260 {"?", "Lists available commands, same as ls", menucmd},
JonFreeman 7:6deaeace9a3e 261 {"fw", "forward", fw_cmd},
JonFreeman 7:6deaeace9a3e 262 {"re", "reverse", re_cmd},
JonFreeman 7:6deaeace9a3e 263 {"rb", "regen brake 0 to 99 %", rb_cmd},
JonFreeman 7:6deaeace9a3e 264 {"hb", "hand brake", hb_cmd},
JonFreeman 7:6deaeace9a3e 265 {"v", "set motors V percent RANGE 0 to 100", v_cmd},
JonFreeman 7:6deaeace9a3e 266 {"i", "set motors I percent RANGE 0 to 100", i_cmd},
JonFreeman 7:6deaeace9a3e 267 {"vi", "set motors V and I percent RANGE 0 to 100", vi_cmd},
JonFreeman 7:6deaeace9a3e 268 {"who", "search for connected units, e.g. 3who returs 'Hi there' if found", who_cmd},
JonFreeman 7:6deaeace9a3e 269 {"mode", "read or set params in eeprom", mode_cmd},
JonFreeman 7:6deaeace9a3e 270 {"erase", "set eeprom contents to all 0xff", erase_cmd},
JonFreeman 7:6deaeace9a3e 271 {"tem", "report temperature", temperature_cmd},
JonFreeman 7:6deaeace9a3e 272 {"kd", "kick the dog, reloads WatchDog", kd_cmd},
JonFreeman 7:6deaeace9a3e 273 {"rpm", "read motor pair speeds", rpm_cmd},
JonFreeman 7:6deaeace9a3e 274 {"mph", "read loco speed miles per hour", mph_cmd},
JonFreeman 7:6deaeace9a3e 275 {"rvi", "read most recent values sent to pwms", rvi_cmd},
JonFreeman 7:6deaeace9a3e 276 {"rdi", "read motor currents and power voltage", rdi_cmd},
JonFreeman 7:6deaeace9a3e 277 {"bc", "bogie constants - wheel dia, motor pinion, wheel gear", bogie_constants_report_cmd},
JonFreeman 7:6deaeace9a3e 278 {"nu", "do nothing", null_cmd},
JonFreeman 7:6deaeace9a3e 279 };
JonFreeman 7:6deaeace9a3e 280
JonFreeman 7:6deaeace9a3e 281 void setup_comms () {
JonFreeman 7:6deaeace9a3e 282 pccom.com = & pc;
JonFreeman 7:6deaeace9a3e 283 pccom.command_list = pc_command_list;
JonFreeman 7:6deaeace9a3e 284 pccom.numof_menu_items = sizeof(pc_command_list) / sizeof(kb_command);
JonFreeman 7:6deaeace9a3e 285 pccom.cl_index = 0;
JonFreeman 7:6deaeace9a3e 286 pccom.gp_i = 0; // general puropse integer, not used to 30/4/2018
JonFreeman 7:6deaeace9a3e 287 pccom.resp_always = true;
JonFreeman 7:6deaeace9a3e 288 lococom.com = & com2;
JonFreeman 7:6deaeace9a3e 289 lococom.command_list = loco_command_list;
JonFreeman 7:6deaeace9a3e 290 lococom.numof_menu_items = sizeof(loco_command_list) / sizeof(kb_command);
JonFreeman 7:6deaeace9a3e 291 lococom.cl_index = 0;
JonFreeman 7:6deaeace9a3e 292 lococom.gp_i = 0; // general puropse integer, toggles 0 / 1 to best guess source of rpm
JonFreeman 7:6deaeace9a3e 293 lococom.resp_always = false;
JonFreeman 7:6deaeace9a3e 294 }
JonFreeman 7:6deaeace9a3e 295
JonFreeman 7:6deaeace9a3e 296
JonFreeman 6:f289a49c1eae 297 void menucmd (struct parameters & a)
JonFreeman 6:f289a49c1eae 298 {
JonFreeman 6:f289a49c1eae 299 if (a.respond) {
JonFreeman 7:6deaeace9a3e 300 a.com->printf("\r\n\nDouble Brushless Motor Driver 2018\r\nAt menucmd function - listing commands:-\r\n");
JonFreeman 7:6deaeace9a3e 301 for(int i = 0; i < a.numof_menu_items; i++)
JonFreeman 7:6deaeace9a3e 302 a.com->printf("[%s]\t\t%s\r\n", a.command_list[i].cmd_word, a.command_list[i].explan);
JonFreeman 7:6deaeace9a3e 303 a.com->printf("End of List of Commands\r\n");
JonFreeman 6:f289a49c1eae 304 }
JonFreeman 6:f289a49c1eae 305 }
JonFreeman 6:f289a49c1eae 306
JonFreeman 6:f289a49c1eae 307 /*
JonFreeman 6:f289a49c1eae 308 New - March 2018
JonFreeman 6:f289a49c1eae 309 Using opto isolated serial port, paralleled up using same pair to multiple boards running this code.
JonFreeman 6:f289a49c1eae 310 New feature - commands have optional prefix digit 0-9 indicating which unit message is addressed to.
JonFreeman 6:f289a49c1eae 311 Commands without prefix digit - broadcast to all units, all to obey but none to respond.
JonFreeman 6:f289a49c1eae 312 Only units recognising its address from prefix digit may respond. This avoids bus contention.
JonFreeman 6:f289a49c1eae 313 But for BROADCAST commands, '0' may respond on behalf of the group
JonFreeman 6:f289a49c1eae 314 */
JonFreeman 6:f289a49c1eae 315 //void command_line_interpreter (void const *argument)
JonFreeman 7:6deaeace9a3e 316 void cli_core (struct parameters & a) {
JonFreeman 7:6deaeace9a3e 317 const int MAX_CMD_LEN = 120;
JonFreeman 7:6deaeace9a3e 318 int ch, IAm = I_Am();
JonFreeman 7:6deaeace9a3e 319 char * pEnd;//, * cmd_line_ptr;
JonFreeman 7:6deaeace9a3e 320 while (a.com->readable()) {
JonFreeman 7:6deaeace9a3e 321 ch = a.com->getc();
JonFreeman 7:6deaeace9a3e 322 if (a.cl_index > MAX_CMD_LEN) { // trap out stupidly long command lines
JonFreeman 7:6deaeace9a3e 323 a.com->printf ("Error!! Stupidly long cmd line\r\n");
JonFreeman 7:6deaeace9a3e 324 a.cl_index = 0;
JonFreeman 7:6deaeace9a3e 325 }
JonFreeman 7:6deaeace9a3e 326 if(ch != '\r') // was this the 'Enter' key?
JonFreeman 7:6deaeace9a3e 327 a.cmd_line[a.cl_index++] = ch; // added char to command being assembled
JonFreeman 7:6deaeace9a3e 328 else { // key was CR, may or may not be command to lookup
JonFreeman 7:6deaeace9a3e 329 a.target_unit = BROADCAST; // Set to BROADCAST default once found command line '\r'
JonFreeman 7:6deaeace9a3e 330 a.cmd_line_ptr = a.cmd_line;
JonFreeman 7:6deaeace9a3e 331 a.cmd_line[a.cl_index] = 0; // null terminate command string
JonFreeman 7:6deaeace9a3e 332 if(a.cl_index) { // If have got some chars to lookup
JonFreeman 7:6deaeace9a3e 333 int i, wrdlen;
JonFreeman 7:6deaeace9a3e 334 if (isdigit(a.cmd_line[0])) { // Look for command with prefix digit
JonFreeman 7:6deaeace9a3e 335 a.cmd_line_ptr++; // point past identified digit prefix
JonFreeman 7:6deaeace9a3e 336 a.target_unit = a.cmd_line[0]; // '0' to '9'
JonFreeman 7:6deaeace9a3e 337 //com->printf ("Got prefix %c\r\n", cmd_line[0]);
JonFreeman 7:6deaeace9a3e 338 }
JonFreeman 7:6deaeace9a3e 339 for (i = 0; i < a.numof_menu_items; i++) { // Look for input match in command list
JonFreeman 7:6deaeace9a3e 340 wrdlen = strlen(a.command_list[i].cmd_word);
JonFreeman 7:6deaeace9a3e 341 if(strncmp(a.command_list[i].cmd_word, a.cmd_line_ptr, wrdlen) == 0 && !isalpha(a.cmd_line_ptr[wrdlen])) { // If match found
JonFreeman 7:6deaeace9a3e 342 for (int k = 0; k < MAX_PARAMS; k++) {
JonFreeman 7:6deaeace9a3e 343 a.dbl[k] = 0.0;
JonFreeman 7:6deaeace9a3e 344 }
JonFreeman 7:6deaeace9a3e 345 a.position_in_list = i;
JonFreeman 7:6deaeace9a3e 346 a.numof_dbls = 0;
JonFreeman 7:6deaeace9a3e 347 pEnd = a.cmd_line_ptr + wrdlen;
JonFreeman 7:6deaeace9a3e 348 while (*pEnd) { // Assemble all numerics as doubles
JonFreeman 7:6deaeace9a3e 349 a.dbl[a.numof_dbls++] = strtod (pEnd, &pEnd);
JonFreeman 7:6deaeace9a3e 350 while (*pEnd && !isdigit(*pEnd) && '-' != *pEnd && '+' != *pEnd) {
JonFreeman 7:6deaeace9a3e 351 pEnd++;
JonFreeman 7:6deaeace9a3e 352 }
JonFreeman 7:6deaeace9a3e 353 }
JonFreeman 7:6deaeace9a3e 354 //com->printf ("\r\n"); // Not allowed as many may output this.
JonFreeman 7:6deaeace9a3e 355 //for (int k = 0; k < param_block.numof_dbls; k++)
JonFreeman 7:6deaeace9a3e 356 // com->printf ("Read %.3f\r\n", param_block.dbl[k]);
JonFreeman 7:6deaeace9a3e 357 // param_block.times[i] = clock();
JonFreeman 7:6deaeace9a3e 358 // a.respond = false;
JonFreeman 7:6deaeace9a3e 359 a.respond = a.resp_always;
JonFreeman 7:6deaeace9a3e 360 if (((a.target_unit == BROADCAST) && (IAm == '0')) || (IAm == a.target_unit))
JonFreeman 7:6deaeace9a3e 361 a.respond = true; // sorted 26/4/18
JonFreeman 7:6deaeace9a3e 362 // All boards to obey BROADCAST command, only specific board to obey number prefixed command
JonFreeman 7:6deaeace9a3e 363 if ((a.target_unit == BROADCAST) || (IAm == a.target_unit))
JonFreeman 7:6deaeace9a3e 364 a.command_list[i].f(a); // execute command if addressed to this unit
JonFreeman 7:6deaeace9a3e 365 i = a.numof_menu_items + 1; // to exit for loop
JonFreeman 7:6deaeace9a3e 366 } // end of match found
JonFreeman 7:6deaeace9a3e 367 } // End of for numof_menu_items
JonFreeman 7:6deaeace9a3e 368 if(i == a.numof_menu_items)
JonFreeman 7:6deaeace9a3e 369 a.com->printf("No Match Found for CMD [%s]\r\n", a.cmd_line);
JonFreeman 7:6deaeace9a3e 370 } // End of If have got some chars to lookup
JonFreeman 7:6deaeace9a3e 371 //com->printf("\r\n>");
JonFreeman 7:6deaeace9a3e 372 a.cl_index = 0;
JonFreeman 7:6deaeace9a3e 373 } // End of else key was CR, may or may not be command to lookup
JonFreeman 7:6deaeace9a3e 374 } // End of while (com->readable())
JonFreeman 7:6deaeace9a3e 375 }
JonFreeman 7:6deaeace9a3e 376
JonFreeman 7:6deaeace9a3e 377 void command_line_interpreter_pc () {
JonFreeman 7:6deaeace9a3e 378 cli_core (pccom);
JonFreeman 7:6deaeace9a3e 379 }
JonFreeman 7:6deaeace9a3e 380 void command_line_interpreter_loco () {
JonFreeman 7:6deaeace9a3e 381 cli_core (lococom);
JonFreeman 7:6deaeace9a3e 382 }
JonFreeman 7:6deaeace9a3e 383
JonFreeman 6:f289a49c1eae 384 void command_line_interpreter ()
JonFreeman 6:f289a49c1eae 385 {
JonFreeman 7:6deaeace9a3e 386 /* const int MAX_CMD_LEN = 120;
JonFreeman 6:f289a49c1eae 387 static char cmd_line[MAX_CMD_LEN + 4];
JonFreeman 6:f289a49c1eae 388 static int cl_index = 0;
JonFreeman 6:f289a49c1eae 389 int ch, IAm = I_Am();
JonFreeman 6:f289a49c1eae 390 char * pEnd, * cmd_line_ptr;
JonFreeman 6:f289a49c1eae 391 static struct parameters param_block ;
JonFreeman 6:f289a49c1eae 392 com = &com2;
JonFreeman 6:f289a49c1eae 393 while (com->readable()) {
JonFreeman 6:f289a49c1eae 394 // ch = tolower(com->getc());
JonFreeman 6:f289a49c1eae 395 ch = com->getc();
JonFreeman 6:f289a49c1eae 396 if (cl_index > MAX_CMD_LEN) { // trap out stupidly long command lines
JonFreeman 6:f289a49c1eae 397 com->printf ("Error!! Stupidly long cmd line\r\n");
JonFreeman 6:f289a49c1eae 398 cl_index = 0;
JonFreeman 6:f289a49c1eae 399 }
JonFreeman 6:f289a49c1eae 400 if(ch != '\r') // was this the 'Enter' key?
JonFreeman 6:f289a49c1eae 401 cmd_line[cl_index++] = ch; // added char to command being assembled
JonFreeman 6:f289a49c1eae 402 else { // key was CR, may or may not be command to lookup
JonFreeman 6:f289a49c1eae 403 param_block.target_unit = BROADCAST; // Set to BROADCAST default once found command line '\r'
JonFreeman 6:f289a49c1eae 404 cmd_line_ptr = cmd_line;
JonFreeman 6:f289a49c1eae 405 cmd_line[cl_index] = 0; // null terminate command string
JonFreeman 6:f289a49c1eae 406 if(cl_index) { // If have got some chars to lookup
JonFreeman 6:f289a49c1eae 407 int i, wrdlen;
JonFreeman 6:f289a49c1eae 408 if (isdigit(cmd_line[0])) { // Look for command with prefix digit
JonFreeman 6:f289a49c1eae 409 cmd_line_ptr++; // point past identified digit prefix
JonFreeman 6:f289a49c1eae 410 param_block.target_unit = cmd_line[0]; // '0' to '9'
JonFreeman 6:f289a49c1eae 411 //com->printf ("Got prefix %c\r\n", cmd_line[0]);
JonFreeman 6:f289a49c1eae 412 }
JonFreeman 7:6deaeace9a3e 413 for (i = 0; i < a.numof_menu_items; i++) { // Look for input match in command list
JonFreeman 7:6deaeace9a3e 414 wrdlen = strlen(a.command_list[i].cmd_word);
JonFreeman 7:6deaeace9a3e 415 if(strncmp(a.command_list[i].cmd_word, a.cmd_line_ptr, wrdlen) == 0 && !isalpha(a.cmd_line_ptr[wrdlen])) { // If match found
JonFreeman 6:f289a49c1eae 416 for (int k = 0; k < MAX_PARAMS; k++) {
JonFreeman 6:f289a49c1eae 417 param_block.dbl[k] = 0.0;
JonFreeman 6:f289a49c1eae 418 }
JonFreeman 6:f289a49c1eae 419 param_block.position_in_list = i;
JonFreeman 6:f289a49c1eae 420 // param_block.last_time = clock ();
JonFreeman 6:f289a49c1eae 421 param_block.numof_dbls = 0;
JonFreeman 6:f289a49c1eae 422 pEnd = cmd_line_ptr + wrdlen;
JonFreeman 6:f289a49c1eae 423 while (*pEnd) { // Assemble all numerics as doubles
JonFreeman 6:f289a49c1eae 424 param_block.dbl[param_block.numof_dbls++] = strtod (pEnd, &pEnd);
JonFreeman 6:f289a49c1eae 425 while (*pEnd && !isdigit(*pEnd) && '-' != *pEnd && '+' != *pEnd) {
JonFreeman 6:f289a49c1eae 426 pEnd++;
JonFreeman 6:f289a49c1eae 427 }
JonFreeman 6:f289a49c1eae 428 }
JonFreeman 6:f289a49c1eae 429 //com->printf ("\r\n"); // Not allowed as many may output this.
JonFreeman 6:f289a49c1eae 430 //for (int k = 0; k < param_block.numof_dbls; k++)
JonFreeman 6:f289a49c1eae 431 // com->printf ("Read %.3f\r\n", param_block.dbl[k]);
JonFreeman 6:f289a49c1eae 432 // param_block.times[i] = clock();
JonFreeman 6:f289a49c1eae 433 param_block.respond = false;
JonFreeman 6:f289a49c1eae 434 if (((param_block.target_unit == BROADCAST) && (IAm == '0')) || (IAm == param_block.target_unit))
JonFreeman 6:f289a49c1eae 435 param_block.respond = true; // sorted 26/4/18
JonFreeman 6:f289a49c1eae 436 // All boards to obey BROADCAST command, only specific board to obey number prefixed command
JonFreeman 6:f289a49c1eae 437 if ((param_block.target_unit == BROADCAST) || (IAm == param_block.target_unit))
JonFreeman 6:f289a49c1eae 438 command_list[i].f(param_block); // execute command if addressed to this unit
JonFreeman 6:f289a49c1eae 439 i = numof_menu_items + 1; // to exit for loop
JonFreeman 6:f289a49c1eae 440 } // end of match found
JonFreeman 6:f289a49c1eae 441 } // End of for numof_menu_items
JonFreeman 6:f289a49c1eae 442 if(i == numof_menu_items)
JonFreeman 6:f289a49c1eae 443 com->printf("No Match Found for CMD [%s]\r\n", cmd_line);
JonFreeman 6:f289a49c1eae 444 } // End of If have got some chars to lookup
JonFreeman 6:f289a49c1eae 445 //com->printf("\r\n>");
JonFreeman 6:f289a49c1eae 446 cl_index = 0;
JonFreeman 6:f289a49c1eae 447 } // End of else key was CR, may or may not be command to lookup
JonFreeman 6:f289a49c1eae 448 } // End of while (com->readable())
JonFreeman 6:f289a49c1eae 449 // Thread::wait(20); // Using RTOS on this project
JonFreeman 7:6deaeace9a3e 450 // }*/
JonFreeman 6:f289a49c1eae 451 }
JonFreeman 6:f289a49c1eae 452
JonFreeman 6:f289a49c1eae 453