From 797742ecd889201cf0f071c1f4d45f38e6328717 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Tue, 12 Nov 2024 21:54:19 -0500
Subject: [PATCH] ramp states

---
 src/robot.cpp | 22 ++++++++++++++++++++++
 1 file changed, 22 insertions(+)

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