Diff: main.cpp

Revision:

0:89afb50e974f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jul 25 09:13:38 2018 +0000
@@ -0,0 +1,284 @@
+#include "mbed.h"
+#include "XBeeLib.h"
+
+using namespace XBeeLib;
+/* System states */    
+typedef enum
+{
+    BOOTING,    
+    NORMAL_OPERATION,
+    EMERGENCY,
+    WAIT_FOR_RESET,
+    HEARTBEAT_ERROR,
+    GRACE_PERIOD,
+} state_t;
+
+state_t currentState = BOOTING;
+
+int receivedData, currentMessageCounter, graceCounter, currentTimerValue;
+int heartbeat_msg = 49, emergency_msg = 50, reset_msg = 46;
+int systemState = 1; // 0 = Emergency, 1 = System Ok
+int systemResetUsed = 0;
+
+/* Change network values */
+//------------DIGIMESH CONFIG-------//
+#define channel 0x18
+#define networkId 0xD164
+#define powerLevel 4
+#define nodeId "robotNode"
+#define baudRate 230400
+//------------DIGIMESH CONFIG-------//
+
+//------------PIN CONFIG------------//
+DigitalOut statusLED(PB_4);
+DigitalOut powerLED(PB_5);
+DigitalOut MCU_Status(PF_1);
+DigitalOut resetOutput(PA_11);
+
+DigitalIn localEstop(PA_12);
+DigitalIn safetyRelayStatus(PB_1);
+DigitalIn bumperStatus(PB_0);
+//------------PIN CONFIG------------//
+
+//-----------SPI CONFIG------------//
+SPI spi(PA_7, PA_6, PA_5);      // mosi, miso, sclk
+DigitalOut chipSelect(PA_4);    // nss
+#define spiSpeed 10000000
+//-----------SPI CONFIG------------//
+
+//-----------TIMER CONFIG----------//
+Timer runCheckHeartbeatTimer, runSystemChecksTimer;
+#define systemCheckTimeout 5    // ms   // Check system state every 5ms
+#define heartbeatTimeout 100    // ms   // Check heartbeat every 100ms
+//-----------TIMER CONFIG----------//
+
+/* If heartbeat is missed for more then 10 times, 
+system goes into HEARTBEAT_ERROR state.
+The reset relay is activated for 100*5ms to ensure
+the emergencystop relay is reverting to normal state.
+Changing this time to lower values causes the emergencystop
+relay to not always acknowledge the reset */
+//-----------SYSTEM VARIABLES------//
+#define hearbeatMisses 10               
+#define resetRelayActivationTime 100
+//-----------SYSTEM VARIABLES------//
+
+/* Config XBee radio. All the previously defined 
+values are written to the XBee module. DMLocalNode.write_config()
+is used to save the values to the rom of the module. */
+// RADIO CONFIG
+void radioConfig(XBeeDM &DMLocalNode){    
+    RadioStatus temp = DMLocalNode.init();
+    temp = DMLocalNode.set_channel(channel);
+    temp = DMLocalNode.set_network_id(networkId);
+    temp = DMLocalNode.set_power_level(powerLevel);
+    temp = DMLocalNode.set_node_identifier(nodeId);
+    temp = DMLocalNode.write_config();
+}
+
+/* Functions who need to run once are performed here */
+void boot(XBeeDM &DMLocalNode){
+    radioConfig(DMLocalNode);
+    statusLED.write(1);
+    powerLED.write(1);
+        
+    spi.format(8,3);
+    spi.frequency(spiSpeed);
+}
+
+/* If a message has been received, this function
+is used to retreive the message from the buffer.
+A substraction of 3 is used to get the right value. 
+This is due to some conversion being done in the library */
+static void receive_cb(const RemoteXBeeDM& remote, bool broadcast, const uint8_t *const data, uint16_t len){
+    receivedData = (data[0]-3);
+}
+
+/* If a message is to be sent, this function
+is used to send the message. */
+static void sendMessage(XBeeDM &DMLocalNode, char *sendData){
+    const char data[] = {*sendData};
+    const uint16_t data_len = strlen(data);
+
+    const TxStatus txStatus = DMLocalNode.send_data_broadcast((const uint8_t *)data, data_len);
+    
+    powerLED = !powerLED;
+}
+
+/* This function checks the physical E-Stop status. */
+void checkSystemStatus(){
+    if (localEstop == 0 || bumperStatus == 0 ){
+        currentState = EMERGENCY;
+        systemState = 0;
+    }
+    if (localEstop == 1 && bumperStatus == 1 && currentState == EMERGENCY){
+        currentState = WAIT_FOR_RESET;
+        systemState = 1; 
+    }
+}
+
+/* This function checks if a system state change is
+needed, and performs the necessary actions tied to 
+this state */
+void stateHandler(XBeeDM &DMLocalNode){        
+    if (currentState == NORMAL_OPERATION){
+        checkSystemStatus();
+        MCU_Status.write(1);
+        statusLED.write(0);        
+        powerLED.write(1);
+                
+        if (systemResetUsed != resetRelayActivationTime){
+            resetOutput.write(1);
+            systemResetUsed++; 
+        }
+        if (systemResetUsed == resetRelayActivationTime){
+            resetOutput.write(0);
+        }
+    }
+    
+    if (currentState == EMERGENCY && currentMessageCounter != 5){
+        sendMessage(DMLocalNode, "50");
+        checkSystemStatus();
+        currentMessageCounter++;
+        
+        MCU_Status.write(0);
+        statusLED.write(1);
+        powerLED.write(0);
+        
+        systemResetUsed = 0; 
+    }
+    
+    if (currentState == WAIT_FOR_RESET){
+        currentMessageCounter = 0;
+        
+        MCU_Status.write(0);
+        statusLED.write(1);
+        powerLED.write(0);
+        
+        systemResetUsed = 0;
+    }
+    
+    if (currentState == HEARTBEAT_ERROR){
+        MCU_Status.write(0);
+        statusLED.write(0);
+        powerLED.write(0);
+        
+        systemResetUsed = 0; 
+    }
+    
+    if (currentState == GRACE_PERIOD){
+        graceCounter++;
+        if (graceCounter == 40){
+            currentState = NORMAL_OPERATION;
+            graceCounter = 0;
+        }
+    }
+}
+
+/* If a heartbeat message has been received
+currentHeartbeatValue is reset */
+void heartbeatTimerReset(){
+    currentTimerValue = 0;
+}
+
+/* If more heartbeats are missed then allowed
+the system enters the HEARTBEAT_ERROR state */
+void checkHeartbeat(){
+    if (currentState == NORMAL_OPERATION){
+    currentTimerValue++;
+        if (currentTimerValue > hearbeatMisses){
+            currentState = HEARTBEAT_ERROR;
+        }
+    }   
+}
+
+
+/* Incoming message are handled here and necessary
+actions are taken if needed */
+void handleMessages(XBeeDM &DMLocalNode){
+    if (receivedData == emergency_msg){
+        currentState = EMERGENCY;
+        graceCounter = 0;
+    }
+    
+/* The system enters the GRACE_PERIOD state if
+the previous state was WAITING_FOR_RESET and a
+reset message has been received. If a emergency
+message is received in this period, the system, 
+again, enters the WAIT_FOR_RESET state. This means
+that there are one or multiple nodes still in 
+EMERGENCY_STATE. Check all nodes to ensure all of
+them have been taken care of*/ 
+    if (currentState == WAIT_FOR_RESET && systemState == 1 && receivedData == reset_msg){
+        currentState = GRACE_PERIOD;
+    }
+    
+    if (receivedData == heartbeat_msg){
+        heartbeatTimerReset();
+    }
+    
+    if (receivedData == heartbeat_msg && currentState == HEARTBEAT_ERROR){
+        currentState = WAIT_FOR_RESET;
+        heartbeatTimerReset();
+    }
+    
+    if (currentState == EMERGENCY && currentMessageCounter == 5 && receivedData == reset_msg){
+        currentMessageCounter = 0;
+    }
+}
+
+// Handle SPI messages
+void handleSPI(){
+    chipSelect = 0; // Select device
+    spi.write(systemState);
+    chipSelect = 1; // Deselect device
+}
+
+void runSystemChecks(XBeeDM &DMLocalNode){
+    stateHandler(DMLocalNode);
+    handleMessages(DMLocalNode);
+    handleSPI();
+}
+
+int main() {
+/* The creation of the DMLocalNode */
+    XBeeDM DMLocalNode = XBeeDM(RADIO_TX, RADIO_RX, RADIO_RESET, NC, NC, baudRate);
+ 
+ /* Perform boot functions and create the callback
+for the received messages */   
+    boot(DMLocalNode);
+    DMLocalNode.register_receive_cb(&receive_cb);
+    
+/* Upon boot the system waits to be included in 
+the network. A reset can be given at any point, 
+this does not interfere with the current state
+of the network. Thus a new node can be included 
+whenever */
+    currentState = WAIT_FOR_RESET;
+    
+/* Start timers */    
+    runSystemChecksTimer.start();
+    runCheckHeartbeatTimer.start();
+    
+    static int systemTaskCounter, checkHeartbeatCounter;
+    
+   while(1){
+        systemTaskCounter = runSystemChecksTimer.read_ms();
+        checkHeartbeatCounter = runCheckHeartbeatTimer.read_ms();
+        
+        checkSystemStatus();
+        if ( systemTaskCounter > systemCheckTimeout){
+            DMLocalNode.process_rx_frames();
+            runSystemChecks(DMLocalNode);
+            systemTaskCounter = 0;
+            runSystemChecksTimer.reset();
+            receivedData = 0; 
+        }    
+        
+        if (checkHeartbeatCounter > heartbeatTimeout){
+            checkHeartbeat();   
+            runCheckHeartbeatTimer.reset();
+            checkHeartbeatCounter = 0;
+        }
+    }
+}
\ No newline at end of file