diff --git a/teensy/loli_teensy/OrderLong.h b/teensy/loli_teensy/OrderLong.h index 82c9d5a9..806ef38a 100644 --- a/teensy/loli_teensy/OrderLong.h +++ b/teensy/loli_teensy/OrderLong.h @@ -231,13 +231,22 @@ class StartMatchChrono : public OrderLong, public Singleton { Serial.println("Start match chrono"); beginTime = millis(); + matchFinished = false; } void onExecute(std::vector & output) { - if (millis() - beginTime > 90000) + if (millis() - beginTime > 90000 && !matchFinished) { - returnStatement = 0x00; // MATCH_FINISHED - finished = true; + matchFinished = true; + actuatorMgr.funnyAction(true); + } + else if (matchFinished) + { + if (actuatorMgr.funnyAction(false) != RUNNING) + { + finished = true; + returnStatement = 0x00; // MATCH + FUNNY_ACTION FINISHED + } } } void terminate(std::vector & output) @@ -248,6 +257,7 @@ class StartMatchChrono : public OrderLong, public Singleton private: uint32_t beginTime; uint8_t returnStatement; + bool matchFinished; };