weigh bin

lib/Servo32u4/src/servo32u4.h

@@ -45,20 +45,27 @@ 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;}
-    void update(void) 
+    void setTargetPos(uint16_t target) {targetPos = target; moving = true;}
+    bool update(void) 
     {
-        if(targetPos == currentPos) return; // no need to update
-
+        if(targetPos == currentPos) {
+            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,8 +22,11 @@ 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__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
-        tagId = tag.id;
+        blinkTagId = tag.id;
     }
     Serial.print(F("Tag [cx="));
     Serial.print(tag.cx);
@@ -62,6 +62,13 @@ 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: ");
@@ -82,10 +89,6 @@ 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;
 
@@ -114,7 +117,9 @@ 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
@@ -122,9 +127,27 @@ void Robot::EnterApproachingState(void) {
  * you may need floats.
  */
 bool Robot::CheckApproachComplete(int headingTolerance, int distanceTolerance) {
-    return ((tag.w > distanceTolerance) && abs(tag.cx - CENTERPOINT_X) < headingTolerance);
+    bool done = false;
+    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() {
-   EnterIdleState();
+   EnterLiftingState();
 }

src/robot-remote.cpp

@@ -95,6 +95,35 @@ 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

@@ -0,0 +1,40 @@
+#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,6 +23,10 @@ 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)
@@ -303,6 +307,8 @@ void Robot::RobotLoop(void)
      * and asynchronous events (IR presses, distance readings, etc.).
     */
 
+    servo.update();
+
     /**
      * Handle any IR remote keypresses.
      */
@@ -314,6 +320,9 @@ 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)
         {
@@ -324,6 +333,7 @@ void Robot::RobotLoop(void)
 
         // add synchronous, post-motor-update actions here
 
+        if (CheckApproachComplete(headingTolerance, distanceTolerance)) { HandleApproachComplete(); }
     }
 
     /**
@@ -339,5 +349,12 @@ void Robot::RobotLoop(void)
     {
         HandleOrientationUpdate();
     }
+    if (FindAprilTags()) HandleAprilTag(tag);
+
+    int32_t reading = 0;
+    if(loadCellHX1.GetReading(reading)) 
+    {
+        HandleLoadMeasurement(reading);
+    }
 }
 

src/robot.h

@@ -3,6 +3,8 @@
 #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;
@@ -42,6 +44,8 @@ protected:
         ROBOT_TURNING,
         ROBOT_SEARCHING,
         ROBOT_APPROACHING,
+        ROBOT_LIFTING,
+        ROBOT_WEIGHING,
     };
     ROBOT_STATE robotState = ROBOT_IDLE;
 
@@ -51,6 +55,10 @@ 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
@@ -59,6 +67,8 @@ protected:
     AprilTagDatum tag;
     OpenMV camera;
 
+    int most_recent_tag = 0;
+
     // For managing key presses
     String keyString;
 
@@ -100,6 +110,8 @@ protected:
 
     /* State changes */    
     void EnterIdleState(void);
+    void EnterLiftingState(void);
+    void EnterWeighingState(void);
 
     /* Mode changes */
     void EnterTeleopMode(void);
@@ -115,6 +127,8 @@ 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);