Modified the Robot state's variables: the method setState saves the t…

…ime stamp in lastTimeActive.

commander.cpp

@@ -35,7 +35,6 @@ Thread threadSerial(threadSerial_run, 50);
 void threadNRF_run();
 Thread threadNRF(threadNRF_run, 1000); // Starts listening after 1sec, then set itself to 5ms
 
-long lastMessageTimestamp = 0;
 
 // ====================================
 //           INITIALIZATION
@@ -201,7 +200,7 @@ void threadIddleDetection_run(){
   if(Robot::state != ACTIVE)
     return;
 
-  if(millis() - lastMessageTimestamp > RADIO_TIMEOUT_TO_IDDLE){
+  if(millis() - Robot::lastTimeActive > RADIO_TIMEOUT_TO_IDDLE){
     Robot::setState(IDDLE);
     Robot::doBeep(Robot::getRobotID(), 50);
   }
@@ -270,17 +269,17 @@ void threadNRF_run(){
   radio->send(radioBufferOut, 3);
   radio->waitPacketSent();
 
-    // Got message. Robot is now Active if message was an action message
-    if(Robot::alarm == NONE){
-      if(Robot::state == IDDLE && activate){
-        Robot::setState(ACTIVE);
-        Robot::doBeep(1, 100);
-    }
+  // Got message. Robot is now Active if message was an action message
+  if(Robot::alarm == NONE){
+    if(Robot::state == IDDLE && activate){
+      Robot::setState(ACTIVE);
+      Robot::doBeep(1, 100);
+  }
 
-      // Save timestamp of message
-      lastMessageTimestamp = millis();
-      // Serial.print(" ");
-      // Serial.print(lastMessageTimestamp - start);
-      // Serial.print(" ms\n");
+    // Save timestamp of message
+    Robot::lastTimeActive = millis();
+    // Serial.print(" ");
+    // Serial.print(lastMessageTimestamp - start);
+    // Serial.print(" ms\n");
   }
 }

robot.cpp

@@ -55,11 +55,15 @@ int Robot::getRobotID(){
 
 RobotState Robot::state = IDDLE;
 RobotAlarm Robot::alarm = NONE;
+Robot::lastTimeActive = 0;
 
+// Sets the robot's state
 void Robot::setState(RobotState _state){
   state = _state;
 
-  if(state == IDDLE)
+  if(_state == ACTIVE)
+    Robot::lastTimeActive = millis();
+  else if(state == IDDLE)
     Motors::stop();
 };
 

robot.h

@@ -32,6 +32,11 @@ class Robot{
   */
   static RobotState state;
 
+  /*
+    Last time when robot was Active
+  */
+  static unsigned long lastTimeActive;
+
   /*
     Current alarm of the robot (Errors)
   */