Claw actor/action now uses trapezoid motion profiles.
Change-Id: I500847f2f957922aa788e57b9a73207af55ed7ac
diff --git a/frc971/actors/claw_action.q b/frc971/actors/claw_action.q
index 08b463f..ce5bee7 100644
--- a/frc971/actors/claw_action.q
+++ b/frc971/actors/claw_action.q
@@ -3,8 +3,12 @@
import "aos/common/actions/actions.q";
struct ClawParams {
- double claw_angle;
- double claw_max_velocity;
+ // Goal angle.
+ double angle;
+ // Maximum velocity during the motion.
+ double max_velocity;
+ // Maximum acceleration during the motion.
+ double max_acceleration;
// Positive is sucking in, negative is spitting out.
double intake_voltage;
bool rollers_closed;
diff --git a/frc971/actors/claw_actor.cc b/frc971/actors/claw_actor.cc
index ff66def..ab1cffc 100644
--- a/frc971/actors/claw_actor.cc
+++ b/frc971/actors/claw_actor.cc
@@ -20,27 +20,23 @@
namespace {
// Defines finished.
-constexpr double kAngleEpsilon = 0.01;
+constexpr double kAngleEpsilon = 0.07;
} // namespace
ClawActor::ClawActor(actors::ClawActionQueueGroup *s)
- : aos::common::actions::ActorBase<actors::ClawActionQueueGroup>(s) {}
+ : aos::common::actions::ActorBase<actors::ClawActionQueueGroup>(s),
+ profile_(::aos::controls::kLoopFrequency) {}
bool ClawActor::Iterate(const ClawParams ¶ms) {
- const double goal_angle = params.claw_angle;
- const double goal_velocity = params.claw_max_velocity;
+ const double goal_angle = params.angle;
- if (::std::abs(claw_start_angle_ + delta_angle_ - goal_angle) >
- kAngleEpsilon) {
- delta_angle_ += goal_velocity * ::aos::controls::kLoopFrequency.ToSeconds();
- } else {
- delta_angle_ = goal_angle - claw_start_angle_;
- }
+ ::Eigen::Matrix<double, 2, 1> goal_state =
+ profile_.Update(goal_angle, 0.0);
auto message = control_loops::claw_queue.goal.MakeMessage();
- message->angle = claw_start_angle_ + delta_angle_;
- message->angular_velocity = goal_velocity;
+ message->angle = goal_state(0, 0);
+ message->angular_velocity = goal_state(1, 0);
message->intake = params.intake_voltage;
message->rollers_closed = params.rollers_closed;
@@ -54,7 +50,8 @@
const double current_angle = control_loops::claw_queue.status->angle;
LOG_STRUCT(DEBUG, "Got claw status", *control_loops::claw_queue.status);
- if (::std::abs(goal_angle - current_angle) < kAngleEpsilon) {
+ if (::std::abs(goal_angle - current_angle) < kAngleEpsilon &&
+ ::std::abs(goal_angle - goal_state(0, 0)) < 0.0000001) {
return true;
}
@@ -62,22 +59,28 @@
}
bool ClawActor::RunAction(const ClawParams ¶ms) {
- LOG(INFO, "Claw goal (angle, velocity): %f, %f\n", params.claw_angle,
- params.claw_max_velocity);
-
control_loops::claw_queue.status.FetchLatest();
if (control_loops::claw_queue.status.get()) {
if (!control_loops::claw_queue.status->zeroed) {
LOG(ERROR, "We are not running actions on an unzeroed claw!\n");
return false;
}
- claw_start_angle_ = control_loops::claw_queue.status->angle;
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << control_loops::claw_queue.status->goal_angle, 0.0;
+
+ // Re-initialize the profile to start where we currently are.
+ profile_.MoveCurrentState(current);
+
+ // Update the parameters.
+ profile_.set_maximum_velocity(params.max_velocity);
+ profile_.set_maximum_acceleration(params.max_acceleration);
+
} else {
LOG(ERROR, "No claw status!\n");
return false;
}
- delta_angle_ = 0.0;
while (!Iterate(params)) {
// wait until next Xms tick
::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
diff --git a/frc971/actors/claw_actor.h b/frc971/actors/claw_actor.h
index 9885d3d..bc4888f 100644
--- a/frc971/actors/claw_actor.h
+++ b/frc971/actors/claw_actor.h
@@ -25,8 +25,7 @@
// Returns true if it's reached its ultimate goal, false otherwise.
bool Iterate(const ClawParams ¶ms);
- double delta_angle_ = 0.0;
- double claw_start_angle_ = 0.0;
+ ::aos::util::TrapezoidProfile profile_;
};
typedef aos::common::actions::TypedAction<ClawActionQueueGroup> ClawAction;
diff --git a/frc971/actors/claw_actor_test.cc b/frc971/actors/claw_actor_test.cc
index 7c0cf4e..2fa972c 100644
--- a/frc971/actors/claw_actor_test.cc
+++ b/frc971/actors/claw_actor_test.cc
@@ -43,12 +43,14 @@
// Make some reasonable parameters.
ClawParams params;
- params.claw_angle = 0.5;
- params.claw_max_velocity = 0.5;
+ params.angle = 0.5;
+ params.max_velocity = 0.5;
+ params.max_acceleration = 5.0;
// Fake the status so that it thinks we're already there.
control_loops::claw_queue.status.MakeWithBuilder()
.angle(0.5)
+ .goal_angle(0.5)
.zeroed(true)
.Send();
@@ -57,40 +59,7 @@
// It shouldn't have sent us anywhere.
ASSERT_TRUE(control_loops::claw_queue.goal.FetchLatest());
- EXPECT_EQ(params.claw_angle, control_loops::claw_queue.goal->angle);
-}
-
-// Tests that it's outputting the goals we expect it to.
-TEST_F(ClawActorTest, ValidGoals) {
- ClawActor claw(&frc971::actors::claw_action);
-
- // Make some reasonable parameters.
- ClawParams params;
- params.claw_angle = 0.5;
- params.claw_max_velocity = 0.5;
-
- // Set the starting parameters to what we want them to be.
- claw.delta_angle_ = 0.0;
- claw.claw_start_angle_ = 0.0;
-
- // Do the action iteration by iteration.
- double delta_goal = 0.0;
- while (!claw.Iterate(params)) {
- // Check that it sent a reasonable goal.
- control_loops::claw_queue.goal.FetchLatest();
- ASSERT_TRUE(control_loops::claw_queue.goal.get() != nullptr);
-
- delta_goal +=
- params.claw_max_velocity * ::aos::controls::kLoopFrequency.ToSeconds();
- EXPECT_EQ(delta_goal, control_loops::claw_queue.goal->angle);
-
- // Fake a status.
- control_loops::claw_queue.status.MakeWithBuilder().angle(delta_goal).Send();
- }
-
- delta_goal +=
- params.claw_max_velocity * ::aos::controls::kLoopFrequency.ToSeconds();
- EXPECT_NEAR(params.claw_angle, delta_goal, 0.01);
+ EXPECT_EQ(params.angle, control_loops::claw_queue.goal->angle);
}
} // namespace testing.
diff --git a/frc971/actors/intake_actor.cc b/frc971/actors/intake_actor.cc
index 5dba4bb..ce8eca3 100644
--- a/frc971/actors/intake_actor.cc
+++ b/frc971/actors/intake_actor.cc
@@ -25,6 +25,7 @@
const Time kBackSpitTime = Time::InSeconds(0.5);
constexpr double kClawVelocity = 1.0;
+constexpr double kClawAcceleration = 4.0;
constexpr double kClawIntakeVoltage = 12.0;
constexpr double kClawBackSpitVoltage = 12.0;
constexpr double kElevatorVelocity = 0.3;
@@ -44,8 +45,9 @@
// intake_voltage: The voltage to run the intake rollers at.
::std::unique_ptr<ClawAction> MoveClaw(double angle, double intake_voltage) {
ClawParams params;
- params.claw_angle = angle;
- params.claw_max_velocity = kClawVelocity;
+ params.angle = angle;
+ params.max_velocity = kClawVelocity;
+ params.max_acceleration = kClawAcceleration;
params.intake_voltage = intake_voltage;
params.rollers_closed = true;