lib/Servo32u4/src/servo32u4.h

@@ -45,27 +45,20 @@ protected:
     uint16_t targetPos = 1500;
     uint16_t currentPos = 1500;
 
-    bool moving = false;
-
 public:
     // Virtual functions defined for each specific class
     virtual void attach(void) = 0;
     virtual void detach(void) = 0;
-    void setTargetPos(uint16_t target) {targetPos = target; moving = true;}
+    void setTargetPos(uint16_t target) {targetPos = target;}
-    bool update(void) 
+    void update(void) 
     {
-        if(targetPos == currentPos) {
+        if(targetPos == currentPos) return; // no need to update
-            bool completed = moving;
+
-            moving = false; 
-            return completed;
-        } // no need to update
         else if(abs(targetPos - currentPos) <= 40) currentPos = targetPos;
         else if(targetPos > currentPos) currentPos += 40;
         else if(targetPos < currentPos) currentPos -= 40;
 
         writeMicroseconds(currentPos);
-
-        return false;
     }
     uint16_t setMinMaxMicroseconds(uint16_t min, uint16_t max);
 

platformio.ini

@@ -22,11 +22,8 @@ lib_deps =
     https://github.com/WPIRoboticsEngineering/openmv-apriltags
 ;    https://github.com/gcl8a/LSM6
     https://github.com/WPIRoboticsEngineering/Romi-32u4-utilities
-    https://github.com/gcl8a/Load-Cell-Amps
 
 build_flags = 
 ;    -D__MOTOR_DEBUG__
 ;    -D__LOOP_DEBUG__
 ;    -D__IMU_DEBUG__
-;    -D__TRACK_DEBUG__
-;    -DDARK

src/robot-cam.cpp

