Diff: esc_comms.cpp

Revision:

12:a25bdf135348

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/esc_comms.cpp	Mon Jan 14 16:39:41 2019 +0000
@@ -0,0 +1,113 @@
+#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
+}
+