Merge pull request #107 from ut-ras/agitator-unjam

Add automatic agitator unjamming
ut-ras · Mar 31, 2024 · f3d6837 · f3d6837
2 parents 26a3ef4 + 8a90613
commit f3d6837
Show file tree

Hide file tree

Showing 7 changed files with 53 additions and 8 deletions.
diff --git a/ut-robomaster/src/robots/hero/hero_constants.hpp b/ut-robomaster/src/robots/hero/hero_constants.hpp
@@ -67,7 +67,10 @@ const float FEEDER_RATIO = 1.0f;  // feeder speed / agitator speed
 
 const uint16_t BARREL_HEAT_BUFFER = 100.0f;
 
-const float UNJAM_SPEED = 0.4f;  // rev/s
+const float JAM_TRIGGER_RATIO = 0.5;      // measured speed to driven speed ratio
+const float JAM_TRIGGER_DURATION = 0.1f;  // s
+const float UNJAM_DURATION = 0.1f;        // s
+const float UNJAM_SPEED = 12.0f;          // rev/s
 
 static constexpr int FLYWHEELS = 2;
 static constexpr float DEFAULT_SPEED = 60.0f;

diff --git a/ut-robomaster/src/robots/sentry/sentry_constants.hpp b/ut-robomaster/src/robots/sentry/sentry_constants.hpp
@@ -66,7 +66,10 @@ const float BALLS_PER_REV = 8.0f;
 
 const uint16_t BARREL_HEAT_BUFFER = 20.0f;
 
-const float UNJAM_SPEED = 0.4f;  // rev/s
+const float JAM_TRIGGER_RATIO = 0.5;      // measured speed to driven speed ratio
+const float JAM_TRIGGER_DURATION = 0.1f;  // s
+const float UNJAM_DURATION = 0.1f;        // s
+const float UNJAM_SPEED = 15.0f;          // rev/s
 
 // Turret constants ------------------------------------------------
 constexpr MotorId ID_YAW = MOTOR6;

diff --git a/ut-robomaster/src/robots/standard/standard_constants.hpp b/ut-robomaster/src/robots/standard/standard_constants.hpp
@@ -65,7 +65,10 @@ const float BALLS_PER_REV = 8.0f;
 
 const uint16_t BARREL_HEAT_BUFFER = 20.0f;
 
-const float UNJAM_SPEED = 0.4f;  // rev/s
+const float JAM_TRIGGER_RATIO = 0.5;      // measured speed to driven speed ratio
+const float JAM_TRIGGER_DURATION = 0.1f;  // s
+const float UNJAM_DURATION = 0.1f;        // s
+const float UNJAM_SPEED = 15.0f;          // rev/s
 
 // Turret constants ------------------------------------------------
 constexpr MotorId ID_YAW = MOTOR6;

diff --git a/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp b/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp
@@ -70,4 +70,5 @@ float AgitatorSubsystem::getShapedVelocity(float time, float a, float phi, float
 
 void AgitatorSubsystem::setBallsPerSecond(float bps) { ballsPerSecond = bps; }
 float AgitatorSubsystem::getPosition() { return agitator.measurePosition(); }
+float AgitatorSubsystem::getVelocity() { return agitator.measureVelocity(); }
 }  // namespace subsystems::agitator
diff --git a/ut-robomaster/src/subsystems/agitator/agitator_subsystem.hpp b/ut-robomaster/src/subsystems/agitator/agitator_subsystem.hpp
@@ -30,6 +30,7 @@ class AgitatorSubsystem : public tap::control::Subsystem
     void setBallsPerSecond(float bps);
 
     float getPosition();
+    float getVelocity();
 
 private:
     src::Drivers *drivers;

diff --git a/ut-robomaster/src/subsystems/agitator/command_agitator_continuous.cpp b/ut-robomaster/src/subsystems/agitator/command_agitator_continuous.cpp
@@ -2,19 +2,47 @@
 
 namespace commands
 {
-void CommandAgitatorContinuous::initialize() {}
+
+void CommandAgitatorContinuous::initialize()
+{
+    jamTriggerTimeout.restart(JAM_TRIGGER_DURATION * 1000);
+    isJammed = false;
+}
 
 void CommandAgitatorContinuous::execute()
 {
-    float bps = BALLS_PER_SEC;
+    if (isJammed)
+    {
+        if (unjammingTimeout.isExpired())
+        {
+            jamTriggerTimeout.restart(JAM_TRIGGER_DURATION * 1000);
+            isJammed = false;
+        }
 
-    if (drivers->refSerial.getRefSerialReceivingData() &&
+        agitator->setBallsPerSecond(-UNJAM_SPEED);
+    }
+    else if (  // barrel overheat
+        drivers->refSerial.getRefSerialReceivingData() &&
         power_limiter::getRemainingCooldown(drivers, barrelId) <= BARREL_HEAT_BUFFER)
     {
-        bps = 0.0f;
+        agitator->setBallsPerSecond(0.0f);
     }
+    else  // heat remaining and no jams
+    {
+        float speedRatio = abs(agitator->getVelocity()) / (BALLS_PER_SEC / BALLS_PER_REV);
 
-    agitator->setBallsPerSecond(bps);
+        if (speedRatio > JAM_TRIGGER_RATIO)  // no jam
+        {
+            jamTriggerTimeout.restart(JAM_TRIGGER_DURATION * 1000);
+        }
+        else if (jamTriggerTimeout.isExpired())  // jam detected
+        {
+            unjammingTimeout.restart(UNJAM_DURATION * 1000);
+            isJammed = true;
+        }
+
+        agitator->setBallsPerSecond(BALLS_PER_SEC);
+    }
 }
 
 void CommandAgitatorContinuous::end(bool) { agitator->setBallsPerSecond(0.0f); }

diff --git a/ut-robomaster/src/subsystems/agitator/command_agitator_continuous.hpp b/ut-robomaster/src/subsystems/agitator/command_agitator_continuous.hpp
@@ -1,5 +1,6 @@
 #pragma once
 
+#include "tap/architecture/timeout.hpp"
 #include "tap/control/command.hpp"
 
 #include "robots/robot_constants.hpp"
@@ -8,6 +9,8 @@
 
 #include "drivers.hpp"
 
+using tap::arch::MilliTimeout;
+
 namespace commands
 {
 using power_limiter::BarrelId;
@@ -38,5 +41,8 @@ class CommandAgitatorContinuous : public tap::control::Command
     src::Drivers *drivers;
     AgitatorSubsystem *agitator;
     BarrelId barrelId;
+    MilliTimeout jamTriggerTimeout;
+    MilliTimeout unjammingTimeout;
+    bool isJammed = false;
 };
 }  // namespace commands