Claw control loop now is only profiled.
Change-Id: I862b2abdd61ef70a483ebb7066dde13e946274d8
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 004ea7a..64ba98e 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -7,6 +7,7 @@
#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"
+#include "aos/common/util/trapezoid_profile.h"
namespace frc971 {
namespace control_loops {
@@ -33,7 +34,8 @@
: aos::controls::ControlLoop<control_loops::ClawQueue>(claw),
last_piston_edge_(Time::Now()),
claw_loop_(new ClawCappedStateFeedbackLoop(MakeClawLoop())),
- claw_estimator_(constants::GetValues().claw.zeroing) {}
+ claw_estimator_(constants::GetValues().claw.zeroing),
+ profile_(::aos::controls::kLoopFrequency) {}
void Claw::UpdateZeroingState() {
if (claw_estimator_.offset_ratio_ready() < 1.0) {
@@ -169,6 +171,14 @@
claw_goal_ +=
claw_goal_velocity * ::aos::controls::kLoopFrequency.ToSeconds();
}
+
+ // Clear the current profile state if we are zeroing.
+ {
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << claw_goal_, claw_goal_velocity;
+ profile_.MoveCurrentState(current);
+ }
break;
case RUNNING:
@@ -177,8 +187,25 @@
// Update state_.
UpdateZeroingState();
if (unsafe_goal) {
- claw_goal_ = unsafe_goal->angle;
- claw_goal_velocity = unsafe_goal->angular_velocity;
+ // Pick a set of sane defaults if none are specified.
+ if (unsafe_goal->max_velocity != 0.0) {
+ profile_.set_maximum_velocity(unsafe_goal->max_velocity);
+ } else {
+ profile_.set_maximum_velocity(2.5);
+ }
+ if (unsafe_goal->max_acceleration != 0.0) {
+ profile_.set_maximum_acceleration(unsafe_goal->max_acceleration);
+ } else {
+ profile_.set_maximum_acceleration(4.0);
+ }
+
+ const double unfiltered_goal = ::std::max(
+ ::std::min(unsafe_goal->angle, values.claw.wrist.upper_limit),
+ values.claw.wrist.lower_limit);
+ ::Eigen::Matrix<double, 2, 1> goal_state =
+ profile_.Update(unfiltered_goal, unsafe_goal->angular_velocity);
+ claw_goal_ = goal_state(0, 0);
+ claw_goal_velocity = goal_state(1, 0);
}
if (state_ != RUNNING && state_ != ESTOP) {
@@ -249,12 +276,15 @@
zeroing::PopulateEstimatorState(claw_estimator_, &status->zeroing_state);
status->angle = claw_loop_->X_hat(0, 0);
+ status->angular_velocity = claw_loop_->X_hat(1, 0);
+
if (output) {
status->intake = output->intake_voltage;
} else {
status->intake = 0;
}
status->goal_angle = claw_goal_;
+ status->goal_velocity = claw_goal_velocity;
if (output) {
status->rollers_open =
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index 16001c3..b84b5c0 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -29,6 +29,7 @@
'claw_queue',
'<(AOS)/common/controls/controls.gyp:control_loop',
'<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/zeroing/zeroing.gyp:zeroing',
@@ -37,6 +38,7 @@
'claw_queue',
'<(AOS)/common/controls/controls.gyp:control_loop',
'<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/zeroing/zeroing.gyp:zeroing',
],
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 381bd15..0413b8d 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -5,6 +5,7 @@
#include "aos/common/controls/control_loop.h"
#include "aos/common/time.h"
+#include "aos/common/util/trapezoid_profile.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"
@@ -105,6 +106,8 @@
// Whether claw was closed last cycle.
bool last_rollers_closed_ = false;
+
+ ::aos::util::TrapezoidProfile profile_;
};
} // namespace control_loops
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index e05f779..7696d3b 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -15,6 +15,10 @@
double angle;
// Angular velocity of wrist.
double angular_velocity;
+ // Maximum profile velocity, or 0 for the default.
+ double max_velocity;
+ // Maximum profile acceleration, or 0 for the default.
+ double max_acceleration;
// Voltage of intake rollers. Positive means sucking in, negative means
// spitting out.
double intake;
@@ -47,10 +51,14 @@
uint32_t state;
EstimatorState zeroing_state;
- // Angle of wrist joint.
+ // Estimated angle of wrist joint.
double angle;
+ // Estimated angular velocity of wrist.
+ double angular_velocity;
+
// Goal angle of wrist joint.
double goal_angle;
+ double goal_velocity;
// Voltage of intake rollers. Positive means sucking in, negative means
// spitting out.
double intake;
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 142f5ab..e53c0ed 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -211,7 +211,7 @@
.angle(values.claw.wrist.upper_hard_limit + 5.0)
.Send());
- RunForTime(Time::InSeconds(6));
+ RunForTime(Time::InSeconds(7));
claw_queue_.status.FetchLatest();
EXPECT_NEAR(values.claw.wrist.upper_limit,
@@ -223,7 +223,7 @@
.angle(values.claw.wrist.lower_hard_limit - 5.0)
.Send());
- RunForTime(Time::InMS(4000));
+ RunForTime(Time::InMS(6000));
claw_queue_.status.FetchLatest();
EXPECT_NEAR(values.claw.wrist.lower_limit,
@@ -312,7 +312,7 @@
claw_.claw_goal_ += 100.0;
RunIteration();
- EXPECT_NEAR(orig_claw_goal, claw_.claw_goal_, 0.05);
+ EXPECT_NEAR(orig_claw_goal, claw_.claw_goal_, 0.10);
RunIteration();
@@ -335,7 +335,7 @@
claw_.claw_goal_ -= 100.0;
RunIteration();
- EXPECT_NEAR(orig_claw_goal, claw_.claw_goal_, 0.05);
+ EXPECT_NEAR(orig_claw_goal, claw_.claw_goal_, 0.10);
RunIteration();