File content as of revision 11:bfb73f083009:

//  DualBLS2018_06
#include "mbed.h"
#include "BufferedSerial.h"
#include "FastPWM.h"
#include "DualBLS.h"
#include "brushless_motor.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'
extern  int     WatchDog;
extern  bool    WatchDogEnable;
extern  char    mode_bytes[];

extern  brushless_motor MotorA, MotorB;

const int   BROADCAST   = '\r';
const   int MAX_PARAMS = 20;
struct  parameters  {
    struct kb_command const * command_list;
    BufferedSerial * com;   //  pc or com2
    char    cmd_line[120];
    char    * cmd_line_ptr;
    int32_t position_in_list, numof_dbls, target_unit, numof_menu_items, cl_index, gp_i;
    double  dbl[MAX_PARAMS];
    bool    respond, resp_always;
}   ;

struct parameters pccom, lococom;
//  WithOUT RTOS
extern  BufferedSerial com2, pc;
extern  void    send_test   ()  ;
extern  void    setVI   (double v, double i)  ;
extern  void    setV    (double v)  ;
extern  void    setI    (double i)  ;
//extern  void    last_VI    (double * val)  ;   //  only for test from cli

//BufferedSerial * com;
extern  double  Read_DriverPot  ();
extern  double  Read_BatteryVolts   ();
void    pot_cmd (struct parameters & a)
{
    pc.printf   ("Driver's pot %.3f\r\n", Read_DriverPot  ());
}

void    null_cmd (struct parameters & a)
{
    if  (a.respond) 
        a.com->printf   ("At null_cmd, board ID %c, parameters : First %.3f, second %.3f\r\n", I_Am(), a.dbl[0], a.dbl[1]);
}

//    {"wden", "enable watchdog if modes allow", wden_lococmd},
//    {"wddi", "disable watchdog always", wddi_lococmd},

void    wden_lococmd (struct parameters & a)
{
    if  (mode_bytes[COMM_SRC] != 3)     //  When not hand pot control, allow watchdog enable
        WatchDogEnable  = true;
}
void    wden_pccmd (struct parameters & a)
{
    wden_lococmd    (a);
    a.com->printf   ("Watchdog %sabled\r\n", WatchDogEnable ? "En":"Dis");
}

void    wddi_lococmd (struct parameters & a)
{
    WatchDogEnable  = false;
}
void    wddi_pccmd (struct parameters & a)
{
    wddi_lococmd    (a);
    a.com->printf   ("Watchdog %sabled\r\n", WatchDogEnable ? "En":"Dis");
}

