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;
        }
    }
}