From 9bbe7a839c903fe43f880b40e157c9c49ed29ed0 Mon Sep 17 00:00:00 2001 From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com> Date: Tue, 26 Nov 2024 16:37:56 -0500 Subject: [PATCH] weigh bin --- lib/Servo32u4/src/servo32u4.h | 15 +++++++++---- platformio.ini | 5 ++++- src/robot-cam.cpp | 37 ++++++++++++++++++++++++++------ src/robot-remote.cpp | 6 ++++++ src/robot-scale.cpp | 40 +++++++++++++++++++++++++++++++++++ src/robot.cpp | 15 ++++++++++++- src/robot.h | 11 ++++++++++ 7 files changed, 116 insertions(+), 13 deletions(-) create mode 100644 src/robot-scale.cpp diff --git a/lib/Servo32u4/src/servo32u4.h b/lib/Servo32u4/src/servo32u4.h index 27e5e3a..1cfb3b2 100644 --- a/lib/Servo32u4/src/servo32u4.h +++ b/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); diff --git a/platformio.ini b/platformio.ini index 3afa0dc..84c34cb 100644 --- a/platformio.ini +++ b/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__ \ No newline at end of file +; -D__IMU_DEBUG__ +; -D__TRACK_DEBUG__ +; -DDARK diff --git a/src/robot-cam.cpp b/src/robot-cam.cpp index 5dbf8a7..e427417 100644 --- a/src/robot-cam.cpp +++ b/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(); } diff --git a/src/robot-remote.cpp b/src/robot-remote.cpp index ca1e576..e23d1bb 100644 --- a/src/robot-remote.cpp +++ b/src/robot-remote.cpp @@ -100,6 +100,12 @@ void Robot::HandleKeyCode(int16_t keyCode) Serial.println(keyString); keyString = ""; break; + case VOLminus: + EnterLiftingState(); + break; + case VOLplus: + EnterWeighingState(); + break; case NUM_1: case NUM_2: case NUM_3: diff --git a/src/robot-scale.cpp b/src/robot-scale.cpp new file mode 100644 index 0000000..b9bf004 --- /dev/null +++ b/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(); + } + } +} diff --git a/src/robot.cpp b/src/robot.cpp index 3d78682..4de3028 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -24,7 +24,9 @@ void Robot::InitializeRobot(void) // The line sensor elements default to INPUTs, but we'll initialize anyways, for completeness lineSensor.Initialize(); - servo.attach(); + servo.attach(); + + loadCellHX1.Init(); } void Robot::EnterIdleState(void) @@ -318,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) { @@ -328,6 +333,7 @@ void Robot::RobotLoop(void) // add synchronous, post-motor-update actions here + if (CheckApproachComplete(headingTolerance, distanceTolerance)) { HandleApproachComplete(); } } /** @@ -343,5 +349,12 @@ void Robot::RobotLoop(void) { HandleOrientationUpdate(); } + if (FindAprilTags()) HandleAprilTag(tag); + + int32_t reading = 0; + if(loadCellHX1.GetReading(reading)) + { + HandleLoadMeasurement(reading); + } } diff --git a/src/robot.h b/src/robot.h index 521912f..5da246e 100644 --- a/src/robot.h +++ b/src/robot.h @@ -4,6 +4,7 @@ #include #include #include +#include static float apriltag_Kp = 0.043; // rads per second per pixel static float lining_kP2 = 0.67; @@ -43,6 +44,8 @@ protected: ROBOT_TURNING, ROBOT_SEARCHING, ROBOT_APPROACHING, + ROBOT_LIFTING, + ROBOT_WEIGHING, }; ROBOT_STATE robotState = ROBOT_IDLE; @@ -54,6 +57,8 @@ protected: Servo32U4Pin5 servo; + HX711<6, 13> loadCellHX1; + /* To add later: rangefinder, camera, etc.*/ uint16_t distanceTolerance = 65; //for width of tag #define CENTER_THRESHOLD 10 @@ -62,6 +67,8 @@ protected: AprilTagDatum tag; OpenMV camera; + int most_recent_tag = 0; + // For managing key presses String keyString; @@ -103,6 +110,8 @@ protected: /* State changes */ void EnterIdleState(void); + void EnterLiftingState(void); + void EnterWeighingState(void); /* Mode changes */ void EnterTeleopMode(void); @@ -118,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);