esc_comms.cpp

00001 #include "mbed.h"
00002 #include "Electric_Loco.h"
00003 #include "AsyncSerial.hpp"
00004 
00005 extern  Serial  pc;
00006 extern  AsyncSerial com2escs;
00007 extern  error_handling_Jan_2019   Controller_Error    ;         //  Provides array usable to store error codes.
00008 extern  volatile    bool    trigger_32ms;
00009 extern  command_line_interpreter_core   pcli, ploco;    //  pcli handles comms with pc, ploco handles comms with STM3_ESC boards
00010 
00011 void    STM3_ESC_Interface::message (int board, char * msg)  //  Send message to one individual STM3_ESC
00012 {
00013     if  (!(isdigit(board)))   {
00014         pc.printf   ("Error in STM3_ESC_Interface::message, '%c' not valid board ID\r\n");
00015         Controller_Error.set    (FAULT_BOARD_ID_IN_MSG, -1);
00016         return  ;
00017     }
00018     com2escs.putc    (board);
00019     message         (msg);
00020 }
00021 
00022 void    STM3_ESC_Interface::message (char * msg)  //  Broadcast message to all STM3_ESCs
00023 {
00024         com2escs.printf    (msg);
00025 }
00026 
00027 void    STM3_ESC_Interface::set_V_limit (double p)  //  Sets max motor voltage
00028 {
00029     if  (p < 0.0)
00030         p = 0.0;
00031     if  (p > 1.0)
00032         p = 1.0;
00033     last_V = p;
00034     com2escs.printf  ("v%d\r", (int)(last_V * 99.0));
00035 }
00036 
00037 void    STM3_ESC_Interface::set_I_limit (double p)     //  Sets max motor current
00038 {
00039     if  (p < 0.0)
00040         p = 0.0;
00041     if  (p > 1.0)
00042         p = 1.0;
00043     last_I = p;     //  New 30/4/2018 ; no use for this yet, included to be consistent with V
00044     com2escs.printf  ("i%d\r", (int)(last_I * 99.0));
00045 }
00046 
00047 void    STM3_ESC_Interface::get_boards_list (int * dest)    {
00048     for (int i = 0; i < MAX_ESCS; i++)
00049         dest[i] = board_IDs[i];
00050 }
00051 
00052 void    STM3_ESC_Interface::search_for_escs ()  {   //  Seek out all STM3_ESC boards connected to TS controller
00053     char    whotxt[] = "0who\r\0";
00054     for (int i = 0; i < MAX_ESCS; i++)
00055         board_IDs[i] = 0;
00056     board_count = 0;
00057     pc.printf   ("Searching for connected STM3_ESC boards - ");
00058     while   (!trigger_32ms) 
00059         ploco.sniff  ();    //  Allow any previous STM3_ESC comms opportunity to complete
00060     while   (whotxt[0] <= '9')   {       //  Sniff out system, discover motor controllers connected
00061         trigger_32ms = false;
00062         message  (whotxt);   //  Issue '0who' etc
00063         whotxt[0]++;
00064         while   (!trigger_32ms) {     //  Give time for STM3_ESC board to respond
00065             pcli.sniff  ();     //  Check commands from pc also
00066             ploco.sniff  ();    //  This is where responses to 'who' get picked up and dealt with
00067         }
00068     }       //  Completed quick sniff to identify all connected STM3 ESC boards
00069     if  (board_count)   {
00070         pc.printf   ("Found %d boards, IDs ", board_count);
00071         for (int i = 0; i < board_count; i++)
00072             pc.printf   ("%c ", board_IDs[i]);
00073         pc.printf   ("\r\n");
00074     }
00075     else    
00076         pc.printf   ("None found\r\n");
00077 }
00078 
00079 void    STM3_ESC_Interface::set_board_ID (int a)   {    //  called in response to 'whon' coming back from a STM3_ESC
00080     board_count = 0;    //  reset and recalculate board_count
00081     while   (board_IDs[board_count])  {
00082         if  (board_IDs[board_count++] == a)   {
00083 //            pc.printf   ("set_board_ID %c already listed\r\n", a);
00084             return;
00085         }
00086     }
00087     board_IDs[board_count++] = a;
00088 }
00089 
00090 bool    STM3_ESC_Interface::request_mph ()  {   //  Issue "'n'mph\r" to BLDC board to request RPM   22/06/2018
00091     if  (board_IDs[0] == 0)
00092         return  false;      //  No boards identified
00093     if  (board_IDs[reqno] == 0)
00094         reqno = 0;
00095     message (board_IDs[reqno++], "mph\r");
00096     return  true;
00097 }
00098 
00099 //void    STM3_ESC_Interface::mph_update(struct parameters & a)  {    //  Puts new readings into mem  22/06/2018
00100 void    STM3_ESC_Interface::mph_update(double mph_reading)  {    //  Puts new readings into mem  22/06/2018
00101     static int identified_board = 0;
00102     esc_speeds[identified_board++] = mph_reading;               //  A single mph reading returned from one STM3ESC. a.dbl[0] gives esc number
00103     if  (identified_board >= board_count)    
00104         identified_board = 0;                       //  Circular buffer for number of STM3ESC boards sniffed and found
00105     double  temp = 0.0;
00106         for (int j = 0; j < board_count; j++)
00107             temp += esc_speeds[j];
00108     if  (board_count < 1)           //  Avoid possible DIV0 error
00109         mph = 0.0;
00110     else
00111         mph = temp / board_count;                       //  Updated average of most recent mph readings recieved
00112 }
00113