Diff: cli_nortos.cpp

Revision:

5:ca86a7848d54

Parent:

4:21d91465e4b1

--- a/cli_nortos.cpp	Thu Apr 26 08:23:04 2018 +0000
+++ b/cli_nortos.cpp	Tue May 29 16:36:34 2018 +0000
@@ -1,60 +1,85 @@
+//  DualBLS2018_03
 #include "mbed.h"
 #include "BufferedSerial.h"
+
 #include <cctype>
 #include "DualBLS.h"
 using namespace std;
 
 extern  int I_Am    ()  ;      //  Returns boards id number as ASCII char '0', '1' etc. Code for Broadcast = '\r'
-//typedef double  fl_typ;  //  
+extern  int WatchDog;
 
 const int   BROADCAST   = '\r';
 const   int MAX_PARAMS = 20;
 struct  parameters  {
-//    int32_t times[numof_menu_items];
-//    int32_t times[50];
-    int32_t position_in_list, last_time, numof_dbls, target_unit;
+    int32_t position_in_list,   //  set but not used Apr 2018, contains i for i'th menu item
+//            last_time,          //  gets reading from clock() ; not known to be useful or reliable
+            numof_dbls,
+            target_unit;
     double  dbl[MAX_PARAMS];
     bool    respond;
 }   ;
 
 //  WithOUT RTOS
 extern  BufferedSerial com2, pc;
-//extern  void    set_I_limit (double p)  ;   //  Sets max motor current
-//extern  void    set_V_limit (double p)  ;   //  Sets max motor voltage
 extern  void    send_test   ()  ;
 extern  void    setVI   (double v, double i)  ;
 extern  void    setV    (double v)  ;
 extern  void    setI    (double i)  ;
+extern  void    read_last_VI    (double * val)  ;   //  only for test from cli
 
 BufferedSerial * com;
 
