File content as of revision 12:a25bdf135348:

#include "mbed.h"
#include "Electric_Loco.h"
#include "AsyncSerial.hpp"

extern  Serial  pc;
extern  AsyncSerial com2escs;
extern  error_handling_Jan_2019   Controller_Error    ;         //  Provides array usable to store error codes.
extern  volatile    bool    trigger_32ms;
extern  command_line_interpreter_core   pcli, ploco;    //  pcli handles comms with pc, ploco handles comms with STM3_ESC boards

void    STM3_ESC_Interface::message (int board, char * msg)  //  Send message to one individual STM3_ESC
{
    if  (!(isdigit(board)))   {
        pc.printf   ("Error in STM3_ESC_Interface::message, '%c' not valid board ID\r\n");
        Controller_Error.set    (FAULT_BOARD_ID_IN_MSG, -1);
        return  ;
    }
    com2escs.putc    (board);
    message         (msg);
}

void    STM3_ESC_Interface::message (char * msg)  //  Broadcast message to all STM3_ESCs
{
        com2escs.printf    (msg);
}

void    STM3_ESC_Interface::set_V_limit (double p)  //  Sets max motor voltage
{
    if  (p < 0.0)
        p = 0.0;
    if  (p > 1.0)
        p = 1.0;
    last_V = p;
    com2escs.printf  ("v%d\r", (int)(last_V * 99.0));
}

void    STM3_ESC_Interface::set_I_limit (double p)     //  Sets max motor current
{
    if  (p < 0.0)
        p = 0.0;
    if  (p > 1.0)
        p = 1.0;
    last_I = p;     //  New 30/4/2018 ; no use for this yet, included to be consistent with V
    com2escs.printf  ("i%d\r", (int)(last_I * 99.0));
}

void    STM3_ESC_Interface::get_boards_list (int * dest)    {
    for (int i = 0; i < MAX_ESCS; i++)
        dest[i] = board_IDs[i];
}

void    STM3_ESC_Interface::search_for_escs ()  {   //  Seek out all STM3_ESC boards connected to TS controller
    char    whotxt[] = "0who\r\0";
    for (int i = 0; i < MAX_ESCS; i++)
        board_IDs[i] = 0;
    board_count = 0;
    pc.printf   ("Searching for connected STM3_ESC boards - ");
    while   (!trigger_32ms) 
        ploco.sniff  ();    //  Allow any previous STM3_ESC comms opportunity to complete
    while   (whotxt[0] <= '9')   {       //  Sniff out system, discover motor controllers connected
        trigger_32ms = false;
        message  (whotxt);   //  Issue '0who' etc
        whotxt[0]++;
        while   (!trigger_32ms) {     //  Give time for STM3_ESC board to respond
            pcli.sniff  ();     //  Check commands from pc also
            ploco.sniff  ();    //  This is where responses to 'who' get picked up and dealt with
        }
    }       //  Completed quick sniff to identify all connected STM3 ESC boards
    if  (board_count)   {
        pc.printf   ("Found %d boards, IDs ", board_count);
        for (int i = 0; i < board_count; i++)
            pc.printf   ("%c ", board_IDs[i]);
        pc.printf   ("\r\n");
    }
    else    
        pc.printf   ("None found\r\n");
}

void    STM3_ESC_Interface::set_board_ID (int a)   {    //  called in response to 'whon' coming back from a STM3_ESC
    board_count = 0;    //  reset and recalculate board_count
    while   (board_IDs[board_count])  {
        if  (board_IDs[board_count++] == a)   {
//            pc.printf   ("set_board_ID %c already listed\r\n", a);
            return;
        }
    }
    board_IDs[board_count++] = a;
}

bool    STM3_ESC_Interface::request_mph ()  {   //  Issue "'n'mph\r" to BLDC board to request RPM   22/06/2018
    if  (board_IDs[0] == 0)
        return  false;      //  No boards identified
    if  (board_IDs[reqno] == 0)
        reqno = 0;
    message (board_IDs[reqno++], "mph\r");
    return  true;
}

//void    STM3_ESC_Interface::mph_update(struct parameters & a)  {    //  Puts new readings into mem  22/06/2018
void    STM3_ESC_Interface::mph_update(double mph_reading)  {    //  Puts new readings into mem  22/06/2018
    static int identified_board = 0;
    esc_speeds[identified_board++] = mph_reading;               //  A single mph reading returned from one STM3ESC. a.dbl[0] gives esc number
    if  (identified_board >= board_count)    
        identified_board = 0;                       //  Circular buffer for number of STM3ESC boards sniffed and found
    double  temp = 0.0;
        for (int j = 0; j < board_count; j++)
            temp += esc_speeds[j];
    if  (board_count < 1)           //  Avoid possible DIV0 error
        mph = 0.0;
    else
        mph = temp / board_count;                       //  Updated average of most recent mph readings recieved
}