diff --git a/src/robot-remote.cpp b/src/robot-remote.cpp index 906ceb2..ca1e576 100644 --- a/src/robot-remote.cpp +++ b/src/robot-remote.cpp @@ -95,6 +95,29 @@ void Robot::HandleKeyCode(int16_t keyCode) case ENTER_SAVE: chassis.SetTwist(0, 0); break; + case REWIND: + servo.setTargetPos(keyString.toInt()); + Serial.println(keyString); + keyString = ""; + break; + case NUM_1: + case NUM_2: + case NUM_3: + keyString += (char)(keyCode + 33); + break; + case NUM_4: + case NUM_5: + case NUM_6: + keyString += (char)(keyCode + 32); + break; + case NUM_7: + case NUM_8: + case NUM_9: + keyString += (char)(keyCode + 31); + break; + case NUM_0_10: + keyString += '0'; + break; } } diff --git a/src/robot.cpp b/src/robot.cpp index 80f83c9..3d78682 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -23,6 +23,8 @@ void Robot::InitializeRobot(void) // The line sensor elements default to INPUTs, but we'll initialize anyways, for completeness lineSensor.Initialize(); + + servo.attach(); } void Robot::EnterIdleState(void) @@ -303,6 +305,8 @@ void Robot::RobotLoop(void) * and asynchronous events (IR presses, distance readings, etc.). */ + servo.update(); + /** * Handle any IR remote keypresses. */ diff --git a/src/robot.h b/src/robot.h index ab8d164..521912f 100644 --- a/src/robot.h +++ b/src/robot.h @@ -3,6 +3,7 @@ #include #include #include +#include static float apriltag_Kp = 0.043; // rads per second per pixel static float lining_kP2 = 0.67; @@ -51,6 +52,8 @@ protected: /* Line sensor */ LineSensor lineSensor; + Servo32U4Pin5 servo; + /* To add later: rangefinder, camera, etc.*/ uint16_t distanceTolerance = 65; //for width of tag #define CENTER_THRESHOLD 10