@@ -32,7 +32,7 @@ uint8_t Robot::FindAprilTags(void) {
   if(camera.checkUART(tag))
   {
     if (tag.id < 10) { // activate blinkenlights
-        blinkTagId = tag.id;
+        tagId = tag.id;
     }
     Serial.print(F("Tag [cx="));
     Serial.print(tag.cx);
@@ -62,13 +62,6 @@ uint8_t Robot::FindAprilTags(void) {
   return 0;
 }
 
-enum APPROACH_STATE {
-    APPROACH_TRACKED,
-    APPROACH_RECKONED,
-};
-int approachState = APPROACH_TRACKED;
-int reckonTimer = 0;
-
 void Robot::HandleAprilTag(const AprilTagDatum &tag) {
     // You may want to comment some of these out when you’re done testing.
     //Serial.print("Tag: ");
@@ -89,6 +82,10 @@ void Robot::HandleAprilTag(const AprilTagDatum &tag) {
     EnterApproachingState(); //found tag while searching
   }
   if (robotState == ROBOT_APPROACHING) {
+    if (CheckApproachComplete(headingTolerance, distanceTolerance)) {
+        HandleApproachComplete();
+        return;
+    }
 
     float error = (float)tag.cx - (float)CENTERPOINT_X;
 
@@ -117,9 +114,7 @@ void Robot::EnterApproachingState(void) {
      */
   Serial.println(" -> APPROACHING");
   chassis.Stop();
-  servo.setTargetPos(1950);
   robotState = ROBOT_APPROACHING;
-  approachState = APPROACH_TRACKED;
 }
 
 /** Note that the tolerances are in integers, since the camera works
@@ -127,27 +122,9 @@ void Robot::EnterApproachingState(void) {
  * you may need floats.
  */
 bool Robot::CheckApproachComplete(int headingTolerance, int distanceTolerance) {
-    bool done = false;
+    return ((tag.w > distanceTolerance) && abs(tag.cx - CENTERPOINT_X) < headingTolerance);
-    if (robotState == ROBOT_APPROACHING) {
-        if ((tag.w > distanceTolerance) && abs(tag.cx - CENTERPOINT_X) < headingTolerance && approachState == APPROACH_TRACKED) {
-            chassis.SetTwist(-8, 0);
-            reckonTimer = 100;
-            approachState = APPROACH_RECKONED;
-            Serial.println("-> RECKONING");
-        }
-
-        if (approachState == APPROACH_RECKONED ) {
-            chassis.SetTwist(-8, 0);
-            if (reckonTimer-- < 0) {
-                done = true;
-                chassis.Stop();
-            }
-        }
-
-    }
-    return done;
 }
 
 void Robot::HandleApproachComplete() {
-   EnterLiftingState();
+   EnterIdleState();
 }

src/robot-remote.cpp

@@ -95,35 +95,6 @@ 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 VOLminus:
-                EnterLiftingState();
-                break;
-            case VOLplus:
-                EnterWeighingState();
-                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-scale.cpp

@@ -1,40 +0,0 @@
-#include "robot.h"
-
-void Robot::EnterLiftingState(void)
-{
-    Serial.println("-> LIFTING");
-    chassis.Stop();
-    servo.setTargetPos(1000);
-    robotState = ROBOT_LIFTING;
-}
-
-int index;
-float mass_measurements[8];
-
-void Robot::EnterWeighingState(void)
-{
-    Serial.println("-> WEIGHING");
-    index = 0;
-    chassis.Stop();
-    robotState = ROBOT_WEIGHING;
-}
-
-void Robot::HandleLoadMeasurement(int32_t& reading) {
-    if (robotState == ROBOT_WEIGHING) {
-        float grams = (299578.337 + (float)reading) / 748.32172;
-        mass_measurements[index++] = grams;
-        if (index > 7) {
-            float sum=0;
-            for (int i : mass_measurements) {sum+=i;};
-            Serial.println("lifted!"); 
-            Serial.print("weight: ");
-            Serial.print(sum/8);
-            Serial.println("grams");
-            Serial.print("tag id: ");
-            Serial.print(most_recent_tag);
-            Serial.println();
-
-            EnterIdleState();
-        }
-    }
-}

src/robot.cpp

@@ -23,10 +23,6 @@ void Robot::InitializeRobot(void)
 
     // The line sensor elements default to INPUTs, but we'll initialize anyways, for completeness
     lineSensor.Initialize();
-
-    servo.attach();  
-
-    loadCellHX1.Init();
 }
 
 void Robot::EnterIdleState(void)
@@ -307,8 +303,6 @@ void Robot::RobotLoop(void)
      * and asynchronous events (IR presses, distance readings, etc.).
     */
 
-    servo.update();
-
     /**
      * Handle any IR remote keypresses.
      */
@@ -320,9 +314,6 @@ void Robot::RobotLoop(void)
      */
     if(chassis.CheckChassisTimer())
     {
-        if (servo.update() && robotState == ROBOT_LIFTING) {
-            EnterWeighingState();
-        }
         // add synchronous, pre-motor-update actions here
         if(robotState == ROBOT_LINING)
         {
@@ -333,7 +324,6 @@ void Robot::RobotLoop(void)
 
         // add synchronous, post-motor-update actions here
 
-        if (CheckApproachComplete(headingTolerance, distanceTolerance)) { HandleApproachComplete(); }
     }
 
     /**
@@ -349,12 +339,5 @@ void Robot::RobotLoop(void)
     {
         HandleOrientationUpdate();
     }
-    if (FindAprilTags()) HandleAprilTag(tag);
-
-    int32_t reading = 0;
-    if(loadCellHX1.GetReading(reading)) 
-    {
-        HandleLoadMeasurement(reading);
-    }
 }
 

src/robot.h

@@ -3,8 +3,6 @@
 #include <LineSensor.h>
 #include <LSM6.h>
 #include <openmv.h>
-#include <servo32u4.h>
-#include <HX711.h>
 
 static float apriltag_Kp = 0.043; // rads per second per pixel
 static float lining_kP2 = 0.67;
@@ -44,8 +42,6 @@ protected:
         ROBOT_TURNING,
         ROBOT_SEARCHING,
         ROBOT_APPROACHING,
-        ROBOT_LIFTING,
-        ROBOT_WEIGHING,
     };
     ROBOT_STATE robotState = ROBOT_IDLE;
 
@@ -55,10 +51,6 @@ protected:
     /* Line sensor */
     LineSensor lineSensor;
 
-    Servo32U4Pin5 servo;
-
-    HX711<6, 13> loadCellHX1;
-
     /* To add later: rangefinder, camera, etc.*/
     uint16_t distanceTolerance = 65; //for width of tag
     #define CENTER_THRESHOLD 10
@@ -67,8 +59,6 @@ protected:
     AprilTagDatum tag;
     OpenMV camera;
 
-    int most_recent_tag = 0;
-
     // For managing key presses
     String keyString;
 
@@ -110,8 +100,6 @@ protected:
 
     /* State changes */    
     void EnterIdleState(void);
-    void EnterLiftingState(void);
-    void EnterWeighingState(void);
 
     /* Mode changes */
     void EnterTeleopMode(void);
@@ -127,8 +115,6 @@ protected:
     bool CheckIntersection(void) {return lineSensor.CheckIntersection();}
     void HandleIntersection(void);
 
-    void HandleLoadMeasurement(int32_t& reading);
-
     void EnterTurn(float angleInDeg);
     bool CheckTurnComplete(void);
     void HandleTurnComplete(void);