Bas Vuyk / Mbed 2 deprecated Z_Robot_Node_copy

Robot_Node

Dependencies:   XBeeLib_Robot mbed

main.cpp@0:89afb50e974f, 2018-07-25 (annotated)

Committer:
basvuyk
Date:
Wed Jul 25 09:13:38 2018 +0000
Revision:
0:89afb50e974f
Robot_Node;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
basvuyk 0:89afb50e974f 1 #include "mbed.h"
basvuyk 0:89afb50e974f 2 #include "XBeeLib.h"
basvuyk 0:89afb50e974f 3
basvuyk 0:89afb50e974f 4 using namespace XBeeLib;
basvuyk 0:89afb50e974f 5 /* System states */
basvuyk 0:89afb50e974f 6 typedef enum
basvuyk 0:89afb50e974f 7 {
basvuyk 0:89afb50e974f 8 BOOTING,
basvuyk 0:89afb50e974f 9 NORMAL_OPERATION,
basvuyk 0:89afb50e974f 10 EMERGENCY,
basvuyk 0:89afb50e974f 11 WAIT_FOR_RESET,
basvuyk 0:89afb50e974f 12 HEARTBEAT_ERROR,
basvuyk 0:89afb50e974f 13 GRACE_PERIOD,
basvuyk 0:89afb50e974f 14 } state_t;
basvuyk 0:89afb50e974f 15
basvuyk 0:89afb50e974f 16 state_t currentState = BOOTING;
basvuyk 0:89afb50e974f 17
basvuyk 0:89afb50e974f 18 int receivedData, currentMessageCounter, graceCounter, currentTimerValue;
basvuyk 0:89afb50e974f 19 int heartbeat_msg = 49, emergency_msg = 50, reset_msg = 46;
basvuyk 0:89afb50e974f 20 int systemState = 1; // 0 = Emergency, 1 = System Ok
basvuyk 0:89afb50e974f 21 int systemResetUsed = 0;
basvuyk 0:89afb50e974f 22
basvuyk 0:89afb50e974f 23 /* Change network values */
basvuyk 0:89afb50e974f 24 //------------DIGIMESH CONFIG-------//
basvuyk 0:89afb50e974f 25 #define channel 0x18
basvuyk 0:89afb50e974f 26 #define networkId 0xD164
basvuyk 0:89afb50e974f 27 #define powerLevel 4
basvuyk 0:89afb50e974f 28 #define nodeId "robotNode"
basvuyk 0:89afb50e974f 29 #define baudRate 230400
basvuyk 0:89afb50e974f 30 //------------DIGIMESH CONFIG-------//
basvuyk 0:89afb50e974f 31
basvuyk 0:89afb50e974f 32 //------------PIN CONFIG------------//
basvuyk 0:89afb50e974f 33 DigitalOut statusLED(PB_4);
basvuyk 0:89afb50e974f 34 DigitalOut powerLED(PB_5);
basvuyk 0:89afb50e974f 35 DigitalOut MCU_Status(PF_1);
basvuyk 0:89afb50e974f 36 DigitalOut resetOutput(PA_11);
basvuyk 0:89afb50e974f 37
basvuyk 0:89afb50e974f 38 DigitalIn localEstop(PA_12);
basvuyk 0:89afb50e974f 39 DigitalIn safetyRelayStatus(PB_1);
basvuyk 0:89afb50e974f 40 DigitalIn bumperStatus(PB_0);
basvuyk 0:89afb50e974f 41 //------------PIN CONFIG------------//
basvuyk 0:89afb50e974f 42
basvuyk 0:89afb50e974f 43 //-----------SPI CONFIG------------//
basvuyk 0:89afb50e974f 44 SPI spi(PA_7, PA_6, PA_5); // mosi, miso, sclk
basvuyk 0:89afb50e974f 45 DigitalOut chipSelect(PA_4); // nss
basvuyk 0:89afb50e974f 46 #define spiSpeed 10000000
basvuyk 0:89afb50e974f 47 //-----------SPI CONFIG------------//
basvuyk 0:89afb50e974f 48
basvuyk 0:89afb50e974f 49 //-----------TIMER CONFIG----------//
basvuyk 0:89afb50e974f 50 Timer runCheckHeartbeatTimer, runSystemChecksTimer;
basvuyk 0:89afb50e974f 51 #define systemCheckTimeout 5 // ms // Check system state every 5ms
basvuyk 0:89afb50e974f 52 #define heartbeatTimeout 100 // ms // Check heartbeat every 100ms
basvuyk 0:89afb50e974f 53 //-----------TIMER CONFIG----------//
basvuyk 0:89afb50e974f 54
basvuyk 0:89afb50e974f 55 /* If heartbeat is missed for more then 10 times,
basvuyk 0:89afb50e974f 56 system goes into HEARTBEAT_ERROR state.
basvuyk 0:89afb50e974f 57 The reset relay is activated for 100*5ms to ensure
basvuyk 0:89afb50e974f 58 the emergencystop relay is reverting to normal state.
basvuyk 0:89afb50e974f 59 Changing this time to lower values causes the emergencystop
basvuyk 0:89afb50e974f 60 relay to not always acknowledge the reset */
basvuyk 0:89afb50e974f 61 //-----------SYSTEM VARIABLES------//
basvuyk 0:89afb50e974f 62 #define hearbeatMisses 10
basvuyk 0:89afb50e974f 63 #define resetRelayActivationTime 100
basvuyk 0:89afb50e974f 64 //-----------SYSTEM VARIABLES------//
basvuyk 0:89afb50e974f 65
basvuyk 0:89afb50e974f 66 /* Config XBee radio. All the previously defined
basvuyk 0:89afb50e974f 67 values are written to the XBee module. DMLocalNode.write_config()
basvuyk 0:89afb50e974f 68 is used to save the values to the rom of the module. */
basvuyk 0:89afb50e974f 69 // RADIO CONFIG
basvuyk 0:89afb50e974f 70 void radioConfig(XBeeDM &DMLocalNode){
basvuyk 0:89afb50e974f 71 RadioStatus temp = DMLocalNode.init();
basvuyk 0:89afb50e974f 72 temp = DMLocalNode.set_channel(channel);
basvuyk 0:89afb50e974f 73 temp = DMLocalNode.set_network_id(networkId);
basvuyk 0:89afb50e974f 74 temp = DMLocalNode.set_power_level(powerLevel);
basvuyk 0:89afb50e974f 75 temp = DMLocalNode.set_node_identifier(nodeId);
basvuyk 0:89afb50e974f 76 temp = DMLocalNode.write_config();
basvuyk 0:89afb50e974f 77 }
basvuyk 0:89afb50e974f 78
basvuyk 0:89afb50e974f 79 /* Functions who need to run once are performed here */
basvuyk 0:89afb50e974f 80 void boot(XBeeDM &DMLocalNode){
basvuyk 0:89afb50e974f 81 radioConfig(DMLocalNode);
basvuyk 0:89afb50e974f 82 statusLED.write(1);
basvuyk 0:89afb50e974f 83 powerLED.write(1);
basvuyk 0:89afb50e974f 84
basvuyk 0:89afb50e974f 85 spi.format(8,3);
basvuyk 0:89afb50e974f 86 spi.frequency(spiSpeed);
basvuyk 0:89afb50e974f 87 }
basvuyk 0:89afb50e974f 88
basvuyk 0:89afb50e974f 89 /* If a message has been received, this function
basvuyk 0:89afb50e974f 90 is used to retreive the message from the buffer.
basvuyk 0:89afb50e974f 91 A substraction of 3 is used to get the right value.
basvuyk 0:89afb50e974f 92 This is due to some conversion being done in the library */
basvuyk 0:89afb50e974f 93 static void receive_cb(const RemoteXBeeDM& remote, bool broadcast, const uint8_t *const data, uint16_t len){
basvuyk 0:89afb50e974f 94 receivedData = (data[0]-3);
basvuyk 0:89afb50e974f 95 }
basvuyk 0:89afb50e974f 96
basvuyk 0:89afb50e974f 97 /* If a message is to be sent, this function
basvuyk 0:89afb50e974f 98 is used to send the message. */
basvuyk 0:89afb50e974f 99 static void sendMessage(XBeeDM &DMLocalNode, char *sendData){
basvuyk 0:89afb50e974f 100 const char data[] = {*sendData};
basvuyk 0:89afb50e974f 101 const uint16_t data_len = strlen(data);
basvuyk 0:89afb50e974f 102
basvuyk 0:89afb50e974f 103 const TxStatus txStatus = DMLocalNode.send_data_broadcast((const uint8_t *)data, data_len);
basvuyk 0:89afb50e974f 104
basvuyk 0:89afb50e974f 105 powerLED = !powerLED;
basvuyk 0:89afb50e974f 106 }
basvuyk 0:89afb50e974f 107
basvuyk 0:89afb50e974f 108 /* This function checks the physical E-Stop status. */
basvuyk 0:89afb50e974f 109 void checkSystemStatus(){
basvuyk 0:89afb50e974f 110 if (localEstop == 0 || bumperStatus == 0 ){
basvuyk 0:89afb50e974f 111 currentState = EMERGENCY;
basvuyk 0:89afb50e974f 112 systemState = 0;
basvuyk 0:89afb50e974f 113 }
basvuyk 0:89afb50e974f 114 if (localEstop == 1 && bumperStatus == 1 && currentState == EMERGENCY){
basvuyk 0:89afb50e974f 115 currentState = WAIT_FOR_RESET;
basvuyk 0:89afb50e974f 116 systemState = 1;
basvuyk 0:89afb50e974f 117 }
basvuyk 0:89afb50e974f 118 }
basvuyk 0:89afb50e974f 119
basvuyk 0:89afb50e974f 120 /* This function checks if a system state change is
basvuyk 0:89afb50e974f 121 needed, and performs the necessary actions tied to
basvuyk 0:89afb50e974f 122 this state */
basvuyk 0:89afb50e974f 123 void stateHandler(XBeeDM &DMLocalNode){
basvuyk 0:89afb50e974f 124 if (currentState == NORMAL_OPERATION){
basvuyk 0:89afb50e974f 125 checkSystemStatus();
basvuyk 0:89afb50e974f 126 MCU_Status.write(1);
basvuyk 0:89afb50e974f 127 statusLED.write(0);
basvuyk 0:89afb50e974f 128 powerLED.write(1);
basvuyk 0:89afb50e974f 129
basvuyk 0:89afb50e974f 130 if (systemResetUsed != resetRelayActivationTime){
basvuyk 0:89afb50e974f 131 resetOutput.write(1);
basvuyk 0:89afb50e974f 132 systemResetUsed++;
basvuyk 0:89afb50e974f 133 }
basvuyk 0:89afb50e974f 134 if (systemResetUsed == resetRelayActivationTime){
basvuyk 0:89afb50e974f 135 resetOutput.write(0);
basvuyk 0:89afb50e974f 136 }
basvuyk 0:89afb50e974f 137 }
basvuyk 0:89afb50e974f 138
basvuyk 0:89afb50e974f 139 if (currentState == EMERGENCY && currentMessageCounter != 5){
basvuyk 0:89afb50e974f 140 sendMessage(DMLocalNode, "50");
basvuyk 0:89afb50e974f 141 checkSystemStatus();
basvuyk 0:89afb50e974f 142 currentMessageCounter++;
basvuyk 0:89afb50e974f 143
basvuyk 0:89afb50e974f 144 MCU_Status.write(0);
basvuyk 0:89afb50e974f 145 statusLED.write(1);
basvuyk 0:89afb50e974f 146 powerLED.write(0);
basvuyk 0:89afb50e974f 147
basvuyk 0:89afb50e974f 148 systemResetUsed = 0;
basvuyk 0:89afb50e974f 149 }
basvuyk 0:89afb50e974f 150
basvuyk 0:89afb50e974f 151 if (currentState == WAIT_FOR_RESET){
basvuyk 0:89afb50e974f 152 currentMessageCounter = 0;
basvuyk 0:89afb50e974f 153
basvuyk 0:89afb50e974f 154 MCU_Status.write(0);
basvuyk 0:89afb50e974f 155 statusLED.write(1);
basvuyk 0:89afb50e974f 156 powerLED.write(0);
basvuyk 0:89afb50e974f 157
basvuyk 0:89afb50e974f 158 systemResetUsed = 0;
basvuyk 0:89afb50e974f 159 }
basvuyk 0:89afb50e974f 160
basvuyk 0:89afb50e974f 161 if (currentState == HEARTBEAT_ERROR){
basvuyk 0:89afb50e974f 162 MCU_Status.write(0);
basvuyk 0:89afb50e974f 163 statusLED.write(0);
basvuyk 0:89afb50e974f 164 powerLED.write(0);
basvuyk 0:89afb50e974f 165
basvuyk 0:89afb50e974f 166 systemResetUsed = 0;
basvuyk 0:89afb50e974f 167 }
basvuyk 0:89afb50e974f 168
basvuyk 0:89afb50e974f 169 if (currentState == GRACE_PERIOD){
basvuyk 0:89afb50e974f 170 graceCounter++;
basvuyk 0:89afb50e974f 171 if (graceCounter == 40){
basvuyk 0:89afb50e974f 172 currentState = NORMAL_OPERATION;
basvuyk 0:89afb50e974f 173 graceCounter = 0;
basvuyk 0:89afb50e974f 174 }
basvuyk 0:89afb50e974f 175 }
basvuyk 0:89afb50e974f 176 }
basvuyk 0:89afb50e974f 177
basvuyk 0:89afb50e974f 178 /* If a heartbeat message has been received
basvuyk 0:89afb50e974f 179 currentHeartbeatValue is reset */
basvuyk 0:89afb50e974f 180 void heartbeatTimerReset(){
basvuyk 0:89afb50e974f 181 currentTimerValue = 0;
basvuyk 0:89afb50e974f 182 }
basvuyk 0:89afb50e974f 183
basvuyk 0:89afb50e974f 184 /* If more heartbeats are missed then allowed
basvuyk 0:89afb50e974f 185 the system enters the HEARTBEAT_ERROR state */
basvuyk 0:89afb50e974f 186 void checkHeartbeat(){
basvuyk 0:89afb50e974f 187 if (currentState == NORMAL_OPERATION){
basvuyk 0:89afb50e974f 188 currentTimerValue++;
basvuyk 0:89afb50e974f 189 if (currentTimerValue > hearbeatMisses){
basvuyk 0:89afb50e974f 190 currentState = HEARTBEAT_ERROR;
basvuyk 0:89afb50e974f 191 }
basvuyk 0:89afb50e974f 192 }
basvuyk 0:89afb50e974f 193 }
basvuyk 0:89afb50e974f 194
basvuyk 0:89afb50e974f 195
basvuyk 0:89afb50e974f 196 /* Incoming message are handled here and necessary
basvuyk 0:89afb50e974f 197 actions are taken if needed */
basvuyk 0:89afb50e974f 198 void handleMessages(XBeeDM &DMLocalNode){
basvuyk 0:89afb50e974f 199 if (receivedData == emergency_msg){
basvuyk 0:89afb50e974f 200 currentState = EMERGENCY;
basvuyk 0:89afb50e974f 201 graceCounter = 0;
basvuyk 0:89afb50e974f 202 }
basvuyk 0:89afb50e974f 203
basvuyk 0:89afb50e974f 204 /* The system enters the GRACE_PERIOD state if
basvuyk 0:89afb50e974f 205 the previous state was WAITING_FOR_RESET and a
basvuyk 0:89afb50e974f 206 reset message has been received. If a emergency
basvuyk 0:89afb50e974f 207 message is received in this period, the system,
basvuyk 0:89afb50e974f 208 again, enters the WAIT_FOR_RESET state. This means
basvuyk 0:89afb50e974f 209 that there are one or multiple nodes still in
basvuyk 0:89afb50e974f 210 EMERGENCY_STATE. Check all nodes to ensure all of
basvuyk 0:89afb50e974f 211 them have been taken care of*/
basvuyk 0:89afb50e974f 212 if (currentState == WAIT_FOR_RESET && systemState == 1 && receivedData == reset_msg){
basvuyk 0:89afb50e974f 213 currentState = GRACE_PERIOD;
basvuyk 0:89afb50e974f 214 }
basvuyk 0:89afb50e974f 215
basvuyk 0:89afb50e974f 216 if (receivedData == heartbeat_msg){
basvuyk 0:89afb50e974f 217 heartbeatTimerReset();
basvuyk 0:89afb50e974f 218 }
basvuyk 0:89afb50e974f 219
basvuyk 0:89afb50e974f 220 if (receivedData == heartbeat_msg && currentState == HEARTBEAT_ERROR){
basvuyk 0:89afb50e974f 221 currentState = WAIT_FOR_RESET;
basvuyk 0:89afb50e974f 222 heartbeatTimerReset();
basvuyk 0:89afb50e974f 223 }
basvuyk 0:89afb50e974f 224
basvuyk 0:89afb50e974f 225 if (currentState == EMERGENCY && currentMessageCounter == 5 && receivedData == reset_msg){
basvuyk 0:89afb50e974f 226 currentMessageCounter = 0;
basvuyk 0:89afb50e974f 227 }
basvuyk 0:89afb50e974f 228 }
basvuyk 0:89afb50e974f 229
basvuyk 0:89afb50e974f 230 // Handle SPI messages
basvuyk 0:89afb50e974f 231 void handleSPI(){
basvuyk 0:89afb50e974f 232 chipSelect = 0; // Select device
basvuyk 0:89afb50e974f 233 spi.write(systemState);
basvuyk 0:89afb50e974f 234 chipSelect = 1; // Deselect device
basvuyk 0:89afb50e974f 235 }
basvuyk 0:89afb50e974f 236
basvuyk 0:89afb50e974f 237 void runSystemChecks(XBeeDM &DMLocalNode){
basvuyk 0:89afb50e974f 238 stateHandler(DMLocalNode);
basvuyk 0:89afb50e974f 239 handleMessages(DMLocalNode);
basvuyk 0:89afb50e974f 240 handleSPI();
basvuyk 0:89afb50e974f 241 }
basvuyk 0:89afb50e974f 242
basvuyk 0:89afb50e974f 243 int main() {
basvuyk 0:89afb50e974f 244 /* The creation of the DMLocalNode */
basvuyk 0:89afb50e974f 245 XBeeDM DMLocalNode = XBeeDM(RADIO_TX, RADIO_RX, RADIO_RESET, NC, NC, baudRate);
basvuyk 0:89afb50e974f 246
basvuyk 0:89afb50e974f 247 /* Perform boot functions and create the callback
basvuyk 0:89afb50e974f 248 for the received messages */
basvuyk 0:89afb50e974f 249 boot(DMLocalNode);
basvuyk 0:89afb50e974f 250 DMLocalNode.register_receive_cb(&receive_cb);
basvuyk 0:89afb50e974f 251
basvuyk 0:89afb50e974f 252 /* Upon boot the system waits to be included in
basvuyk 0:89afb50e974f 253 the network. A reset can be given at any point,
basvuyk 0:89afb50e974f 254 this does not interfere with the current state
basvuyk 0:89afb50e974f 255 of the network. Thus a new node can be included
basvuyk 0:89afb50e974f 256 whenever */
basvuyk 0:89afb50e974f 257 currentState = WAIT_FOR_RESET;
basvuyk 0:89afb50e974f 258
basvuyk 0:89afb50e974f 259 /* Start timers */
basvuyk 0:89afb50e974f 260 runSystemChecksTimer.start();
basvuyk 0:89afb50e974f 261 runCheckHeartbeatTimer.start();
basvuyk 0:89afb50e974f 262
basvuyk 0:89afb50e974f 263 static int systemTaskCounter, checkHeartbeatCounter;
basvuyk 0:89afb50e974f 264
basvuyk 0:89afb50e974f 265 while(1){
basvuyk 0:89afb50e974f 266 systemTaskCounter = runSystemChecksTimer.read_ms();
basvuyk 0:89afb50e974f 267 checkHeartbeatCounter = runCheckHeartbeatTimer.read_ms();
basvuyk 0:89afb50e974f 268
basvuyk 0:89afb50e974f 269 checkSystemStatus();
basvuyk 0:89afb50e974f 270 if ( systemTaskCounter > systemCheckTimeout){
basvuyk 0:89afb50e974f 271 DMLocalNode.process_rx_frames();
basvuyk 0:89afb50e974f 272 runSystemChecks(DMLocalNode);
basvuyk 0:89afb50e974f 273 systemTaskCounter = 0;
basvuyk 0:89afb50e974f 274 runSystemChecksTimer.reset();
basvuyk 0:89afb50e974f 275 receivedData = 0;
basvuyk 0:89afb50e974f 276 }
basvuyk 0:89afb50e974f 277
basvuyk 0:89afb50e974f 278 if (checkHeartbeatCounter > heartbeatTimeout){
basvuyk 0:89afb50e974f 279 checkHeartbeat();
basvuyk 0:89afb50e974f 280 runCheckHeartbeatTimer.reset();
basvuyk 0:89afb50e974f 281 checkHeartbeatCounter = 0;
basvuyk 0:89afb50e974f 282 }
basvuyk 0:89afb50e974f 283 }
basvuyk 0:89afb50e974f 284 }