turn around

src/robot.cpp

@@ -40,28 +40,37 @@ void Robot::EnterTurn(float angleInDeg)
 {
     Serial.println(" -> TURN");
     robotState = ROBOT_TURNING;
-
+    Serial.println(" -> TURN");
-    /**
+    robotState = ROBOT_TURNING;
-     * TODO: Add code to initiate the turn and set the target
+    targetDirection = (currDirection+2)%4;
-     */
+    chassis.SetTwist(0.0,0.5);
 }
 
 bool Robot::CheckTurnComplete(void)
 {
     bool retVal = false;
 
-    /**
+    if (targetDirection == currDirection) {return retVal;};
-     * TODO: add a checker to detect when the turn is complete
+
-     */
+    // unsound
+    if (lineSensor.LineDetected() && eulerAngles.z > targetDirection * 90) {
+        retVal = true;
+    }
 
     return retVal;
 }
 
 void Robot::HandleTurnComplete(void)
 {
-    /**
+    currDirection = targetDirection;
-     * TODO: Add code to handle the completed turn
+    chassis.Stop();
-     */
+
+    if (robotRampState == RAMP_ASCENDING) {
+        robotRampState = RAMP_DESCENDING;
+        Robot::EnterLineFollowing(20.);
+    } else {
+        Robot::EnterIdleState();
+    }
 }
 
 /**
@@ -131,6 +140,8 @@ int lineLostFrames = 24;
 
 float lastError = 0;
 
+int leveltimer = 0;
+
 void Robot::LineFollowingUpdate(void)
 {
     if(robotState == ROBOT_LINING) 
@@ -213,8 +224,12 @@ void Robot::LineFollowingUpdate(void)
                 leveltimer--; // count down when off ramp
                 if (leveltimer == 1) {
                     leveltimer = 0;
+                    if (robotRampState == RAMP_ASCENDING) {
+                        Robot::EnterTurn(180);
+                    } else {
                         EnterIdleState();
                     }
+                }
             } else {
                 leveltimer = 30;
             }

src/robot.h

@@ -22,6 +22,13 @@ protected:
     };
     ROBOT_CTRL_MODE robotCtrlMode = CTRL_TELEOP;
 
+    enum ROBOT_RAMP_STATE
+    {
+        RAMP_ASCENDING,
+        RAMP_DESCENDING,
+    };
+    ROBOT_RAMP_STATE robotRampState = RAMP_ASCENDING;
+
     /**
      * robotState is used to track the current task of the robot. You will add new states as 
      * the term progresses.