Robot_Node
Dependencies: XBeeLib_Robot mbed
Diff: main.cpp
- Revision:
- 0:89afb50e974f
diff -r 000000000000 -r 89afb50e974f main.cpp --- /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