diff --git a/src/robot.cpp b/src/robot.cpp index 22401fd..80f83c9 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -40,28 +40,37 @@ void Robot::EnterTurn(float angleInDeg) { Serial.println(" -> TURN"); robotState = ROBOT_TURNING; - - /** - * TODO: Add code to initiate the turn and set the target - */ + Serial.println(" -> TURN"); + robotState = ROBOT_TURNING; + targetDirection = (currDirection+2)%4; + chassis.SetTwist(0.0,0.5); } bool Robot::CheckTurnComplete(void) { bool retVal = false; - /** - * TODO: add a checker to detect when the turn is complete - */ + if (targetDirection == currDirection) {return retVal;}; + + // unsound + if (lineSensor.LineDetected() && eulerAngles.z > targetDirection * 90) { + retVal = true; + } return retVal; } void Robot::HandleTurnComplete(void) { - /** - * TODO: Add code to handle the completed turn - */ + currDirection = targetDirection; + 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,7 +224,11 @@ void Robot::LineFollowingUpdate(void) leveltimer--; // count down when off ramp if (leveltimer == 1) { leveltimer = 0; - EnterIdleState(); + if (robotRampState == RAMP_ASCENDING) { + Robot::EnterTurn(180); + } else { + EnterIdleState(); + } } } else { leveltimer = 30; diff --git a/src/robot.h b/src/robot.h index 99def59..fdffbdf 100644 --- a/src/robot.h +++ b/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.