Merge pull request #199 from cwruRobotics/float-comp-fixes

Float comp fixes

src/float/float_transceiver/float_transceiver.ino

@@ -34,14 +34,15 @@ const uint32_t PRESSURE_READ_INTERVAL = 5000;
 const uint32_t PROFILE_SEGMENT = 60000;
 #endif
 
+const uint32_t ONE_MINUTE = 60000;
+
 // Schedule (all delays in ms)
-const uint32_t RELEASE_MAX = 300000;
+const uint32_t RELEASE_MAX = 8 * ONE_MINUTE;
 const uint32_t SUCK_MAX = PROFILE_SEGMENT;
 const uint32_t DESCEND_TIME = PROFILE_SEGMENT;
 const uint32_t PUMP_MAX = PROFILE_SEGMENT;
 const uint32_t ASCEND_TIME = 0;  // Disable ascend times now that we're properly ballasted
-const uint32_t TX_MAX = 60000;
-const uint32_t ONE_HOUR = 360000;
+const uint32_t TX_MAX_TIME = 2 * ONE_MINUTE;
 
 const size_t SCHEDULE_LENGTH = 12;
 
@@ -57,7 +58,7 @@ struct Stage {
 OverrideState overrideState = OverrideState::NoOverride;
 uint8_t currentStage = 0;
 
-Stage SCHEDULE[SCHEDULE_LENGTH] = {
+const Stage SCHEDULE[SCHEDULE_LENGTH] = {
   // Pump immediately in case we just rebooted at the bottom of the pool
   {StageType::Pump,             PUMP_MAX    },
 
@@ -70,15 +71,15 @@ Stage SCHEDULE[SCHEDULE_LENGTH] = {
   {StageType::Pump,             PUMP_MAX    },
   {StageType::WaitProfiling,    ASCEND_TIME },
 
-  {StageType::WaitTransmitting, TX_MAX      },
+  {StageType::WaitTransmitting, TX_MAX_TIME },
 
  // Profile 2
   {StageType::Suck,             SUCK_MAX    },
   {StageType::WaitProfiling,    DESCEND_TIME},
   {StageType::Pump,             PUMP_MAX    },
   {StageType::WaitProfiling,    ASCEND_TIME },
 
-  {StageType::WaitTransmitting, ONE_HOUR    },
+  {StageType::WaitTransmitting, TX_MAX_TIME },
 };
 
 uint32_t stageStartTime;
@@ -138,7 +139,13 @@ void setup() {
 }
 
 void loop() {
-  bool submergeReceived = receiveCommand();
+  bool submergeReceived = false;
+
+  // Disable command Rx when the motor is moving
+  // Otherwise the motor can overrun the limit switch
+  if (!isMotorMoving()) {
+    submergeReceived = receiveCommand();
+  }
 
   if (overrideState == OverrideState::Suck) {
     if (digitalRead(LIMIT_FULL) == HIGH) {
@@ -252,7 +259,7 @@ void loop() {
 
     // If we signal a third profile, restart the schedule
     if (currentStage >= SCHEDULE_LENGTH) {
-      currentStage = 1;
+      currentStage = 2;
     }
 
     // Switch to sucking/pumping depending on the stage we're entering
@@ -391,6 +398,11 @@ MotorState getMotorState() {
   }
 }
 
+bool isMotorMoving() {
+  const MotorState motorState = getMotorState();
+  return motorState == MotorState::Pump || motorState == MotorState::Suck;
+}
+
 bool stageIs(StageType type) { return SCHEDULE[currentStage].type == type; }
 
 bool isSurfaced() {