diff --git a/src/robot.cpp b/src/robot.cpp index 6f1061a..22401fd 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -198,6 +198,28 @@ void Robot::LineFollowingUpdate(void) } chassis.SetTwist(speed, turnEffort); + + if (eulerAngles.x > 10.) { + Serial.println("climbing"); + } else if (eulerAngles.x <= 10.) { + Serial.println("level"); + } + + if (eulerAngles.x > 10.) { // activate timer when on ramp + leveltimer = 30; + } + if (leveltimer > 0) { + if (eulerAngles.x <= 5.) { + leveltimer--; // count down when off ramp + if (leveltimer == 1) { + leveltimer = 0; + EnterIdleState(); + } + } else { + leveltimer = 30; + } + } + } }