Add arm/elevator profile instances into the control loop.
This is heavily based upon Austin's corresponding changes to the claw:
bd5308ac4e9715a3d73d0dbaec9eb604ea16f44b
Change-Id: I205038c8807c52cd1aa4efa5abced6f696152d54
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index a4aa2e7..613cf37 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -57,7 +57,9 @@
right_arm_estimator_(constants::GetValues().fridge.right_arm_zeroing),
left_elevator_estimator_(constants::GetValues().fridge.left_elev_zeroing),
right_elevator_estimator_(
- constants::GetValues().fridge.right_elev_zeroing) {}
+ constants::GetValues().fridge.right_elev_zeroing),
+ arm_profile_(::aos::controls::kLoopFrequency),
+ elevator_profile_(::aos::controls::kLoopFrequency) {}
void Fridge::UpdateZeroingState() {
if (left_elevator_estimator_.offset_ratio_ready() < 1.0 ||
@@ -197,6 +199,14 @@
return elevator_zeroing_velocity_;
}
+double Fridge::UseUnlessZero(double target_value, double default_value) {
+ if (target_value != 0.0) {
+ return target_value;
+ } else {
+ return default_value;
+ }
+}
+
double Fridge::arm_zeroing_velocity() {
const double average_arm = (constants::GetValues().fridge.arm.lower_limit +
constants::GetValues().fridge.arm.upper_limit) /
@@ -330,6 +340,17 @@
elevator_goal_ += elevator_goal_velocity *
::aos::controls::kLoopFrequency.ToSeconds();
}
+
+ // Bypass motion profiles while we are zeroing.
+ // This is also an important step right after the elevator is zeroed and
+ // we reach into the elevator's state matrix and change it based on the
+ // newly-obtained offset.
+ {
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << elevator_goal_, elevator_goal_velocity;
+ elevator_profile_.MoveCurrentState(current);
+ }
break;
case ZEROING_ARM:
@@ -347,22 +368,55 @@
arm_goal_ +=
arm_goal_velocity * ::aos::controls::kLoopFrequency.ToSeconds();
}
+
+ // Bypass motion profiles while we are zeroing.
+ // This is also an important step right after the arm is zeroed and
+ // we reach into the arm's state matrix and change it based on the
+ // newly-obtained offset.
+ {
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << arm_goal_, arm_goal_velocity;
+ arm_profile_.MoveCurrentState(current);
+ }
break;
case RUNNING:
LOG(DEBUG, "Running!\n");
if (unsafe_goal) {
- arm_goal_velocity = unsafe_goal->angular_velocity;
- elevator_goal_velocity = unsafe_goal->velocity;
+ // Pick a set of sane arm defaults if none are specified.
+ arm_profile_.set_maximum_velocity(
+ UseUnlessZero(unsafe_goal->max_angular_velocity, 1.0));
+ arm_profile_.set_maximum_acceleration(
+ UseUnlessZero(unsafe_goal->max_angular_acceleration, 3.0));
+ elevator_profile_.set_maximum_velocity(
+ UseUnlessZero(unsafe_goal->max_velocity, 0.50));
+ elevator_profile_.set_maximum_acceleration(
+ UseUnlessZero(unsafe_goal->max_acceleration, 2.0));
+
+ // Use the profiles to limit the arm's movements.
+ const double unfiltered_arm_goal = ::std::max(
+ ::std::min(unsafe_goal->angle, values.fridge.arm.upper_limit),
+ values.fridge.arm.lower_limit);
+ ::Eigen::Matrix<double, 2, 1> arm_goal_state = arm_profile_.Update(
+ unfiltered_arm_goal, unsafe_goal->angular_velocity);
+ arm_goal_ = arm_goal_state(0, 0);
+ arm_goal_velocity = arm_goal_state(1, 0);
+
+ // Use the profiles to limit the elevator's movements.
+ const double unfiltered_elevator_goal = ::std::max(
+ ::std::min(unsafe_goal->height, values.fridge.elevator.upper_limit),
+ values.fridge.elevator.lower_limit);
+ ::Eigen::Matrix<double, 2, 1> elevator_goal_state =
+ elevator_profile_.Update(unfiltered_elevator_goal,
+ unsafe_goal->velocity);
+ elevator_goal_ = elevator_goal_state(0, 0);
+ elevator_goal_velocity = elevator_goal_state(1, 0);
}
// Update state_ to accurately represent the state of the zeroing
// estimators.
UpdateZeroingState();
- if (unsafe_goal) {
- arm_goal_ = unsafe_goal->angle;
- elevator_goal_ = unsafe_goal->height;
- }
if (state_ != RUNNING && state_ != ESTOP) {
state_ = UNINITIALIZED;
@@ -517,9 +571,13 @@
// TODO(austin): Populate these fully.
status->zeroed = state_ == RUNNING;
status->angle = arm_loop_->X_hat(0, 0);
+ status->angular_velocity = arm_loop_->X_hat(1, 0);
status->height = elevator_loop_->X_hat(0, 0);
+ status->velocity = elevator_loop_->X_hat(1, 0);
status->goal_angle = arm_goal_;
+ status->goal_angular_velocity = arm_goal_velocity;
status->goal_height = elevator_goal_;
+ status->goal_velocity = elevator_goal_velocity;
if (unsafe_goal) {
status->grabbers = unsafe_goal->grabbers;
} else {
diff --git a/frc971/control_loops/fridge/fridge.gyp b/frc971/control_loops/fridge/fridge.gyp
index dc25b63..865d42d 100644
--- a/frc971/control_loops/fridge/fridge.gyp
+++ b/frc971/control_loops/fridge/fridge.gyp
@@ -30,6 +30,7 @@
'dependencies': [
'fridge_queue',
'<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/control_loops/voltage_cap/voltage_cap.gyp:voltage_cap',
@@ -37,6 +38,7 @@
'export_dependent_settings': [
'fridge_queue',
'<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
],
},
diff --git a/frc971/control_loops/fridge/fridge.h b/frc971/control_loops/fridge/fridge.h
index e9cbaa5..5761c79 100644
--- a/frc971/control_loops/fridge/fridge.h
+++ b/frc971/control_loops/fridge/fridge.h
@@ -4,6 +4,7 @@
#include <memory>
#include "aos/common/controls/control_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/fridge/fridge.q.h"
#include "frc971/zeroing/zeroing.h"
@@ -112,6 +113,8 @@
// Corrects the Observer with the current position.
void Correct();
+ double UseUnlessZero(double target_value, double default_value);
+
// The state feedback control loop or loops to talk to.
::std::unique_ptr<CappedStateFeedbackLoop<5>> arm_loop_;
::std::unique_ptr<CappedStateFeedbackLoop<4>> elevator_loop_;
@@ -140,6 +143,9 @@
State last_state_ = UNINITIALIZED;
control_loops::FridgeQueue::Position current_position_;
+
+ aos::util::TrapezoidProfile arm_profile_;
+ aos::util::TrapezoidProfile elevator_profile_;
};
} // namespace control_loops
diff --git a/frc971/control_loops/fridge/fridge.q b/frc971/control_loops/fridge/fridge.q
index f8fa16e..d1f5158 100644
--- a/frc971/control_loops/fridge/fridge.q
+++ b/frc971/control_loops/fridge/fridge.q
@@ -35,6 +35,16 @@
// Linear velocity of the elevator.
double velocity;
+ // Maximum arm profile angular velocity or 0 for the default.
+ double max_angular_velocity;
+ // Maximum elevator profile velocity or 0 for the default.
+ double max_velocity;
+
+ // Maximum arm profile acceleration or 0 for the default.
+ double max_angular_acceleration;
+ // Maximum elevator profile acceleration or 0 for the default.
+ double max_acceleration;
+
// TODO(austin): Do I need acceleration here too?
GrabberPistons grabbers;
@@ -49,17 +59,23 @@
// Are both the arm and elevator zeroed?
bool zeroed;
- // Angle of the arm.
+ // Estimated angle of the arm.
double angle;
- // Height of the elevator.
+ // Estimated angular velocity of the arm.
+ double angular_velocity;
+ // Estimated height of the elevator.
double height;
+ // Estimated velocity of the elvator.
+ double velocity;
// state of the grabber pistons
GrabberPistons grabbers;
// Goal angle of the arm.
double goal_angle;
+ double goal_angular_velocity;
// Goal height of the elevator.
double goal_height;
+ double goal_velocity;
// If true, we have aborted.
bool estopped;