STM3 Twin Brushless Motor Electronic Speed Controller

Dependencies:   BufferedSerial FastPWM Servo mbed

cli_BLS_nortos.cpp

Committer:
JonFreeman
Date:
4 months ago
Revision:
7:6deaeace9a3e
Parent:
6:f289a49c1eae

File content as of revision 7:6deaeace9a3e:

//  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'
extern  int WatchDog;

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    read_last_VI    (double * val)  ;   //  only for test from cli

//BufferedSerial * com;

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]);
}

extern  void    mode_set   (int mode, double val)  ;   //  called from cli to set fw, re, rb, hb
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
        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
    }
}

void    rvi_cmd (struct parameters & a)  //  read last normalised values sent to pwms
{
    if  (a.respond) {
        double  r[6];
        read_last_VI    (r);
        a.com->printf ("rvi%.2f %.2f %.2f %.2f\r", r[0], r[1], r[2], r[3]);
    }
}

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

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;
//    a.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
{
    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;
}   ;
*/
extern  char mode_bytes[];

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   (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]);
    }
}

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);     //  gets rpm for each motor
        a.com->printf  ("rpm%d %d\r", dest[0], dest[1]);
    }
}

extern  double  rpm2mph ;
void    mph_cmd (struct parameters & a) //  to report miles per hour
{
        uint32_t dest[3];
        read_RPM    (dest);     //  gets rpm for each motor
        a.com->printf ("mph%c %.3f\r\n", mode_bytes[ID], (double)(dest[0] + dest[1]) * 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);
}

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[] = {
    {"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 'Hi there' 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},
    {"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},
    {"bc", "bogie constants - wheel dia, motor pinion, wheel gear", bogie_constants_report_cmd},
    {"nu", "do nothing", null_cmd},
};

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


struct  kb_command const pc_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 'Hi there' 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},
    {"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},
    {"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\nDouble Brushless Motor Driver 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 = 120;
    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?
            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
//    }*/
}