diff --git a/IO.hpp b/IO.hpp index eb15e4f..92bf4d8 100644 --- a/IO.hpp +++ b/IO.hpp @@ -67,7 +67,7 @@ class IO { pinMode(PIN_TIRETTE, INPUT_PULLUP); pinMode(PIN_BUTTON_COLOR, INPUT_PULLUP); - m_code_cell->Motion_RotationRead(m_init_x, m_init_y, m_init_z); + initGyroscope(); m_initMotors(); m_initServo(); @@ -84,6 +84,11 @@ class IO { return nb_errors; } + void initGyroscope() + { + m_code_cell->Motion_RotationRead(m_init_x, m_init_y, m_init_z); + } + int update() { static unsigned long last_color_change_time {0}; @@ -122,7 +127,7 @@ class IO { m_is_tirette_pulled = (digitalRead(PIN_TIRETTE) == HIGH); m_is_color_blue = is_color_blue; - is_color_blue = (digitalRead(PIN_BUTTON_COLOR) == HIGH); + is_color_blue = (digitalRead(PIN_BUTTON_COLOR) == LOW); if(m_is_motor_control_activated) { diff --git a/Main.ino b/Main.ino index 11d52ee..0c613e6 100644 --- a/Main.ino +++ b/Main.ino @@ -108,9 +108,10 @@ void loop() case State::WaitingTimer: static unsigned long initial_time = millis(); - //if(millis() - initial_time >= 90000) - if(millis() - initial_time >= 2000) + if(millis() - initial_time >= 90000) + //if(millis() - initial_time >= 2000) { + my_IO.initGyroscope(); actual_state = State::ForwardToRamp; } break; @@ -136,15 +137,6 @@ void loop() break; case State::Turn90Blue: - my_IO.setDirWithAngularSpeed(-90.0f); - - if(my_IO.getAngle(Axes::Z) < -80.0f) - { - actual_state = State::ForwardToSceneEdge; - } - break; - - case State::Turn90Yellow: my_IO.setDirWithAngularSpeed(90.0f); if(my_IO.getAngle(Axes::Z) >= 80.0f) @@ -153,8 +145,17 @@ void loop() } break; + case State::Turn90Yellow: + my_IO.setDirWithAngularSpeed(-90.0f); + + if(my_IO.getAngle(Axes::Z) < -80.0f) + { + actual_state = State::ForwardToSceneEdge; + } + break; + case State::ForwardToSceneEdge: - my_IO.setSpeed(1100.0f); + my_IO.setSpeed(1200.0f); if(my_IO.getAngle(Axes::Y) > 5.0f) {