Add X/Y profiling to the fridge.
The fridge queue now accepts parameters for an additional profiling
type. If you set `profiling_type` to 1 then you can specify the goals
in X/Y co-ordinates.
This is very useful for the scoring actions because it lets us move
the fridge horizontally with nicely profiled motions.
Change-Id: Idc70d5f6737a18a76448af0ffe24079cac616d06
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 613cf37..ca65929 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -58,8 +58,16 @@
left_elevator_estimator_(constants::GetValues().fridge.left_elev_zeroing),
right_elevator_estimator_(
constants::GetValues().fridge.right_elev_zeroing),
+ last_profiling_type_(ProfilingType::ANGLE_HEIGHT_PROFILING),
+ kinematics_(constants::GetValues().fridge.arm_length,
+ constants::GetValues().fridge.elevator.upper_limit,
+ constants::GetValues().fridge.elevator.lower_limit,
+ constants::GetValues().fridge.arm.upper_limit,
+ constants::GetValues().fridge.arm.lower_limit),
arm_profile_(::aos::controls::kLoopFrequency),
- elevator_profile_(::aos::controls::kLoopFrequency) {}
+ elevator_profile_(::aos::controls::kLoopFrequency),
+ x_profile_(::aos::controls::kLoopFrequency),
+ y_profile_(::aos::controls::kLoopFrequency) {}
void Fridge::UpdateZeroingState() {
if (left_elevator_estimator_.offset_ratio_ready() < 1.0 ||
@@ -250,8 +258,6 @@
// Bool to track if we should turn the motors on or not.
bool disable = output == nullptr;
- double elevator_goal_velocity = 0.0;
- double arm_goal_velocity = 0.0;
// Save the current position so it can be used easily in the class.
current_position_ = *position;
@@ -329,15 +335,15 @@
LOG(DEBUG, "Moving elevator to safe height.\n");
elevator_goal_ += kElevatorSafeHeightVelocity *
::aos::controls::kLoopFrequency.ToSeconds();
- elevator_goal_velocity = kElevatorSafeHeightVelocity;
+ elevator_goal_velocity_ = kElevatorSafeHeightVelocity;
state_ = ZEROING_ELEVATOR;
break;
}
} else if (!disable) {
- elevator_goal_velocity = elevator_zeroing_velocity();
- elevator_goal_ += elevator_goal_velocity *
+ elevator_goal_velocity_ = elevator_zeroing_velocity();
+ elevator_goal_ += elevator_goal_velocity_ *
::aos::controls::kLoopFrequency.ToSeconds();
}
@@ -348,7 +354,7 @@
{
Eigen::Matrix<double, 2, 1> current;
current.setZero();
- current << elevator_goal_, elevator_goal_velocity;
+ current << elevator_goal_, elevator_goal_velocity_;
elevator_profile_.MoveCurrentState(current);
}
break;
@@ -364,9 +370,9 @@
right_arm_estimator_.offset());
LOG(DEBUG, "Zeroed the arm!\n");
} else if (!disable) {
- arm_goal_velocity = arm_zeroing_velocity();
+ arm_goal_velocity_ = arm_zeroing_velocity();
arm_goal_ +=
- arm_goal_velocity * ::aos::controls::kLoopFrequency.ToSeconds();
+ arm_goal_velocity_ * ::aos::controls::kLoopFrequency.ToSeconds();
}
// Bypass motion profiles while we are zeroing.
@@ -376,7 +382,7 @@
{
Eigen::Matrix<double, 2, 1> current;
current.setZero();
- current << arm_goal_, arm_goal_velocity;
+ current << arm_goal_, arm_goal_velocity_;
arm_profile_.MoveCurrentState(current);
}
break;
@@ -384,34 +390,106 @@
case RUNNING:
LOG(DEBUG, "Running!\n");
if (unsafe_goal) {
- // 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));
+ // Handle the case where we switch between the types of profiling.
+ ProfilingType new_profiling_type =
+ static_cast<ProfilingType>(unsafe_goal->profiling_type);
- // 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);
+ if (last_profiling_type_ != new_profiling_type) {
+ // Reset the height/angle profiles.
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << arm_goal_, arm_goal_velocity_;
+ arm_profile_.MoveCurrentState(current);
+ current << elevator_goal_, elevator_goal_velocity_;
+ elevator_profile_.MoveCurrentState(current);
- // 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);
+ // Reset the x/y profiles.
+ aos::util::ElevatorArmKinematics::KinematicResult x_y_result;
+ kinematics_.ForwardKinematic(elevator_goal_, arm_goal_,
+ elevator_goal_velocity_,
+ arm_goal_velocity_, &x_y_result);
+ current << x_y_result.fridge_x, x_y_result.fridge_x_velocity;
+ x_profile_.MoveCurrentState(current);
+ current << x_y_result.fridge_h, x_y_result.fridge_h_velocity;
+ y_profile_.MoveCurrentState(current);
+
+ last_profiling_type_ = new_profiling_type;
+ }
+
+ if (new_profiling_type == ProfilingType::ANGLE_HEIGHT_PROFILING) {
+ // 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);
+ } else if (new_profiling_type == ProfilingType::X_Y_PROFILING) {
+ // Use x/y profiling
+ aos::util::ElevatorArmKinematics::KinematicResult kinematic_result;
+
+ x_profile_.set_maximum_velocity(
+ UseUnlessZero(unsafe_goal->max_x_velocity, 0.5));
+ x_profile_.set_maximum_acceleration(
+ UseUnlessZero(unsafe_goal->max_x_acceleration, 2.0));
+ y_profile_.set_maximum_velocity(
+ UseUnlessZero(unsafe_goal->max_y_velocity, 0.50));
+ y_profile_.set_maximum_acceleration(
+ UseUnlessZero(unsafe_goal->max_y_acceleration, 2.0));
+
+ // Limit the goals before we update the profiles.
+ kinematics_.InverseKinematic(
+ unsafe_goal->x, unsafe_goal->y, unsafe_goal->x_velocity,
+ unsafe_goal->y_velocity, &kinematic_result);
+
+ // Use the profiles to limit the x movements.
+ ::Eigen::Matrix<double, 2, 1> x_goal_state = x_profile_.Update(
+ kinematic_result.fridge_x, kinematic_result.fridge_x_velocity);
+
+ // Use the profiles to limit the y movements.
+ ::Eigen::Matrix<double, 2, 1> y_goal_state = y_profile_.Update(
+ kinematic_result.fridge_h, kinematic_result.fridge_h_velocity);
+
+ // Convert x/y goal states into arm/elevator goals.
+ // The inverse kinematics functions automatically perform range
+ // checking and adjust the results so that they're always valid.
+ kinematics_.InverseKinematic(x_goal_state(0, 0), y_goal_state(0, 0),
+ x_goal_state(1, 0), y_goal_state(1, 0),
+ &kinematic_result);
+
+ // Store the appropriate inverse kinematic results in the
+ // arm/elevator goals.
+ arm_goal_ = kinematic_result.arm_angle;
+ arm_goal_velocity_ = kinematic_result.arm_velocity;
+
+ elevator_goal_ = kinematic_result.elevator_height;
+ elevator_goal_velocity_ = kinematic_result.arm_velocity;
+ } else {
+ LOG(ERROR, "Unknown profiling_type: %d\n",
+ unsafe_goal->profiling_type);
+ }
}
// Update state_ to accurately represent the state of the zeroing
@@ -516,8 +594,8 @@
}
// Set the goals.
- arm_loop_->mutable_R() << arm_goal_, arm_goal_velocity, 0.0, 0.0, 0.0;
- elevator_loop_->mutable_R() << elevator_goal_, elevator_goal_velocity, 0.0,
+ arm_loop_->mutable_R() << arm_goal_, arm_goal_velocity_, 0.0, 0.0, 0.0;
+ elevator_loop_->mutable_R() << elevator_goal_, elevator_goal_velocity_, 0.0,
0.0;
const double max_voltage = state_ == RUNNING ? 12.0 : kZeroingVoltage;
@@ -575,9 +653,9 @@
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_angular_velocity = arm_goal_velocity_;
status->goal_height = elevator_goal_;
- status->goal_velocity = elevator_goal_velocity;
+ status->goal_velocity = elevator_goal_velocity_;
if (unsafe_goal) {
status->grabbers = unsafe_goal->grabbers;
} else {
diff --git a/frc971/control_loops/fridge/fridge.h b/frc971/control_loops/fridge/fridge.h
index 5761c79..d1fbc68 100644
--- a/frc971/control_loops/fridge/fridge.h
+++ b/frc971/control_loops/fridge/fridge.h
@@ -8,6 +8,7 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/fridge/fridge.q.h"
#include "frc971/zeroing/zeroing.h"
+#include "aos/common/util/kinematics.h"
namespace frc971 {
namespace control_loops {
@@ -61,6 +62,13 @@
ESTOP = 5,
};
+ enum class ProfilingType : int32_t {
+ // Use angle/height to specify the fridge goal.
+ ANGLE_HEIGHT_PROFILING = 0,
+ // Use x/y co-ordinates to specify the fridge goal.
+ X_Y_PROFILING = 1,
+ };
+
State state() const { return state_; }
protected:
@@ -139,13 +147,22 @@
double elevator_goal_ = 0.0;
double arm_goal_ = 0.0;
+ double arm_goal_velocity_ = 0.0;
+ double elevator_goal_velocity_ = 0.0;
+
State state_ = UNINITIALIZED;
State last_state_ = UNINITIALIZED;
control_loops::FridgeQueue::Position current_position_;
+ ProfilingType last_profiling_type_;
+ aos::util::ElevatorArmKinematics kinematics_;
+
aos::util::TrapezoidProfile arm_profile_;
aos::util::TrapezoidProfile elevator_profile_;
+
+ aos::util::TrapezoidProfile x_profile_;
+ aos::util::TrapezoidProfile y_profile_;
};
} // namespace control_loops
diff --git a/frc971/control_loops/fridge/fridge.q b/frc971/control_loops/fridge/fridge.q
index 2337921..837cb69 100644
--- a/frc971/control_loops/fridge/fridge.q
+++ b/frc971/control_loops/fridge/fridge.q
@@ -24,7 +24,18 @@
// In this case, zero is where the portion of the carriage that Spencer
// removed lines up with the bolt.
+ // X/Y positions are distances (in meters) the fridge is away from its origin
+ // position. Origin is considered at a height of zero and an angle of zero.
+ // Positive X positions are towards the front of the robot and negative X is
+ // towards the back of the robot (which is where we score).
+ // Y is positive going up and negative when it goes below the origin.
+
message Goal {
+ // Profile type.
+ // Set to 0 for angle/height profiling.
+ // Set to 1 for x/y profiling.
+ int32_t profiling_type;
+
// Angle of the arm.
double angle;
// Height of the elevator.
@@ -45,6 +56,26 @@
// Maximum elevator profile acceleration or 0 for the default.
double max_acceleration;
+ // X position of the fridge.
+ double x;
+ // Y position of the fridge.
+ double y;
+
+ // Velocity of the x position of the fridge.
+ double x_velocity;
+ // Velocity of the y position of the fridge.
+ double y_velocity;
+
+ // Maximum x profile velocity or 0 for the default.
+ double max_x_velocity;
+ // Maximum y profile velocity or 0 for the default.
+ double max_y_velocity;
+
+ // Maximum x profile acceleration or 0 for the default.
+ double max_x_acceleration;
+ // Maximum y profile acceleration or 0 for the default.
+ double max_y_acceleration;
+
// TODO(austin): Do I need acceleration here too?
GrabberPistons grabbers;
diff --git a/frc971/control_loops/fridge/fridge_lib_test.cc b/frc971/control_loops/fridge/fridge_lib_test.cc
index 712c650..b5e7bbe 100644
--- a/frc971/control_loops/fridge/fridge_lib_test.cc
+++ b/frc971/control_loops/fridge/fridge_lib_test.cc
@@ -10,6 +10,7 @@
#include "aos/common/time.h"
#include "aos/common/commonmath.h"
#include "aos/common/controls/control_loop_test.h"
+#include "aos/common/util/kinematics.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/fridge/arm_motor_plant.h"
#include "frc971/control_loops/fridge/elevator_motor_plant.h"
@@ -216,7 +217,12 @@
".frc971.control_loops.fridge_queue.output",
".frc971.control_loops.fridge_queue.status"),
fridge_(&fridge_queue_),
- fridge_plant_() {
+ fridge_plant_(),
+ kinematics_(constants::GetValues().fridge.arm_length,
+ constants::GetValues().fridge.elevator.upper_limit,
+ constants::GetValues().fridge.elevator.lower_limit,
+ constants::GetValues().fridge.arm.upper_limit,
+ constants::GetValues().fridge.arm.lower_limit) {
set_team_id(kTeamNumber);
}
@@ -225,9 +231,21 @@
fridge_queue_.status.FetchLatest();
EXPECT_TRUE(fridge_queue_.goal.get() != nullptr);
EXPECT_TRUE(fridge_queue_.status.get() != nullptr);
- EXPECT_NEAR(fridge_queue_.goal->angle, fridge_queue_.status->angle, 0.001);
- EXPECT_NEAR(fridge_queue_.goal->height, fridge_queue_.status->height,
- 0.001);
+ if (fridge_queue_.goal->profiling_type == 0) {
+ EXPECT_NEAR(fridge_queue_.goal->angle, fridge_queue_.status->angle,
+ 0.001);
+ EXPECT_NEAR(fridge_queue_.goal->height, fridge_queue_.status->height,
+ 0.001);
+ } else if (fridge_queue_.goal->profiling_type == 1) {
+ aos::util::ElevatorArmKinematics::KinematicResult x_y_status;
+ kinematics_.ForwardKinematic(fridge_queue_.status->height,
+ fridge_queue_.status->angle, 0.0, 0.0, &x_y_status);
+ EXPECT_NEAR(fridge_queue_.goal->x, x_y_status.fridge_x, 0.001);
+ EXPECT_NEAR(fridge_queue_.goal->y, x_y_status.fridge_h, 0.001);
+ } else {
+ // Unhandled profiling type.
+ EXPECT_TRUE(false);
+ }
}
// Runs one iteration of the whole simulation and checks that separation
@@ -258,6 +276,8 @@
// Create a control loop and simulation.
Fridge fridge_;
FridgeSimulation fridge_plant_;
+
+ aos::util::ElevatorArmKinematics kinematics_;
};
// Tests that the loop does nothing when the goal is zero.
@@ -266,9 +286,9 @@
// that the controller does nothing.
const auto &values = constants::GetValues();
ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
- .angle(0.0)
- .height(values.fridge.elevator.lower_limit)
- .Send());
+ .angle(0.0)
+ .height(values.fridge.elevator.lower_limit)
+ .Send());
// Run a few iterations.
RunForTime(Time::InMS(5000));
@@ -277,14 +297,37 @@
}
// Tests that the loop can reach a goal.
+TEST_F(FridgeTest, ReachesXYGoal) {
+ // Set a reasonable goal.
+ const auto &values = constants::GetValues();
+ const double soft_limit = values.fridge.elevator.lower_limit;
+ const double height = soft_limit + 0.4;
+ const double angle = M_PI / 6.0;
+
+ aos::util::ElevatorArmKinematics::KinematicResult x_y_goals;
+ kinematics_.ForwardKinematic(height, angle, 0.0, 0.0, &x_y_goals);
+
+ ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
+ .profiling_type(1)
+ .x(x_y_goals.fridge_x)
+ .y(x_y_goals.fridge_h)
+ .Send());
+
+ // Give it a lot of time to get there.
+ RunForTime(Time::InMS(4000));
+
+ VerifyNearGoal();
+}
+
+// Tests that the loop can reach a goal.
TEST_F(FridgeTest, ReachesGoal) {
// Set a reasonable goal.
const auto &values = constants::GetValues();
const double soft_limit = values.fridge.elevator.lower_limit;
ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
- .angle(M_PI / 4.0)
- .height(soft_limit + 0.2)
- .Send());
+ .angle(M_PI / 4.0)
+ .height(soft_limit + 0.2)
+ .Send());
// Give it a lot of time to get there.
RunForTime(Time::InMS(4000));
@@ -298,10 +341,8 @@
// Put the arm up to get it out of the way.
// We're going to send the elevator to -5, which should be significantly too
// low.
- ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
- .angle(M_PI)
- .height(-5.0)
- .Send());
+ ASSERT_TRUE(
+ fridge_queue_.goal.MakeWithBuilder().angle(M_PI).height(-5.0).Send());
RunForTime(Time::InMS(4000));
@@ -315,10 +356,8 @@
// Put the arm down to get it out of the way.
// We're going to give the elevator some ridiculously high goal.
- ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
- .angle(-M_PI)
- .height(50.0)
- .Send());
+ ASSERT_TRUE(
+ fridge_queue_.goal.MakeWithBuilder().angle(-M_PI).height(50.0).Send());
RunForTime(Time::InMS(4000));
@@ -645,9 +684,10 @@
}
// Phil:
-// TODO(austin): Check that we e-stop if encoder index pulse is not n revolutions away from last one. (got extra counts from noise, etc).
-// TODO(austin): Check that we e-stop if pot disagrees too much with encoder after we are zeroed.
-
+// TODO(austin): Check that we e-stop if encoder index pulse is not n
+// revolutions away from last one. (got extra counts from noise, etc).
+// TODO(austin): Check that we e-stop if pot disagrees too much with encoder
+// after we are zeroed.
} // namespace testing
} // namespace control_loops