changement startup procedure, plus safe

INTechSenpai · May 24, 2017 · a621e6a · a621e6a
1 parent 4861630
commit a621e6a
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 25 deletions.
diff --git a/teensy/loli_teensy/LedMgr.h b/teensy/loli_teensy/LedMgr.h
@@ -16,7 +16,7 @@ class LedMgr : public Singleton<LedMgr>
 public:
 	LedMgr():
 		blink_s1(PIN_DEL_STATUS_1, 300),
-		blink_s2(PIN_DEL_STATUS_2, 300),
+		blink_s2(PIN_DEL_STATUS_2, 900, 100),
 		blink_cd(PIN_CLIGNOTANT_D, 500),
 		blink_cg(PIN_CLIGNOTANT_G, 500),
 		blink_ff(PIN_FEUX_FREIN, 100),

diff --git a/teensy/loli_teensy/OrderLong.h b/teensy/loli_teensy/OrderLong.h
@@ -193,28 +193,43 @@ class WaitForJumper : public OrderLong, public Singleton<WaitForJumper>
 	void _launch(const std::vector<uint8_t> & input)
 	{
 		jumperInPlace = false;
+		pulledStarted = false;
 		Serial.println("Wait for jumper");
 	}
 	void onExecute(std::vector<uint8_t> & output)
 	{
-		if (analogRead(PIN_GET_JUMPER) > 750 && startupMgr.isReady())
-		{// Jumper en place
+		bool jumperInserted = analogRead(PIN_GET_JUMPER) > 500;
+		if (jumperInserted && !jumperInPlace)
+		{
+			Serial.println("Jumper in place");
 			jumperInPlace = true;
+			startupMgr.jumperPluggedIn();
 		}
-		else
-		{// Jumper sorti
-			if (jumperInPlace)
+		else if(!jumperInserted && jumperInPlace)
+		{
+			if (!pulledStarted)
+			{
+				pulledStarted = true;
+				pulledTime = millis();
+			}
+			else if (millis() - pulledTime > 300)
 			{
 				finished = true;
 			}
 		}
+		else if (jumperInserted)
+		{
+			pulledStarted = false;
+		}
 	}
 	void terminate(std::vector<uint8_t> & output)
 	{
 		Serial.println("Jumper pulled");
 	}
 private:
 	bool jumperInPlace;
+	uint32_t pulledTime;
+	bool pulledStarted;
 };
 
 

diff --git a/teensy/loli_teensy/StartupMgr.h b/teensy/loli_teensy/StartupMgr.h
@@ -23,6 +23,7 @@ class StartupMgr : public Singleton<StartupMgr>
 		state = WAIT_FOR_HL;
 		side = UNKNOWN;
 		hlAlive = false;
+		jumperInside = false;
 	}
 
 	enum Side
@@ -49,37 +50,26 @@ class StartupMgr : public Singleton<StartupMgr>
 			else if (state == WAIT_FOR_COLOR_SETUP)
 			{
 				ledMgr.statusLed_doubleBlink();
-				if (analogRead(PIN_GET_JUMPER) > 750)
+				if (jumperInside)
 				{// Jumper inséré
-					state = SIGNAL_SETUP_END;
-					setupEnd_startTime = millis();
-				}
-			}
-			else if (state == SIGNAL_SETUP_END)
-			{
-				ledMgr.statusLed_greenBlink();
-				if (analogRead(PIN_GET_JUMPER) < 750)
-				{// Jumper retiré
-					state = WAIT_FOR_COLOR_SETUP;
-				}
-				else if (millis() - setupEnd_startTime > 2000)
-				{
 					state = READY;
-					if (analogRead(PIN_GET_COLOR) > 750)
+					if (analogRead(PIN_GET_COLOR) > 500)
 					{
+						Serial.println("Color: BLUE");
 						side = BLUE;
 						motionControlSystem.setPosition(Position(X_BLEU, Y_BLEU, O_BLEU));
 					}
 					else
 					{
+						Serial.println("Color: YELLOW");
 						side = YELLOW;
 						motionControlSystem.setPosition(Position(X_JAUNE, Y_JAUNE, O_JAUNE));
 					}
 				}
 			}
 			else
 			{
-				ledMgr.statusLed_off();
+				ledMgr.statusLed_greenBlink();
 			}
 		}
 	}
@@ -89,9 +79,9 @@ class StartupMgr : public Singleton<StartupMgr>
 		return side;
 	}
 
-	bool isReady()
+	void jumperPluggedIn()
 	{
-		return state == READY;
+		jumperInside = true;
 	}
 
 	void hlIsAlive()
@@ -104,14 +94,14 @@ class StartupMgr : public Singleton<StartupMgr>
 	{
 		WAIT_FOR_HL,
 		WAIT_FOR_COLOR_SETUP,
-		SIGNAL_SETUP_END,
 		READY
 	};
 
 	Side side;
 	State state;
 
 	bool hlAlive;
+	bool jumperInside;
 
 	uint32_t setupEnd_startTime; //ms