main.cpp

00001 #include "mbed.h"
00002 #include "XBeeLib.h"
00003 
00004 using namespace XBeeLib;
00005 /* System states */    
00006 typedef enum
00007 {
00008     BOOTING,    
00009     NORMAL_OPERATION,
00010     EMERGENCY,
00011     WAIT_FOR_RESET,
00012     HEARTBEAT_ERROR,
00013     GRACE_PERIOD,
00014 } state_t;
00015 
00016 state_t currentState = BOOTING;
00017 
00018 int receivedData, currentMessageCounter, graceCounter, currentTimerValue;
00019 int heartbeat_msg = 49, emergency_msg = 50, reset_msg = 46;
00020 int systemState = 1; // 0 = Emergency, 1 = System Ok
00021 int systemResetUsed = 0;
00022 
00023 /* Change network values */
00024 //------------DIGIMESH CONFIG-------//
00025 #define channel 0x18
00026 #define networkId 0xD164
00027 #define powerLevel 4
00028 #define nodeId "robotNode"
00029 #define baudRate 230400
00030 //------------DIGIMESH CONFIG-------//
00031 
00032 //------------PIN CONFIG------------//
00033 DigitalOut statusLED(PB_4);
00034 DigitalOut powerLED(PB_5);
00035 DigitalOut MCU_Status(PF_1);
00036 DigitalOut resetOutput(PA_11);
00037 
00038 DigitalIn localEstop(PA_12);
00039 DigitalIn safetyRelayStatus(PB_1);
00040 DigitalIn bumperStatus(PB_0);
00041 //------------PIN CONFIG------------//
00042 
00043 //-----------SPI CONFIG------------//
00044 SPI spi(PA_7, PA_6, PA_5);      // mosi, miso, sclk
00045 DigitalOut chipSelect(PA_4);    // nss
00046 #define spiSpeed 10000000
00047 //-----------SPI CONFIG------------//
00048 
00049 //-----------TIMER CONFIG----------//
00050 Timer runCheckHeartbeatTimer, runSystemChecksTimer;
00051 #define systemCheckTimeout 5    // ms   // Check system state every 5ms
00052 #define heartbeatTimeout 100    // ms   // Check heartbeat every 100ms
00053 //-----------TIMER CONFIG----------//
00054 
00055 /* If heartbeat is missed for more then 10 times, 
00056 system goes into HEARTBEAT_ERROR state.
00057 The reset relay is activated for 100*5ms to ensure
00058 the emergencystop relay is reverting to normal state.
00059 Changing this time to lower values causes the emergencystop
00060 relay to not always acknowledge the reset */
00061 //-----------SYSTEM VARIABLES------//
00062 #define hearbeatMisses 10               
00063 #define resetRelayActivationTime 100
00064 //-----------SYSTEM VARIABLES------//
00065 
00066 /* Config XBee radio. All the previously defined 
00067 values are written to the XBee module. DMLocalNode.write_config()
00068 is used to save the values to the rom of the module. */
00069 // RADIO CONFIG
00070 void radioConfig(XBeeDM &DMLocalNode){    
00071     RadioStatus temp = DMLocalNode.init();
00072     temp = DMLocalNode.set_channel(channel);
00073     temp = DMLocalNode.set_network_id(networkId);
00074     temp = DMLocalNode.set_power_level(powerLevel);
00075     temp = DMLocalNode.set_node_identifier(nodeId);
00076     temp = DMLocalNode.write_config();
00077 }
00078 
00079 /* Functions who need to run once are performed here */
00080 void boot(XBeeDM &DMLocalNode){
00081     radioConfig(DMLocalNode);
00082     statusLED.write(1);
00083     powerLED.write(1);
00084         
00085     spi.format(8,3);
00086     spi.frequency(spiSpeed);
00087 }
00088 
00089 /* If a message has been received, this function
00090 is used to retreive the message from the buffer.
00091 A substraction of 3 is used to get the right value. 
00092 This is due to some conversion being done in the library */
00093 static void receive_cb(const RemoteXBeeDM& remote, bool broadcast, const uint8_t *const data, uint16_t len){
00094     receivedData = (data[0]-3);
00095 }
00096 
00097 /* If a message is to be sent, this function
00098 is used to send the message. */
00099 static void sendMessage(XBeeDM &DMLocalNode, char *sendData){
00100     const char data[] = {*sendData};
00101     const uint16_t data_len = strlen(data);
00102 
00103     const TxStatus txStatus = DMLocalNode.send_data_broadcast((const uint8_t *)data, data_len);
00104     
00105     powerLED = !powerLED;
00106 }
00107 
00108 /* This function checks the physical E-Stop status. */
00109 void checkSystemStatus(){
00110     if (localEstop == 0 || bumperStatus == 0 ){
00111         currentState = EMERGENCY;
00112         systemState = 0;
00113     }
00114     if (localEstop == 1 && bumperStatus == 1 && currentState == EMERGENCY){
00115         currentState = WAIT_FOR_RESET;
00116         systemState = 1; 
00117     }
00118 }
00119 
00120 /* This function checks if a system state change is
00121 needed, and performs the necessary actions tied to 
00122 this state */
00123 void stateHandler(XBeeDM &DMLocalNode){        
00124     if (currentState == NORMAL_OPERATION){
00125         checkSystemStatus();
00126         MCU_Status.write(1);
00127         statusLED.write(0);        
00128         powerLED.write(1);
00129                 
00130         if (systemResetUsed != resetRelayActivationTime){
00131             resetOutput.write(1);
00132             systemResetUsed++; 
00133         }
00134         if (systemResetUsed == resetRelayActivationTime){
00135             resetOutput.write(0);
00136         }
00137     }
00138     
00139     if (currentState == EMERGENCY && currentMessageCounter != 5){
00140         sendMessage(DMLocalNode, "50");
00141         checkSystemStatus();
00142         currentMessageCounter++;
00143         
00144         MCU_Status.write(0);
00145         statusLED.write(1);
00146         powerLED.write(0);
00147         
00148         systemResetUsed = 0; 
00149     }
00150     
00151     if (currentState == WAIT_FOR_RESET){
00152         currentMessageCounter = 0;
00153         
00154         MCU_Status.write(0);
00155         statusLED.write(1);
00156         powerLED.write(0);
00157         
00158         systemResetUsed = 0;
00159     }
00160     
00161     if (currentState == HEARTBEAT_ERROR){
00162         MCU_Status.write(0);
00163         statusLED.write(0);
00164         powerLED.write(0);
00165         
00166         systemResetUsed = 0; 
00167     }
00168     
00169     if (currentState == GRACE_PERIOD){
00170         graceCounter++;
00171         if (graceCounter == 40){
00172             currentState = NORMAL_OPERATION;
00173             graceCounter = 0;
00174         }
00175     }
00176 }
00177 
00178 /* If a heartbeat message has been received
00179 currentHeartbeatValue is reset */
00180 void heartbeatTimerReset(){
00181     currentTimerValue = 0;
00182 }
00183 
00184 /* If more heartbeats are missed then allowed
00185 the system enters the HEARTBEAT_ERROR state */
00186 void checkHeartbeat(){
00187     if (currentState == NORMAL_OPERATION){
00188     currentTimerValue++;
00189         if (currentTimerValue > hearbeatMisses){
00190             currentState = HEARTBEAT_ERROR;
00191         }
00192     }   
00193 }
00194 
00195 
00196 /* Incoming message are handled here and necessary
00197 actions are taken if needed */
00198 void handleMessages(XBeeDM &DMLocalNode){
00199     if (receivedData == emergency_msg){
00200         currentState = EMERGENCY;
00201         graceCounter = 0;
00202     }
00203     
00204 /* The system enters the GRACE_PERIOD state if
00205 the previous state was WAITING_FOR_RESET and a
00206 reset message has been received. If a emergency
00207 message is received in this period, the system, 
00208 again, enters the WAIT_FOR_RESET state. This means
00209 that there are one or multiple nodes still in 
00210 EMERGENCY_STATE. Check all nodes to ensure all of
00211 them have been taken care of*/ 
00212     if (currentState == WAIT_FOR_RESET && systemState == 1 && receivedData == reset_msg){
00213         currentState = GRACE_PERIOD;
00214     }
00215     
00216     if (receivedData == heartbeat_msg){
00217         heartbeatTimerReset();
00218     }
00219     
00220     if (receivedData == heartbeat_msg && currentState == HEARTBEAT_ERROR){
00221         currentState = WAIT_FOR_RESET;
00222         heartbeatTimerReset();
00223     }
00224     
00225     if (currentState == EMERGENCY && currentMessageCounter == 5 && receivedData == reset_msg){
00226         currentMessageCounter = 0;
00227     }
00228 }
00229 
00230 // Handle SPI messages
00231 void handleSPI(){
00232     chipSelect = 0; // Select device
00233     spi.write(systemState);
00234     chipSelect = 1; // Deselect device
00235 }
00236 
00237 void runSystemChecks(XBeeDM &DMLocalNode){
00238     stateHandler(DMLocalNode);
00239     handleMessages(DMLocalNode);
00240     handleSPI();
00241 }
00242 
00243 int main() {
00244 /* The creation of the DMLocalNode */
00245     XBeeDM DMLocalNode = XBeeDM(RADIO_TX, RADIO_RX, RADIO_RESET, NC, NC, baudRate);
00246  
00247  /* Perform boot functions and create the callback
00248 for the received messages */   
00249     boot(DMLocalNode);
00250     DMLocalNode.register_receive_cb(&receive_cb);
00251     
00252 /* Upon boot the system waits to be included in 
00253 the network. A reset can be given at any point, 
00254 this does not interfere with the current state
00255 of the network. Thus a new node can be included 
00256 whenever */
00257     currentState = WAIT_FOR_RESET;
00258     
00259 /* Start timers */    
00260     runSystemChecksTimer.start();
00261     runCheckHeartbeatTimer.start();
00262     
00263     static int systemTaskCounter, checkHeartbeatCounter;
00264     
00265    while(1){
00266         systemTaskCounter = runSystemChecksTimer.read_ms();
00267         checkHeartbeatCounter = runCheckHeartbeatTimer.read_ms();
00268         
00269         checkSystemStatus();
00270         if ( systemTaskCounter > systemCheckTimeout){
00271             DMLocalNode.process_rx_frames();
00272             runSystemChecks(DMLocalNode);
00273             systemTaskCounter = 0;
00274             runSystemChecksTimer.reset();
00275             receivedData = 0; 
00276         }    
00277         
00278         if (checkHeartbeatCounter > heartbeatTimeout){
00279             checkHeartbeat();   
00280             runCheckHeartbeatTimer.reset();
00281             checkHeartbeatCounter = 0;
00282         }
00283     }
00284 }