-void    null_cmd (struct parameters & a)   {
-    com->printf   ("At null_cmd, parameters : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]);
+void    null_cmd (struct parameters & a)
+{
+    if  (a.respond) 
+        com->printf   ("At null_cmd, board ID %c, parameters : First %.3f, second %.3f\r\n", I_Am(), a.dbl[0], a.dbl[1]);
+}
+
+extern  void    mode_set   (int mode, double val)  ;
+extern  void    read_supply_vi   (double * val)  ;
+
+void    rdi_cmd (struct parameters & a)  //  read motor currents
+{
+    if  (a.respond) {
+        double  r[4];
+        read_supply_vi  (r);    //  get MotorA.I.ave, MotorB.I.ave, Battery volts
+        com->printf ("rdi%.0f %.0f %.1f\r", r[0], r[1], r[2]);  //  Format good to be unpicked by cli in touch screen controller
+    }
 }
 
-extern  void    mode_test   (int mode, double val)  ;
-
-void    fw_cmd (struct parameters & a)   {
-    com->printf   ("Forward : First %d, second %d\r\n", a.dbl[0], a.dbl[1]);
-    mode_test   (FORWARD, 0.0);
+void    rvi_cmd (struct parameters & a)  //  read last normalised values sent to pwms
+{
+    if  (a.respond) {
+        double  r[6];
+        read_last_VI    (r);
+        com->printf ("rvi%.2f %.2f %.2f %.2f\r", r[0], r[1], r[2], r[3]);
+    }
 }
 
-void    re_cmd (struct parameters & a)   {
-    com->printf   ("Reverse : First %d, second %d\r\n", a.dbl[0], a.dbl[1]);
-    mode_test   (REVERSE, 0.0);
+void    fw_cmd (struct parameters & a)  //  Forward command
+{
+    mode_set   (FORWARD, 0.0);
 }
 
-void    rb_cmd (struct parameters & a)   {  //  Regen brake
-    double b = a.dbl[0] / 99.0;
-    com->printf   ("Applying brake %.3f\r\n", b);
-    mode_test   (REGENBRAKE, b);
+void    re_cmd (struct parameters & a)  //  Reverse command
+{
+    mode_set   (REVERSE, 0.0);
+}
+
+void    rb_cmd (struct parameters & a)      //  Regen brake command
+{
+    double b = a.dbl[0] / 100.0;
+//    com->printf   ("Applying brake %.3f\r\n", b);
+    mode_set   (REGENBRAKE, b);
 //    apply_brake (b);
 }
 
 extern  bool    wr_24LC64  (int mem_start_addr, char * source, int length)   ;
 extern  bool    rd_24LC64  (int mem_start_addr, char * dest, int length)   ;
 
-void    erase_cmd (struct parameters & a)   {   //  Sets eeprom contents to all 0xff. 256 pages of 32 bytes to do
+void    erase_cmd (struct parameters & a)       //  Sets eeprom contents to all 0xff. 256 pages of 32 bytes to do
+{
     char    t[36];
     for (int i = 0; i < 32; i++)
         t[i] = 0xff;
@@ -74,91 +99,95 @@
             last;
 }   ;
 */
-struct  optpar  {
-    int min, max, def;
-    const char * t;
-}   ;
-struct  optpar const option_list[] = {
-    {0, 1, 1, "MotorA direction 0 or 1"},
-    {0, 1, 0, "MotorB direction 0 or 1"},
-    {0, 1, 1, "gang 0 for separate control (robot mode), 1 for ganged loco bogie mode"},
-    {0, 2, 2, "Servo1 0, 1, 2 = Not used, Input, Output"},
-    {0, 2, 2, "Servo2 0, 1, 2 = Not used, Input, Output"},
-    {1, 5, 2, "Command source 0 Invalid, 1 COM1, 2 COM2, 3 Pot, 4 Servo1, 5 Servo2"},
-    {0, 9, 0, "Alternative ID 0 to 9"},
-}   ;
 
-void    mode_cmd (struct parameters & a)   {   //  With no params, reads eeprom contents. With params sets eeprom contents
-    char    t[36];
-    const int numofopts = sizeof(option_list) / sizeof(struct optpar);
-    rd_24LC64   (0, t, 32);
-    com->printf ("Numof params=%d\r\n", a.numof_dbls);
-    for (int i = 0; i < numofopts; i++)
-        com->printf ("%2x\t%s\r\n", t[i], option_list[i].t);
-    if  (a.numof_dbls == 0) {   //  Read present contents, do not write
-        com->printf ("That's it\r\n");
-    }
-    else    {   //  Write new shit to eeprom
-        com->printf ("\r\n");
-        if  (a.numof_dbls != numofopts) {
-            com->printf ("params required = %d, you offered %d\r\n", numofopts, a.numof_dbls);
-        }
-        else    {   //  Have been passed correct number of parameters
-            int b;
-            com->printf("Ready to write params to eeprom\r\n");
-            for (int i = 0; i < numofopts; i++) {
-                b = (int)a.dbl[i];  //  parameter value to check against limits
-                if  ((b < option_list[i].min) || (b > option_list[i].max))  {   //  if parameter out of range
-                    com->printf("Warning - Parameter = %d, out of range, setting to default %d\r\n", b, option_list[i].def);
-                    b = option_list[i].def;
+void    mode_cmd (struct parameters & a)       //  With no params, reads eeprom contents. With params sets eeprom contents
+{
+    if  (a.target_unit == BROADCAST) {
+//        com->printf ("At mode_cmd, can not use BROADCAST with mode_cmd\r\n");
+    } else    {
+        char    t[36];
+        com->printf ("At mode_cmd with node %d\r\n", a.target_unit);
+        rd_24LC64   (0, t, 32);
+        com->printf ("Numof params=%d\r\n", a.numof_dbls);
+        for (int i = 0; i < numofopts; i++)
+            com->printf ("%2x\t%s\r\n", t[i], option_list[i].t);
+        if  (a.numof_dbls == 0) {   //  Read present contents, do not write
+            com->printf ("That's it\r\n");
+        } else    { //  Write new shit to eeprom
+            com->printf ("\r\n");
+            if  (a.numof_dbls != numofopts) {
+                com->printf ("params required = %d, you offered %d\r\n", numofopts, a.numof_dbls);
+            } else    { //  Have been passed correct number of parameters
+                int b;
+                com->printf("Ready to write params to eeprom\r\n");
+                for (int i = 0; i < numofopts; i++) {
+                    b = (int)a.dbl[i];  //  parameter value to check against limits
+                    if  (i == 6)    //  Alternative ID must be turned to ascii
+                        b |= '0';
+                    if  ((b < option_list[i].min) || (b > option_list[i].max))  {   //  if parameter out of range
+                        com->printf("Warning - Parameter = %d, out of range, setting to default %d\r\n", b, option_list[i].def);
+                        b = option_list[i].def;
+                    }
+                    com->printf ("0x%2x\t%s\r\n", (t[i] = b), option_list[i].t);
                 }
-                com->printf ("%2x\t%s\r\n", (t[i] = b), option_list[i].t);
+                wr_24LC64   (0, t, numofopts);
+                com->printf("Parameters set in eeprom\r\n");
             }
-            wr_24LC64   (0, t, numofopts);
-            com->printf("Parameters set in eeprom\r\n");
         }
     }
 }
 /*void    coast_cmd (struct parameters & a)   {   //  Coast
-    
+
 }
 */
-void    hb_cmd (struct parameters & a)   {
-    com->printf   ("numof params = %d\r\n", a.numof_dbls);
-    com->printf   ("Hand Brake : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]);
-    mode_test   (HANDBRAKE, 0.0);
+void    hb_cmd (struct parameters & a)
+{
+    if  (a.respond) {
+        com->printf   ("numof params = %d\r\n", a.numof_dbls);
+        com->printf   ("Hand Brake : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]);
+    }
+    mode_set   (HANDBRAKE, 0.0);
+}
+
+extern  void    read_RPM    (uint32_t * dest)  ;
+void    rpm_cmd (struct parameters & a) //  to report e.g. RPM 1000 1000 ; speed for both motors
+{
+    if  (a.respond) {
+        uint32_t dest[3];
+        read_RPM    (dest);
+        com->printf  ("rpm%d %d\r", dest[0], dest[1]);
+    }
 }
 
 void    menucmd (struct parameters & a);
 
 void    vi_cmd (struct parameters & a)
 {
-    com->printf   ("In setVI, setting V to %.2f, I %.2f\r\n", a.dbl[0], a.dbl[1]);
+//    if  (a.respond)
+//        com->printf   ("In setVI, setting V to %.2f, I %.2f\r\n", a.dbl[0], a.dbl[1]);
     setVI   (a.dbl[0] / 100.0, a.dbl[1] / 100.0);
 }
 
 void    v_cmd (struct parameters & a)
 {
-    com->printf   ("In setV, setting V to %.2f\r\n", a.dbl[0]);
+//    if  (a.respond)
+//        com->printf   ("In setV, setting V to %.2f\r\n", a.dbl[0]);
     setV   (a.dbl[0] / 100.0);
 }
 
 void    i_cmd (struct parameters & a)
 {
-    com->printf   ("In setI, setting I to %.2f\r\n", a.dbl[0]);
+//    if  (a.respond)
+//        com->printf   ("In setI, setting I to %.2f\r\n", a.dbl[0]);
     setI   (a.dbl[0] / 100.0);
 }
 
 void    kd_cmd (struct parameters & a)  //  kick the watchdog
 {
+    WatchDog = WATCHDOG_RELOAD + (I_Am() & 0x0f);
+//    com->printf ("Poked %d up Dog\r\n", WatchDog);
 }
 
-/*extern  void    seredgetest ()  ;
-void    s_cmd (struct parameters & a)  //  test use of InterruptIn on serialout
-{
-    seredgetest ()  ;
-}*/
-
 void    who_cmd (struct parameters & a)
 {
     int i = I_Am    ();
@@ -175,7 +204,6 @@
 struct  kb_command const command_list[] = {
     {"ls", "Lists available commands", menucmd},
     {"?", "Lists available commands, same as ls", menucmd},
-//    {"s", "seredgetest", s_cmd},
     {"fw", "forward", fw_cmd},
     {"re", "reverse", re_cmd},
     {"rb", "regen brake 0 to 99 %", rb_cmd},
@@ -186,91 +214,98 @@
     {"who", "search for connected units, e.g. 3who returs 'Hi there' if found", who_cmd},
     {"mode", "read or set params in eeprom", mode_cmd},
     {"erase", "set eeprom contents to all 0xff", erase_cmd},
-    {"kd", "kick the dog", kd_cmd},
+    {"kd", "kick the dog, reloads WatchDog", kd_cmd},
+    {"rpm", "read motor pair speeds", rpm_cmd},
+    {"rvi", "read most recent values sent to pwms", rvi_cmd},
+    {"rdi", "read motor currents and power voltage", rdi_cmd},
     {"nu", "do nothing", null_cmd},
 };
 
 const int numof_menu_items = sizeof(command_list) / sizeof(kb_command);
 void    menucmd (struct parameters & a)
 {
-    com->printf("\r\n\nDouble Brushless Motor Driver 2018\r\nAt menucmd function - listing commands:-\r\n");
-    for(int i = 0; i < numof_menu_items; i++)
-        com->printf("[%s]\t\t%s\r\n", command_list[i].cmd_word, command_list[i].explan);
-    com->printf("End of List of Commands\r\n");
+    if  (a.respond) {
+        com->printf("\r\n\nDouble Brushless Motor Driver 2018\r\nAt menucmd function - listing commands:-\r\n");
+        for(int i = 0; i < numof_menu_items; i++)
+            com->printf("[%s]\t\t%s\r\n", command_list[i].cmd_word, command_list[i].explan);
+        com->printf("End of List of Commands\r\n");
+    }
 }
 
-
 /*
 New - March 2018
 Using opto isolated serial port, paralleled up using same pair to multiple boards running this code.
 New feature - commands have optional prefix digit 0-9 indicating which unit message is addressed to.
-Commands without prefix digit - broadcast to all units, none to respond.
+Commands without prefix digit - broadcast to all units, all to obey but none to respond.
 Only units recognising its address from prefix digit may respond. This avoids bus contention.
 But for BROADCAST commands, '0' may respond on behalf of the group
 */
 //void    command_line_interpreter    (void const *argument)
 void    command_line_interpreter    ()
 {
-const int MAX_CMD_LEN = 120;
-static  char    cmd_line[MAX_CMD_LEN + 4];
-static  int     cl_index = 0;
-int ch;
-char * pEnd, * cmd_line_ptr;
-static struct  parameters  param_block  ;
-        com = &com2;
-//        while  (pc.readable()) {
-        while  (com->readable()) {
-            ch = tolower(com->getc());
-            if  (cl_index > MAX_CMD_LEN)  {   //  trap out stupidly long command lines
-                com->printf   ("Error!! Stupidly long cmd line\r\n");
-                cl_index = 0;
-            }
-            if(ch != '\r')  //  was this the 'Enter' key?
-                cmd_line[cl_index++] = ch;  //  added char to command being assembled
-            else    {   //  key was CR, may or may not be command to lookup
-                param_block.target_unit = BROADCAST;    //  Broadcast
-                cmd_line_ptr = cmd_line;
-                cmd_line[cl_index] = 0; //  null terminate command string
-                if(cl_index)    {   //  If have got some chars to lookup
-                    int i, wrdlen;
-                    if  (isdigit(cmd_line[0]))  {   //  Look for command with prefix digit
-                        cmd_line_ptr++;     //  point past identified digit prefix
-                        param_block.target_unit = cmd_line[0];  //  '0' to '9'
-                        //com->printf ("Got prefix %c\r\n", cmd_line[0]);
-                    }
-                    for (i = 0; i < numof_menu_items; i++)   {   //  Look for input match in command list
-                        wrdlen = strlen(command_list[i].cmd_word);
-                        if(strncmp(command_list[i].cmd_word, cmd_line_ptr, wrdlen) == 0 && !isalpha(cmd_line_ptr[wrdlen]))  {   //  If match found
-                            for (int k = 0; k < MAX_PARAMS; k++)    {
-                                param_block.dbl[k] = 0.0;
+    const int MAX_CMD_LEN = 120;
+    static  char    cmd_line[MAX_CMD_LEN + 4];
+    static  int     cl_index = 0;
+    int ch, IAm = I_Am();
+    char * pEnd, * cmd_line_ptr;
+    static struct  parameters  param_block  ;
+    com = &com2;
+    while  (com->readable()) {
+//        ch = tolower(com->getc());
+        ch = com->getc();
+        if  (cl_index > MAX_CMD_LEN)  {   //  trap out stupidly long command lines
+            com->printf   ("Error!! Stupidly long cmd line\r\n");
+            cl_index = 0;
+        }
+        if(ch != '\r')  //  was this the 'Enter' key?
+            cmd_line[cl_index++] = ch;  //  added char to command being assembled
+        else    {   //  key was CR, may or may not be command to lookup
+            param_block.target_unit = BROADCAST;    //  Set to BROADCAST default once found command line '\r'
+            cmd_line_ptr = cmd_line;
+            cmd_line[cl_index] = 0; //  null terminate command string
+            if(cl_index)    {   //  If have got some chars to lookup
+                int i, wrdlen;
+                if  (isdigit(cmd_line[0]))  {   //  Look for command with prefix digit
+                    cmd_line_ptr++;     //  point past identified digit prefix
+                    param_block.target_unit = cmd_line[0];  //  '0' to '9'
+                    //com->printf ("Got prefix %c\r\n", cmd_line[0]);
+                }
+                for (i = 0; i < numof_menu_items; i++)   {   //  Look for input match in command list
+                    wrdlen = strlen(command_list[i].cmd_word);
+                    if(strncmp(command_list[i].cmd_word, cmd_line_ptr, wrdlen) == 0 && !isalpha(cmd_line_ptr[wrdlen]))  {   //  If match found
+                        for (int k = 0; k < MAX_PARAMS; k++)    {
+                            param_block.dbl[k] = 0.0;
+                        }
+                        param_block.position_in_list = i;
+//                        param_block.last_time = clock    ();
+                        param_block.numof_dbls = 0;
+                        pEnd = cmd_line_ptr + wrdlen;
+                        while   (*pEnd)  {          //  Assemble all numerics as doubles
+                            param_block.dbl[param_block.numof_dbls++] = strtod    (pEnd, &pEnd);
+                            while   (*pEnd && !isdigit(*pEnd) && '-' != *pEnd && '+' != *pEnd)  {
+                                pEnd++;
                             }
-                            param_block.position_in_list = i;
-                            param_block.last_time = clock    ();
-                            param_block.numof_dbls = 0;
-                            pEnd = cmd_line_ptr + wrdlen;
-                            while   (*pEnd)  {          //  Assemble all numerics as doubles
-                                param_block.dbl[param_block.numof_dbls++] = strtod    (pEnd, &pEnd);
-                                while   (*pEnd && !isdigit(*pEnd) && '-' != *pEnd && '+' != *pEnd)  {   
-                                    pEnd++;
-                                }
-                            }
-                            //com->printf   ("\r\n");   //  Not allowed as many may output this.
-                            //for (int k = 0; k < param_block.numof_dbls; k++)
-                            //    com->printf   ("Read %.3f\r\n", param_block.dbl[k]);
+                        }
+                        //com->printf   ("\r\n");   //  Not allowed as many may output this.
+                        //for (int k = 0; k < param_block.numof_dbls; k++)
+                        //    com->printf   ("Read %.3f\r\n", param_block.dbl[k]);
 //                            param_block.times[i] = clock();
-                            if  ((param_block.target_unit == BROADCAST) && (I_Am() == '0'))
-                                param_block.respond = true;
-                            command_list[i].f(param_block);   //  execute command
-                            i = numof_menu_items + 1;    //  to exit for loop
-                        }   //  end of match found
-                    }       // End of for numof_menu_items
-                    if(i == numof_menu_items)
-                        com->printf("No Match Found for CMD [%s]\r\n", cmd_line);
-                }           //  End of If have got some chars to lookup
-                //com->printf("\r\n>");
-                cl_index = 0;
-            }               // End of else key was CR, may or may not be command to lookup
-        }                   //  End of while (com->readable())
+                        param_block.respond = false;
+                        if  (((param_block.target_unit == BROADCAST) && (IAm == '0')) || (IAm == param_block.target_unit))
+                            param_block.respond = true; //  sorted 26/4/18
+                        //  All boards to obey BROADCAST command, only specific board to obey number prefixed command
+                        if  ((param_block.target_unit == BROADCAST) || (IAm == param_block.target_unit))
+                            command_list[i].f(param_block);   //  execute command if addressed to this unit
+                        i = numof_menu_items + 1;    //  to exit for loop
+                    }   //  end of match found
+                }       // End of for numof_menu_items
+                if(i == numof_menu_items)
+                    com->printf("No Match Found for CMD [%s]\r\n", cmd_line);
+            }           //  End of If have got some chars to lookup
+            //com->printf("\r\n>");
+            cl_index = 0;
+        }               // End of else key was CR, may or may not be command to lookup
+    }                   //  End of while (com->readable())
 //        Thread::wait(20);  //  Using RTOS on this project
 //    }
 }