![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Robot_Node
Dependencies: XBeeLib_Robot mbed
main.cpp
- Committer:
- basvuyk
- Date:
- 2018-07-25
- Revision:
- 0:89afb50e974f
File content as of revision 0:89afb50e974f:
#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; } } }