ramp states

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;
+            }
+        }
+
     }
 }