servo support

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

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.
      */

src/robot.h

@@ -3,6 +3,7 @@
 #include <LineSensor.h>
 #include <LSM6.h>
 #include <openmv.h>
+#include <servo32u4.h>
 
 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