extern  void    report_motor_types  ()  ;
void    mt_cmd (struct parameters & a)
{
    report_motor_types  ();
//    if  (a.respond) 
//        a.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_both_motors   (int mode, double val)  ;   //  called from cli to set fw, re, rb, hb

void    rdi_cmd (struct parameters & a)  //  read motor currents
{
    if  (a.respond) 
        a.com->printf ("rdi%.0f %.0f %.1f\r", MotorA.I.ave, MotorB.I.ave, Read_BatteryVolts  ());  //  Format good to be unpicked by cli in touch screen controller
}

void    rvi_cmd (struct parameters & a)  //  read last normalised values sent to pwms
{
    if  (a.respond) 
        a.com->printf ("rvi%.2f %.2f %.2f %.2f\r", MotorA.last_V, MotorA.last_I, MotorB.last_V, MotorB.last_I);
}

void    fw_cmd (struct parameters & a)  //  Forward command
{
    mode_set_both_motors   (FORWARD, 0.0);
}

void    re_cmd (struct parameters & a)  //  Reverse command
{
    mode_set_both_motors   (REVERSE, 0.0);
}

void    rb_cmd (struct parameters & a)      //  Regen brake command
{
    double b = a.dbl[0] / 100.0;
//    a.com->printf   ("Applying brake %.3f\r\n", b);
    mode_set_both_motors   (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
{
    char    t[36];
    for (int i = 0; i < 32; i++)
        t[i] = 0xff;
    for (int i = 0; i < 8191; i += 32)  {
        a.com->printf (".");
        if  (!wr_24LC64   (i, t, 32))
            a.com->printf ("eeprom write prob\r\n");
    }
}
/*struct  motorpairoptions    {   //  This to be user settable in eeprom, 32 bytes
    uint8_t MotA_dir,   //  0 or 1
            MotB_dir,   //  0 or 1
            gang,       //  0 for separate control (robot mode), 1 for ganged loco bogie mode
            serv1,      //  0, 1, 2 = Not used, Input, Output
            serv2,      //  0, 1, 2 = Not used, Input, Output
            cmd_source, //  0 Invalid, 1 COM1, 2 COM2, 3 Pot, 4 Servo1, 5 Servo2
    {'1', '9', '0', "Alternative ID ascii '1' to '9'"}, //  defaults to '0' before eerom setup for first time
    {50, 250, 98,  "Wheel diameter mm"},   //  New 01/06/2018
    {10, 250, 27,  "Motor pinion"},   //  New 01/06/2018
    {50, 250, 85,  "Wheel gear"},   //  New 01/06/2018
//            last;
}   ;
*/

//  New 22 June 2018
//  get bogie bytes - report back to touch controller
void    gbb_cmd (struct parameters & a)       //  
{
    if  (a.target_unit == BROADCAST || !a.resp_always) {
//        a.com->printf ("At mode_cmd, can not use BROADCAST with mode_cmd\r\n");
    } else    {
        pc.printf   ("At gbb\r\n");
        char    eeprom_contents[36];    //  might need to be unsigned?
        memset  (eeprom_contents, 0, 36);
        a.com->printf   ("gbb");
        rd_24LC64   (0, eeprom_contents, 32);
        for (int i = 0; i < numof_eeprom_options; i++)
            a.com->printf (" %d", eeprom_contents[i]);
        a.com->putc ('\r');
        a.com->putc ('\n');
    }
}

void    mode_cmd (struct parameters & a)       //  With no params, reads eeprom contents. With params sets eeprom contents
{
    if  (a.target_unit == BROADCAST || !a.resp_always) {
//        a.com->printf ("At mode_cmd, can not use BROADCAST with mode_cmd\r\n");
    } else    {
        char    t[36];
        a.com->printf ("At mode_cmd with node %d\r\n", a.target_unit);
        rd_24LC64   (0, t, 32);
        a.com->printf ("Numof params=%d\r\n", a.numof_dbls);
        for (int i = 0; i < numof_eeprom_options; i++)
            a.com->printf ("%2x\t%s\r\n", t[i], option_list[i].t);
        if  (a.numof_dbls == 0) {   //  Read present contents, do not write
            a.com->printf ("That's it\r\n");
        } else    { //  Write new shit to eeprom
            a.com->printf ("\r\n");
            if  (a.numof_dbls != numof_eeprom_options) {
                a.com->printf ("params required = %d, you offered %d\r\n", numof_eeprom_options, a.numof_dbls);
            } else    { //  Have been passed correct number of parameters
                int b;
                a.com->printf("Ready to write params to eeprom\r\n");
                for (int i = 0; i < numof_eeprom_options; 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
                        a.com->printf("Warning - Parameter = %d, out of range, setting to default %d\r\n", b, option_list[i].def);
                        b = option_list[i].def;
                    }
                    a.com->printf ("0x%2x\t%s\r\n", (t[i] = b), option_list[i].t);
                }
                wr_24LC64   (0, t, numof_eeprom_options);
                memcpy  (mode_bytes,t,32);
                a.com->printf("Parameters set in eeprom\r\n");
            }
        }
    }
}
/*void    coast_cmd (struct parameters & a)   {   //  Coast

}
*/
void    hb_cmd (struct parameters & a)
{
    if  (a.respond) {
        a.com->printf   ("numof params = %d\r\n", a.numof_dbls);
        a.com->printf   ("Hand Brake : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]);
    }
    mode_set_both_motors   (HANDBRAKE, 0.0);
}

extern  uint32_t    last_temp_count;
void    temperature_cmd  (struct parameters & a)  {
    if  (a.respond) {
        a.com->printf ("tem%c %d\r\n", mode_bytes[ID], (last_temp_count / 16) - 50);
    }
}

void    bogie_constants_report_cmd  (struct parameters & a)  {
    if  (a.respond) {
        a.com->printf ("bc%c %d %d %d\r\n", mode_bytes[ID], mode_bytes[WHEELDIA], mode_bytes[MOTPIN], mode_bytes[WHEELGEAR]);
    }
}

void    rpm_cmd (struct parameters & a) //  to report e.g. RPM 1000 1000 ; speed for both motors
{
    if  (a.respond) 
        a.com->printf  ("rpm%d %d\r", MotorA.RPM, MotorB.RPM);
}

extern  double  rpm2mph ;
void    mph_cmd (struct parameters & a) //  to report miles per hour
{
    if  (a.respond) 
        a.com->printf ("mph%c %.3f\r", mode_bytes[ID], (double)(MotorA.RPM + MotorB.RPM) * rpm2mph / 2.0);
}

void    menucmd (struct parameters & a);

void    vi_cmd (struct parameters & a)
{
//    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)
{
//    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)
{
//    if  (a.respond)
//        a.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);
//    a.com->printf ("Poked %d up Dog\r\n", WatchDog);
}

void    who_cmd (struct parameters & a)
{
    int i = I_Am    ();
    if  (I_Am() == a.target_unit)
        a.com->printf ("who%c\r\n", a.target_unit);
}

extern  void    rcin_report ()  ;
void    rcin_pccmd (struct parameters & a)
{
    rcin_report ();
}

struct kb_command  {
    const char * cmd_word;         //  points to text e.g. "menu"
    const char * explan;
    void (*f)(struct parameters &);   //  points to function
}  ;


/**
struct  kb_command const loco_command_list[] = {
List of commands accepted from external controller through opto isolated com port 9600, 8,n,1
*/
struct  kb_command const loco_command_list[] = {
    {"ls", "Lists available commands", menucmd},
    {"?", "Lists available commands, same as ls", menucmd},
    {"fw", "forward", fw_cmd},
    {"re", "reverse", re_cmd},
    {"rb", "regen brake 0 to 99 %", rb_cmd},
    {"hb", "hand brake", hb_cmd},
    {"v", "set motors V percent RANGE 0 to 100", v_cmd},
    {"i", "set motors I percent RANGE 0 to 100", i_cmd},
    {"vi", "set motors V and I percent RANGE 0 to 100", vi_cmd},
    {"who", "search for connected units, e.g. 3who returs 'who3' if found", who_cmd},
    {"mode", "read or set params in eeprom", mode_cmd},
    {"erase", "set eeprom contents to all 0xff", erase_cmd},
    {"tem", "report temperature", temperature_cmd},
    {"kd", "kick the dog, reloads WatchDog", kd_cmd},
    {"wden", "enable watchdog if modes allow", wden_lococmd},
    {"wddi", "disable watchdog always", wddi_lococmd},
    {"rpm", "read motor pair speeds", rpm_cmd},
    {"mph", "read loco speed miles per hour", mph_cmd},
    {"rvi", "read most recent values sent to pwms", rvi_cmd},
    {"rdi", "read motor currents and power voltage", rdi_cmd},
    {"bc", "bogie constants - wheel dia, motor pinion, wheel gear", bogie_constants_report_cmd},    //  OBSOLETE, replaced by 'gbb'
    {"gbb", "get bogie bytes from eeprom and report", gbb_cmd},
    {"nu", "do nothing", null_cmd},
};

//const int numof_loco_menu_items = sizeof(loco_command_list) / sizeof(kb_command);


/**
struct  kb_command const loco_command_list[] = {
List of commands accepted from external pc through non-opto isolated com port 9600, 8,n,1
*/
struct  kb_command const pc_command_list[] = {
    {"ls", "Lists available commands", menucmd},
    {"?", "Lists available commands, same as ls", menucmd},
    {"mtypes", "report types of motors found", mt_cmd},
    {"pot", "read drivers control pot", pot_cmd},
    {"fw", "forward", fw_cmd},
    {"re", "reverse", re_cmd},
    {"rb", "regen brake 0 to 99 %", rb_cmd},
    {"hb", "hand brake", hb_cmd},
    {"v", "set motors V percent RANGE 0 to 100", v_cmd},
    {"i", "set motors I percent RANGE 0 to 100", i_cmd},
    {"vi", "set motors V and I percent RANGE 0 to 100", vi_cmd},
    {"who", "search for connected units, e.g. 3who returs 'who3' if found", who_cmd},
    {"mode", "read or set params in eeprom", mode_cmd},
    {"erase", "set eeprom contents to all 0xff", erase_cmd},
    {"tem", "report temperature", temperature_cmd},
    {"kd", "kick the dog, reloads WatchDog", kd_cmd},
    {"wden", "enable watchdog if modes allow", wden_pccmd},
    {"wddi", "disable watchdog always", wddi_pccmd},
    {"rcin", "Report Radio Control Input stuff", rcin_pccmd},
    {"rpm", "read motor pair speeds", rpm_cmd},
    {"mph", "read loco speed miles per hour", mph_cmd},
    {"rvi", "read most recent values sent to pwms", rvi_cmd},
    {"rdi", "read motor currents and power voltage", rdi_cmd},
    {"bc", "bogie constants - wheel dia, motor pinion, wheel gear", bogie_constants_report_cmd},
    {"gbb", "get bogie bytes from eeprom and report", gbb_cmd},    //  OBSOLETE, replaced by 'gbb'
    {"nu", "do nothing", null_cmd},
};

void    setup_comms ()  {
    pccom.com = & pc;
    pccom.command_list = pc_command_list;
    pccom.numof_menu_items = sizeof(pc_command_list) / sizeof(kb_command);
    pccom.cl_index  = 0;
    pccom.gp_i      = 0;    //  general puropse integer, not used to 30/4/2018
    pccom.resp_always   = true;
    lococom.com = & com2;
    lococom.command_list = loco_command_list;
    lococom.numof_menu_items = sizeof(loco_command_list) / sizeof(kb_command);
    lococom.cl_index    = 0;
    lococom.gp_i        = 0;    //  general puropse integer, toggles 0 / 1 to best guess source of rpm
    lococom.resp_always = false;
}


void    menucmd (struct parameters & a)
{
    if  (a.respond) {
        a.com->printf("\r\n\nDual BLDC ESC type STM3 2018\r\nAt menucmd function - listing commands:-\r\n");
        for(int i = 0; i < a.numof_menu_items; i++)
            a.com->printf("[%s]\t\t%s\r\n", a.command_list[i].cmd_word, a.command_list[i].explan);
        a.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, 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    cli_core    (struct parameters & a) {
    const int MAX_CMD_LEN = 180;
    int ch, IAm = I_Am();
    char * pEnd;//, * cmd_line_ptr;
    while  (a.com->readable()) {
        ch = a.com->getc();
        if  (a.cl_index > MAX_CMD_LEN)  {   //  trap out stupidly long command lines
            a.com->printf   ("Error!! Stupidly long cmd line\r\n");
            a.cl_index = 0;
        }
        if(ch != '\r')  {   //  was this the 'Enter' key?
            if  (ch != '\n')       //  Ignore line feeds
                a.cmd_line[a.cl_index++] = ch;  //  added char to command being assembled
        }
        else    {   //  key was CR, may or may not be command to lookup
            a.target_unit = BROADCAST;    //  Set to BROADCAST default once found command line '\r'
            a.cmd_line_ptr = a.cmd_line;
            a.cmd_line[a.cl_index] = 0; //  null terminate command string
            if(a.cl_index)    {   //  If have got some chars to lookup
                int i, wrdlen;
                if  (isdigit(a.cmd_line[0]))  {   //  Look for command with prefix digit
                    a.cmd_line_ptr++;     //  point past identified digit prefix
                    a.target_unit = a.cmd_line[0];  //  '0' to '9'
                    //com->printf ("Got prefix %c\r\n", cmd_line[0]);
                }
                for (i = 0; i < a.numof_menu_items; i++)   {   //  Look for input match in command list
                    wrdlen = strlen(a.command_list[i].cmd_word);
                    if(strncmp(a.command_list[i].cmd_word, a.cmd_line_ptr, wrdlen) == 0 && !isalpha(a.cmd_line_ptr[wrdlen]))  {   //  If match found
                        for (int k = 0; k < MAX_PARAMS; k++)    {
                            a.dbl[k] = 0.0;
                        }
                        a.position_in_list = i;
                        a.numof_dbls = 0;
                        pEnd = a.cmd_line_ptr + wrdlen;
                        while   (*pEnd)  {          //  Assemble all numerics as doubles
                            a.dbl[a.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]);
//                            param_block.times[i] = clock();
//                        a.respond = false;
                        a.respond = a.resp_always;
                        if  (((a.target_unit == BROADCAST) && (IAm == '0')) || (IAm == a.target_unit))
                            a.respond = true; //  sorted 26/4/18
                        //  All boards to obey BROADCAST command, only specific board to obey number prefixed command
                        if  ((a.target_unit == BROADCAST) || (IAm == a.target_unit))
                            a.command_list[i].f(a);   //  execute command if addressed to this unit
                        i = a.numof_menu_items + 1;    //  to exit for loop
                    }   //  end of match found
                }       // End of for numof_menu_items
                if(i == a.numof_menu_items)
                    a.com->printf("No Match Found for CMD [%s]\r\n", a.cmd_line);
            }           //  End of If have got some chars to lookup
            //com->printf("\r\n>");
            a.cl_index = 0;
        }               // End of else key was CR, may or may not be command to lookup
    }                   //  End of while (com->readable())
}

void    command_line_interpreter_pc    ()   {
    cli_core    (pccom);
}
void    command_line_interpreter_loco    () {
    cli_core    (lococom);
}

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, 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 < a.numof_menu_items; i++)   {   //  Look for input match in command list
                    wrdlen = strlen(a.command_list[i].cmd_word);
                    if(strncmp(a.command_list[i].cmd_word, a.cmd_line_ptr, wrdlen) == 0 && !isalpha(a.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++;
                            }
                        }
                        //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();
                        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
//    }*/
}