Jon Freeman / Mbed 2 deprecated Brushless_STM3_ESC_2019_10

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed BufferedSerial Servo PCT2075 FastPWM

cli_nortos.cpp@3:ecb00e0e8d68, 2018-03-18 (annotated)

Committer:
JonFreeman
Date:
Sun Mar 18 08:17:56 2018 +0000
Revision:
3:ecb00e0e8d68
Parent:
2:04761b196473
Child:
4:21d91465e4b1
Starting motors requires high-side mosfet drivers being enabled. Auto tickleup functions now included to switch high sides off and on again to charge high side supply capacitors (now 2u2, up from 100n)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JonFreeman 0:435bf84ce48a 1 #include "mbed.h"
JonFreeman 0:435bf84ce48a 2 #include "BufferedSerial.h"
JonFreeman 0:435bf84ce48a 3 #include <cctype>
JonFreeman 2:04761b196473 4 #include "DualBLS.h"
JonFreeman 0:435bf84ce48a 5 using namespace std;
JonFreeman 0:435bf84ce48a 6
JonFreeman 3:ecb00e0e8d68 7 extern int I_Am () ; // Returns boards id number as ASCII char '0', '1' etc. Code for Broadcast = '\r'
JonFreeman 3:ecb00e0e8d68 8 //typedef double fl_typ; //
JonFreeman 0:435bf84ce48a 9
JonFreeman 3:ecb00e0e8d68 10 const int BROADCAST = '\r';
JonFreeman 3:ecb00e0e8d68 11 const int MAX_PARAMS = 20;
JonFreeman 0:435bf84ce48a 12 struct parameters {
JonFreeman 0:435bf84ce48a 13 // int32_t times[numof_menu_items];
JonFreeman 3:ecb00e0e8d68 14 // int32_t times[50];
JonFreeman 3:ecb00e0e8d68 15 int32_t position_in_list, last_time, numof_dbls, target_unit;
JonFreeman 0:435bf84ce48a 16 double dbl[MAX_PARAMS];
JonFreeman 3:ecb00e0e8d68 17 bool respond;
JonFreeman 0:435bf84ce48a 18 } ;
JonFreeman 0:435bf84ce48a 19
JonFreeman 0:435bf84ce48a 20 // WithOUT RTOS
JonFreeman 3:ecb00e0e8d68 21 extern BufferedSerial com2, pc;
JonFreeman 0:435bf84ce48a 22 //extern void set_I_limit (double p) ; // Sets max motor current
JonFreeman 0:435bf84ce48a 23 //extern void set_V_limit (double p) ; // Sets max motor voltage
JonFreeman 0:435bf84ce48a 24 extern void send_test () ;
JonFreeman 2:04761b196473 25 extern void setVI (double v, double i) ;
JonFreeman 2:04761b196473 26
JonFreeman 3:ecb00e0e8d68 27 BufferedSerial * com;
JonFreeman 0:435bf84ce48a 28
JonFreeman 0:435bf84ce48a 29 void null_cmd (struct parameters & a) {
JonFreeman 3:ecb00e0e8d68 30 com->printf ("At null_cmd, parameters : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]);
JonFreeman 0:435bf84ce48a 31 }
JonFreeman 0:435bf84ce48a 32
JonFreeman 3:ecb00e0e8d68 33 /*extern void tickleboth () ;
JonFreeman 2:04761b196473 34 void ti_cmd (struct parameters & a) {
JonFreeman 3:ecb00e0e8d68 35 com->printf ("At tickle\r\n");
JonFreeman 3:ecb00e0e8d68 36 tickleboth ();
JonFreeman 3:ecb00e0e8d68 37 }*/
JonFreeman 2:04761b196473 38
JonFreeman 2:04761b196473 39 extern void mode_test (int mode, double val) ;
JonFreeman 0:435bf84ce48a 40
JonFreeman 0:435bf84ce48a 41 void fw_cmd (struct parameters & a) {
JonFreeman 3:ecb00e0e8d68 42 com->printf ("Forward : First %d, second %d\r\n", a.dbl[0], a.dbl[1]);
JonFreeman 2:04761b196473 43 mode_test (FORWARD, 0.0);
JonFreeman 0:435bf84ce48a 44 }
JonFreeman 0:435bf84ce48a 45
JonFreeman 0:435bf84ce48a 46 void re_cmd (struct parameters & a) {
JonFreeman 3:ecb00e0e8d68 47 com->printf ("Reverse : First %d, second %d\r\n", a.dbl[0], a.dbl[1]);
JonFreeman 2:04761b196473 48 mode_test (REVERSE, 0.0);
JonFreeman 0:435bf84ce48a 49 }
JonFreeman 0:435bf84ce48a 50
JonFreeman 0:435bf84ce48a 51 void rb_cmd (struct parameters & a) { // Regen brake
JonFreeman 2:04761b196473 52 double b = a.dbl[0] / 99.0;
JonFreeman 3:ecb00e0e8d68 53 com->printf ("Applying brake %.3f\r\n", b);
JonFreeman 2:04761b196473 54 mode_test (REGENBRAKE, b);
JonFreeman 0:435bf84ce48a 55 // apply_brake (b);
JonFreeman 0:435bf84ce48a 56 }
JonFreeman 0:435bf84ce48a 57
JonFreeman 3:ecb00e0e8d68 58 extern bool wr_24LC64 (int mem_start_addr, char * source, int length) ;
JonFreeman 3:ecb00e0e8d68 59 extern bool rd_24LC64 (int mem_start_addr, char * dest, int length) ;
JonFreeman 0:435bf84ce48a 60
JonFreeman 3:ecb00e0e8d68 61 void erase_cmd (struct parameters & a) { // Sets eeprom contents to all 0xff. 256 pages of 32 bytes to do
JonFreeman 3:ecb00e0e8d68 62 char t[36];
JonFreeman 3:ecb00e0e8d68 63 for (int i = 0; i < 32; i++)
JonFreeman 3:ecb00e0e8d68 64 t[i] = 0xff;
JonFreeman 3:ecb00e0e8d68 65 for (int i = 0; i < 8191; i += 32) {
JonFreeman 3:ecb00e0e8d68 66 com->printf (".");
JonFreeman 3:ecb00e0e8d68 67 if (!wr_24LC64 (i, t, 32))
JonFreeman 3:ecb00e0e8d68 68 com->printf ("eeprom write prob\r\n");
JonFreeman 3:ecb00e0e8d68 69 }
JonFreeman 0:435bf84ce48a 70 }
JonFreeman 3:ecb00e0e8d68 71 /*struct motorpairoptions { // This to be user settable in eeprom, 32 bytes
JonFreeman 3:ecb00e0e8d68 72 uint8_t MotA_dir, // 0 or 1
JonFreeman 3:ecb00e0e8d68 73 MotB_dir, // 0 or 1
JonFreeman 3:ecb00e0e8d68 74 gang, // 0 for separate control (robot mode), 1 for ganged loco bogie mode
JonFreeman 3:ecb00e0e8d68 75 serv1, // 0, 1, 2 = Not used, Input, Output
JonFreeman 3:ecb00e0e8d68 76 serv2, // 0, 1, 2 = Not used, Input, Output
JonFreeman 3:ecb00e0e8d68 77 cmd_source, // 0 Invalid, 1 COM1, 2 COM2, 3 Pot, 4 Servo1, 5 Servo2
JonFreeman 3:ecb00e0e8d68 78 last;
JonFreeman 3:ecb00e0e8d68 79 } ;
JonFreeman 3:ecb00e0e8d68 80 */
JonFreeman 3:ecb00e0e8d68 81 struct optpar {
JonFreeman 3:ecb00e0e8d68 82 int min, max, def;
JonFreeman 3:ecb00e0e8d68 83 const char * t;
JonFreeman 3:ecb00e0e8d68 84 } ;
JonFreeman 3:ecb00e0e8d68 85 struct optpar const option_list[] = {
JonFreeman 3:ecb00e0e8d68 86 {0, 1, 1, "MotorA direction 0 or 1"},
JonFreeman 3:ecb00e0e8d68 87 {0, 1, 0, "MotorB direction 0 or 1"},
JonFreeman 3:ecb00e0e8d68 88 {0, 1, 1, "gang 0 for separate control (robot mode), 1 for ganged loco bogie mode"},
JonFreeman 3:ecb00e0e8d68 89 {0, 2, 2, "Servo1 0, 1, 2 = Not used, Input, Output"},
JonFreeman 3:ecb00e0e8d68 90 {0, 2, 2, "Servo2 0, 1, 2 = Not used, Input, Output"},
JonFreeman 3:ecb00e0e8d68 91 {1, 5, 2, "Command source 0 Invalid, 1 COM1, 2 COM2, 3 Pot, 4 Servo1, 5 Servo2"},
JonFreeman 3:ecb00e0e8d68 92 {0, 9, 0, "Alternative ID 0 to 9"},
JonFreeman 3:ecb00e0e8d68 93 } ;
JonFreeman 3:ecb00e0e8d68 94
JonFreeman 3:ecb00e0e8d68 95 void mode_cmd (struct parameters & a) { // With no params, reads eeprom contents. With params sets eeprom contents
JonFreeman 3:ecb00e0e8d68 96 char t[36];
JonFreeman 3:ecb00e0e8d68 97 const int numofopts = sizeof(option_list) / sizeof(struct optpar);
JonFreeman 3:ecb00e0e8d68 98 rd_24LC64 (0, t, 32);
JonFreeman 3:ecb00e0e8d68 99 com->printf ("Numof params=%d\r\n", a.numof_dbls);
JonFreeman 3:ecb00e0e8d68 100 for (int i = 0; i < numofopts; i++)
JonFreeman 3:ecb00e0e8d68 101 com->printf ("%2x\t%s\r\n", t[i], option_list[i].t);
JonFreeman 3:ecb00e0e8d68 102 if (a.numof_dbls == 0) { // Read present contents, do not write
JonFreeman 3:ecb00e0e8d68 103 com->printf ("That's it\r\n");
JonFreeman 3:ecb00e0e8d68 104 }
JonFreeman 3:ecb00e0e8d68 105 else { // Write new shit to eeprom
JonFreeman 3:ecb00e0e8d68 106 com->printf ("\r\n");
JonFreeman 3:ecb00e0e8d68 107 if (a.numof_dbls != numofopts) {
JonFreeman 3:ecb00e0e8d68 108 com->printf ("params required = %d, you offered %d\r\n", numofopts, a.numof_dbls);
JonFreeman 3:ecb00e0e8d68 109 }
JonFreeman 3:ecb00e0e8d68 110 else { // Have been passed correct number of parameters
JonFreeman 3:ecb00e0e8d68 111 int b;
JonFreeman 3:ecb00e0e8d68 112 com->printf("Ready to write params to eeprom\r\n");
JonFreeman 3:ecb00e0e8d68 113 for (int i = 0; i < numofopts; i++) {
JonFreeman 3:ecb00e0e8d68 114 b = (int)a.dbl[i]; // parameter value to check against limits
JonFreeman 3:ecb00e0e8d68 115 if ((b < option_list[i].min) || (b > option_list[i].max)) { // if parameter out of range
JonFreeman 3:ecb00e0e8d68 116 com->printf("Warning - Parameter = %d, out of range, setting to default %d\r\n", b, option_list[i].def);
JonFreeman 3:ecb00e0e8d68 117 b = option_list[i].def;
JonFreeman 3:ecb00e0e8d68 118 }
JonFreeman 3:ecb00e0e8d68 119 com->printf ("%2x\t%s\r\n", (t[i] = b), option_list[i].t);
JonFreeman 3:ecb00e0e8d68 120 }
JonFreeman 3:ecb00e0e8d68 121 wr_24LC64 (0, t, numofopts);
JonFreeman 3:ecb00e0e8d68 122 com->printf("Parameters set in eeprom\r\n");
JonFreeman 3:ecb00e0e8d68 123 }
JonFreeman 3:ecb00e0e8d68 124 }
JonFreeman 3:ecb00e0e8d68 125 }
JonFreeman 3:ecb00e0e8d68 126 /*void coast_cmd (struct parameters & a) { // Coast
JonFreeman 0:435bf84ce48a 127
JonFreeman 0:435bf84ce48a 128 }
JonFreeman 2:04761b196473 129 */
JonFreeman 0:435bf84ce48a 130 void hb_cmd (struct parameters & a) {
JonFreeman 3:ecb00e0e8d68 131 com->printf ("numof params = %d\r\n", a.numof_dbls);
JonFreeman 3:ecb00e0e8d68 132 com->printf ("Hand Brake : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]);
JonFreeman 2:04761b196473 133 mode_test (HANDBRAKE, 0.0);
JonFreeman 0:435bf84ce48a 134 }
JonFreeman 0:435bf84ce48a 135
JonFreeman 0:435bf84ce48a 136 void menucmd (struct parameters & a);
JonFreeman 0:435bf84ce48a 137
JonFreeman 2:04761b196473 138 void vi_cmd (struct parameters & a)
JonFreeman 0:435bf84ce48a 139 {
JonFreeman 3:ecb00e0e8d68 140 com->printf ("In setVI, setting V to %.2f, I %.2f\r\n", a.dbl[0], a.dbl[1]);
JonFreeman 2:04761b196473 141 setVI (a.dbl[0] / 100.0, a.dbl[1] / 100.0);
JonFreeman 0:435bf84ce48a 142 }
JonFreeman 0:435bf84ce48a 143
JonFreeman 3:ecb00e0e8d68 144 void kd_cmd (struct parameters & a) // kick the watchdog
JonFreeman 0:435bf84ce48a 145 {
JonFreeman 3:ecb00e0e8d68 146 }
JonFreeman 3:ecb00e0e8d68 147
JonFreeman 3:ecb00e0e8d68 148 void who_cmd (struct parameters & a)
JonFreeman 3:ecb00e0e8d68 149 {
JonFreeman 3:ecb00e0e8d68 150 int i = I_Am ();
JonFreeman 3:ecb00e0e8d68 151 if (I_Am() == a.target_unit)
JonFreeman 3:ecb00e0e8d68 152 com->printf ("Hi there, I am %c\r\n", a.target_unit);
JonFreeman 3:ecb00e0e8d68 153 }
JonFreeman 0:435bf84ce48a 154
JonFreeman 0:435bf84ce48a 155 struct kb_command {
JonFreeman 0:435bf84ce48a 156 const char * cmd_word; // points to text e.g. "menu"
JonFreeman 0:435bf84ce48a 157 const char * explan;
JonFreeman 0:435bf84ce48a 158 void (*f)(struct parameters &); // points to function
JonFreeman 0:435bf84ce48a 159 } ;
JonFreeman 0:435bf84ce48a 160
JonFreeman 0:435bf84ce48a 161 struct kb_command const command_list[] = {
JonFreeman 3:ecb00e0e8d68 162 {"ls", "Lists available commands", menucmd},
JonFreeman 3:ecb00e0e8d68 163 {"?", "Lists available commands, same as ls", menucmd},
JonFreeman 3:ecb00e0e8d68 164 // {"ti", "tickle to try to get mosfet driver charge pump primed", ti_cmd},
JonFreeman 0:435bf84ce48a 165 {"fw", "forward", fw_cmd},
JonFreeman 0:435bf84ce48a 166 {"re", "reverse", re_cmd},
JonFreeman 2:04761b196473 167 {"rb", "regen brake 0 to 99 %", rb_cmd},
JonFreeman 0:435bf84ce48a 168 {"hb", "hand brake", hb_cmd},
JonFreeman 2:04761b196473 169 {"vi", "set motors V and I percent RANGE 0 to 100", vi_cmd},
JonFreeman 3:ecb00e0e8d68 170 {"who", "search for connected units, e.g. 3who returs 'Hi there' if found", who_cmd},
JonFreeman 3:ecb00e0e8d68 171 {"mode", "read or set params in eeprom", mode_cmd},
JonFreeman 3:ecb00e0e8d68 172 {"erase", "set eeprom contents to all 0xff", erase_cmd},
JonFreeman 3:ecb00e0e8d68 173 {"kd", "kick the dog", kd_cmd},
JonFreeman 0:435bf84ce48a 174 {"nu", "do nothing", null_cmd},
JonFreeman 0:435bf84ce48a 175 };
JonFreeman 0:435bf84ce48a 176
JonFreeman 0:435bf84ce48a 177 const int numof_menu_items = sizeof(command_list) / sizeof(kb_command);
JonFreeman 0:435bf84ce48a 178 void menucmd (struct parameters & a)
JonFreeman 0:435bf84ce48a 179 {
JonFreeman 3:ecb00e0e8d68 180 com->printf("\r\n\nDouble Brushless Motor Driver 2018\r\nAt menucmd function - listing commands:-\r\n");
JonFreeman 0:435bf84ce48a 181 for(int i = 0; i < numof_menu_items; i++)
JonFreeman 3:ecb00e0e8d68 182 com->printf("[%s]\t\t%s\r\n", command_list[i].cmd_word, command_list[i].explan);
JonFreeman 3:ecb00e0e8d68 183 com->printf("End of List of Commands\r\n");
JonFreeman 0:435bf84ce48a 184 }
JonFreeman 0:435bf84ce48a 185
JonFreeman 3:ecb00e0e8d68 186
JonFreeman 3:ecb00e0e8d68 187 /*
JonFreeman 3:ecb00e0e8d68 188 New - March 2018
JonFreeman 3:ecb00e0e8d68 189 Using opto isolated serial port, paralleled up using same pair to multiple boards running this code.
JonFreeman 3:ecb00e0e8d68 190 New feature - commands have optional prefix digit 0-9 indicating which unit message is addressed to.
JonFreeman 3:ecb00e0e8d68 191 Commands without prefix digit - broadcast to all units, none to respond.
JonFreeman 3:ecb00e0e8d68 192 Only units recognising its address from prefix digit may respond. This avoids bus contention.
JonFreeman 3:ecb00e0e8d68 193 But for BROADCAST commands, '0' may respond on behalf of the group
JonFreeman 3:ecb00e0e8d68 194 */
JonFreeman 0:435bf84ce48a 195 //void command_line_interpreter (void const *argument)
JonFreeman 0:435bf84ce48a 196 void command_line_interpreter ()
JonFreeman 0:435bf84ce48a 197 {
JonFreeman 0:435bf84ce48a 198 const int MAX_CMD_LEN = 120;
JonFreeman 0:435bf84ce48a 199 static char cmd_line[MAX_CMD_LEN + 4];
JonFreeman 0:435bf84ce48a 200 static int cl_index = 0;
JonFreeman 0:435bf84ce48a 201 int ch;
JonFreeman 3:ecb00e0e8d68 202 char * pEnd, * cmd_line_ptr;
JonFreeman 0:435bf84ce48a 203 static struct parameters param_block ;
JonFreeman 3:ecb00e0e8d68 204 com = &com2;
JonFreeman 3:ecb00e0e8d68 205 // while (pc.readable()) {
JonFreeman 3:ecb00e0e8d68 206 while (com->readable()) {
JonFreeman 3:ecb00e0e8d68 207 ch = tolower(com->getc());
JonFreeman 0:435bf84ce48a 208 if (cl_index > MAX_CMD_LEN) { // trap out stupidly long command lines
JonFreeman 3:ecb00e0e8d68 209 com->printf ("Error!! Stupidly long cmd line\r\n");
JonFreeman 0:435bf84ce48a 210 cl_index = 0;
JonFreeman 0:435bf84ce48a 211 }
JonFreeman 0:435bf84ce48a 212 if(ch != '\r') // was this the 'Enter' key?
JonFreeman 0:435bf84ce48a 213 cmd_line[cl_index++] = ch; // added char to command being assembled
JonFreeman 0:435bf84ce48a 214 else { // key was CR, may or may not be command to lookup
JonFreeman 3:ecb00e0e8d68 215 param_block.target_unit = BROADCAST; // Broadcast
JonFreeman 3:ecb00e0e8d68 216 cmd_line_ptr = cmd_line;
JonFreeman 0:435bf84ce48a 217 cmd_line[cl_index] = 0; // null terminate command string
JonFreeman 0:435bf84ce48a 218 if(cl_index) { // If have got some chars to lookup
JonFreeman 0:435bf84ce48a 219 int i, wrdlen;
JonFreeman 3:ecb00e0e8d68 220 if (isdigit(cmd_line[0])) { // Look for command with prefix digit
JonFreeman 3:ecb00e0e8d68 221 cmd_line_ptr++; // point past identified digit prefix
JonFreeman 3:ecb00e0e8d68 222 param_block.target_unit = cmd_line[0]; // '0' to '9'
JonFreeman 3:ecb00e0e8d68 223 //com->printf ("Got prefix %c\r\n", cmd_line[0]);
JonFreeman 3:ecb00e0e8d68 224 }
JonFreeman 0:435bf84ce48a 225 for (i = 0; i < numof_menu_items; i++) { // Look for input match in command list
JonFreeman 0:435bf84ce48a 226 wrdlen = strlen(command_list[i].cmd_word);
JonFreeman 3:ecb00e0e8d68 227 if(strncmp(command_list[i].cmd_word, cmd_line_ptr, wrdlen) == 0 && !isalpha(cmd_line_ptr[wrdlen])) { // If match found
JonFreeman 0:435bf84ce48a 228 for (int k = 0; k < MAX_PARAMS; k++) {
JonFreeman 0:435bf84ce48a 229 param_block.dbl[k] = 0.0;
JonFreeman 0:435bf84ce48a 230 }
JonFreeman 0:435bf84ce48a 231 param_block.position_in_list = i;
JonFreeman 0:435bf84ce48a 232 param_block.last_time = clock ();
JonFreeman 0:435bf84ce48a 233 param_block.numof_dbls = 0;
JonFreeman 3:ecb00e0e8d68 234 pEnd = cmd_line_ptr + wrdlen;
JonFreeman 0:435bf84ce48a 235 while (*pEnd) { // Assemble all numerics as doubles
JonFreeman 0:435bf84ce48a 236 param_block.dbl[param_block.numof_dbls++] = strtod (pEnd, &pEnd);
JonFreeman 0:435bf84ce48a 237 while (*pEnd && !isdigit(*pEnd) && '-' != *pEnd && '+' != *pEnd) {
JonFreeman 0:435bf84ce48a 238 pEnd++;
JonFreeman 0:435bf84ce48a 239 }
JonFreeman 0:435bf84ce48a 240 }
JonFreeman 3:ecb00e0e8d68 241 //com->printf ("\r\n"); // Not allowed as many may output this.
JonFreeman 3:ecb00e0e8d68 242 //for (int k = 0; k < param_block.numof_dbls; k++)
JonFreeman 3:ecb00e0e8d68 243 // com->printf ("Read %.3f\r\n", param_block.dbl[k]);
JonFreeman 3:ecb00e0e8d68 244 // param_block.times[i] = clock();
JonFreeman 3:ecb00e0e8d68 245 if ((param_block.target_unit == BROADCAST) && (I_Am() == '0'))
JonFreeman 3:ecb00e0e8d68 246 param_block.respond = true;
JonFreeman 0:435bf84ce48a 247 command_list[i].f(param_block); // execute command
JonFreeman 0:435bf84ce48a 248 i = numof_menu_items + 1; // to exit for loop
JonFreeman 0:435bf84ce48a 249 } // end of match found
JonFreeman 0:435bf84ce48a 250 } // End of for numof_menu_items
JonFreeman 0:435bf84ce48a 251 if(i == numof_menu_items)
JonFreeman 3:ecb00e0e8d68 252 com->printf("No Match Found for CMD [%s]\r\n", cmd_line);
JonFreeman 0:435bf84ce48a 253 } // End of If have got some chars to lookup
JonFreeman 3:ecb00e0e8d68 254 //com->printf("\r\n>");
JonFreeman 0:435bf84ce48a 255 cl_index = 0;
JonFreeman 0:435bf84ce48a 256 } // End of else key was CR, may or may not be command to lookup
JonFreeman 3:ecb00e0e8d68 257 } // End of while (com->readable())
JonFreeman 0:435bf84ce48a 258 // Thread::wait(20); // Using RTOS on this project
JonFreeman 0:435bf84ce48a 259 // }
JonFreeman 0:435bf84ce48a 260 }
JonFreeman 0:435bf84ce48a 261
JonFreeman 0:435bf84ce48a 262