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/actors/actors.gyp b/frc971/actors/actors.gyp
index b2f93a5..022a924 100644
--- a/frc971/actors/actors.gyp
+++ b/frc971/actors/actors.gyp
@@ -110,16 +110,18 @@
'score_actor.cc',
],
'dependencies': [
- 'fridge_profile_lib',
'score_action_queue',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/actions/actions.gyp:action_lib',
'<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/control_loops/fridge/fridge.gyp:fridge_queue',
+ '<(EXTERNALS):eigen',
],
'export_dependent_settings': [
- 'fridge_profile_lib',
'<(AOS)/common/actions/actions.gyp:action_lib',
'score_action_queue',
+ '<(EXTERNALS):eigen',
],
},
{
diff --git a/frc971/actors/score_action.q b/frc971/actors/score_action.q
index 3e99a75..f8401d2 100644
--- a/frc971/actors/score_action.q
+++ b/frc971/actors/score_action.q
@@ -4,8 +4,10 @@
// Parameters to send with start.
struct ScoreParams {
- // Height for the elevator to be when we're scoring.
+ // Height for the upper fridge pivot to be when we're scoring.
double height;
+ // If false, then extend. Otherwise, place the stack and retract.
+ bool place_the_stack;
};
queue_group ScoreActionQueueGroup {
diff --git a/frc971/actors/score_actor.cc b/frc971/actors/score_actor.cc
index 85c3eff..07bfdd7 100644
--- a/frc971/actors/score_actor.cc
+++ b/frc971/actors/score_actor.cc
@@ -1,47 +1,228 @@
#include "frc971/actors/score_actor.h"
-#include <math.h>
+#include <cmath>
#include "aos/common/controls/control_loop.h"
#include "aos/common/logging/logging.h"
#include "aos/common/util/phased_loop.h"
-#include "frc971/actors/fridge_profile_lib.h"
#include "frc971/constants.h"
+#include "frc971/control_loops/fridge/fridge.q.h"
+
+using ::frc971::control_loops::fridge_queue;
namespace frc971 {
namespace actors {
namespace {
-constexpr ProfileParams kElevatorMove{0.5, 2.0};
-constexpr ProfileParams kArmMove{0.5, 1.0};
+const double kUpperMoveHeight = 0.2;
+const double kBeginHorizontalMoveHeight = 0.04;
+const double kHorizontalMoveTarget = -0.6;
+const double kClearedStopperDistance = 0.2;
+const double kAboveFloorHeight = 0.1;
+const double kPlaceHeight = -0.05;
+const double kHomeReturnHeight = 0.1;
} // namespace
ScoreActor::ScoreActor(ScoreActionQueueGroup* queues)
- : FridgeActorBase<ScoreActionQueueGroup>(queues) {}
+ : aos::common::actions::ActorBase<ScoreActionQueueGroup>(queues),
+ 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) {}
bool ScoreActor::RunAction(const ScoreParams& params) {
- const auto& values = constants::GetValues();
+ // TODO(pschrader): Break these into completely separate actions.
+ if (params.place_the_stack) {
+ return PlaceTheStack(params);
+ } else {
+ return MoveStackIntoPosition(params);
+ }
+}
- // We're going to move the elevator first so we don't crash the fridge into
- // the ground.
- DoFridgeProfile(values.fridge.arm_zeroing_height, 0.0, kElevatorMove,
- kArmMove, true);
- if (ShouldCancel()) return true;
- // Now move them both together.
- DoFridgeProfile(params.height, M_PI / 2.0, kElevatorMove, kArmMove, true);
- if (ShouldCancel()) return true;
- // Release the totes.
- DoFridgeProfile(values.fridge.arm_zeroing_height, 0.0, kElevatorMove,
- kArmMove, false);
- if (ShouldCancel()) return true;
- // Retract. Move back to our lowered position.
- DoFridgeProfile(values.fridge.elevator.lower_limit, 0.0, kElevatorMove,
- kArmMove, false);
+bool ScoreActor::MoveStackIntoPosition(const ScoreParams& /*params*/) {
+ // TODO(pschrader): Assuming right now we start at 0, 0.
+
+ // Move the fridge up a little bit.
+ if (!SendGoal(0.0, kUpperMoveHeight, true)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+
+ while (true) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ if (ShouldCancel()) {
+ return true;
+ }
+
+ // Once we're above a certain height we can start moving backwards.
+ if (CurrentHeight() > kBeginHorizontalMoveHeight) {
+ break;
+ }
+ }
+
+ // Move the fridge over.
+ if (!SendGoal(kHorizontalMoveTarget, kUpperMoveHeight, true)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+
+ while (true) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ if (ShouldCancel()) {
+ return true;
+ }
+
+ // Once we've cleared the tote stopper, then we can start moving down.
+ if (CurrentX() > kClearedStopperDistance) {
+ break;
+ }
+ }
+
+ // Once we're a bit out, start moving the fridge down.
+ if (!SendGoal(kHorizontalMoveTarget, kAboveFloorHeight, true)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+
+ while (true) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ if (ShouldCancel()) {
+ return true;
+ }
+
+ // Wait until we get to just the right height.
+ if (NearGoal(kHorizontalMoveTarget, kAboveFloorHeight)) {
+ break;
+ }
+ }
+
if (ShouldCancel()) return true;
return true;
}
+bool ScoreActor::PlaceTheStack(const ScoreParams& /*params*/) {
+ // Once the fridge is way out, put it on the ground.
+ if (!SendGoal(kHorizontalMoveTarget, kPlaceHeight, true)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+
+ while (true) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ if (ShouldCancel()) {
+ return true;
+ }
+
+ // Once we're on the ground, move on to releasing the grabber.
+ if (NearGoal(kHorizontalMoveTarget, kPlaceHeight)) {
+ break;
+ }
+ }
+
+ if (ShouldCancel()) return true;
+
+ // Release the grabbers.
+ if (!SendGoal(kHorizontalMoveTarget, kPlaceHeight, false)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+
+ if (ShouldCancel()) return true;
+
+ // Go back to the home position.
+ if (!SendGoal(0.0, kHomeReturnHeight, false)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+
+ while (true) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ if (ShouldCancel()) {
+ return true;
+ }
+
+ if (NearGoal(0.0, kHomeReturnHeight)) {
+ break;
+ }
+ }
+
+ return true;
+}
+
+double ScoreActor::CurrentHeight() {
+ fridge_queue.status.FetchLatest();
+
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return false;
+ }
+
+ ::aos::util::ElevatorArmKinematics::KinematicResult results;
+ kinematics_.ForwardKinematic(fridge_queue.status->height,
+ fridge_queue.status->angle, 0.0, 0.0, &results);
+
+ return results.fridge_h;
+}
+
+double ScoreActor::CurrentX() {
+ fridge_queue.status.FetchLatest();
+
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return false;
+ }
+
+ ::aos::util::ElevatorArmKinematics::KinematicResult results;
+ kinematics_.ForwardKinematic(fridge_queue.status->height,
+ fridge_queue.status->angle, 0.0, 0.0, &results);
+
+ return results.fridge_x;
+}
+
+bool ScoreActor::SendGoal(double x, double y, bool grabbers_enabled) {
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+
+ // new_fridge_goal->max_velocity = elevator_parameters.velocity;
+ // new_fridge_goal->max_acceleration = elevator_parameters.acceleration;
+ new_fridge_goal->x = x;
+ new_fridge_goal->y = y;
+ new_fridge_goal->profiling_type = 1;
+ // new_fridge_goal->velocity = velocity;
+ // new_fridge_goal->max_angular_velocity = arm_parameters.velocity;
+ // new_fridge_goal->max_angular_acceleration = arm_parameters.acceleration;
+ // new_fridge_goal->angle = angle;
+ new_fridge_goal->angular_velocity = 0.0;
+ new_fridge_goal->grabbers.top_front = grabbers_enabled;
+ new_fridge_goal->grabbers.top_back = grabbers_enabled;
+ new_fridge_goal->grabbers.bottom_front = grabbers_enabled;
+ new_fridge_goal->grabbers.bottom_back = grabbers_enabled;
+
+ return new_fridge_goal.Send();
+}
+
+bool ScoreActor::NearGoal(double x, double y) {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return false;
+ }
+
+ ::aos::util::ElevatorArmKinematics::KinematicResult results;
+ ::aos::util::ElevatorArmKinematics::KinematicResult goal_results;
+ kinematics_.ForwardKinematic(fridge_queue.status->height,
+ fridge_queue.status->angle, 0.0, 0.0, &results);
+ kinematics_.ForwardKinematic(fridge_queue.status->goal_height,
+ fridge_queue.status->goal_angle, 0.0, 0.0,
+ &goal_results);
+
+ return (::std::abs(results.fridge_x - x) < 0.020 &&
+ ::std::abs(results.fridge_h - y) < 0.020 &&
+ ::std::abs(goal_results.fridge_x - x) < 0.0001 &&
+ ::std::abs(goal_results.fridge_h - y) < 0.0001);
+}
+
::std::unique_ptr<ScoreAction> MakeScoreAction(const ScoreParams& params) {
return ::std::unique_ptr<ScoreAction>(
new ScoreAction(&::frc971::actors::score_action, params));
diff --git a/frc971/actors/score_actor.h b/frc971/actors/score_actor.h
index 35b30af..8d805bd 100644
--- a/frc971/actors/score_actor.h
+++ b/frc971/actors/score_actor.h
@@ -3,25 +3,28 @@
#include "aos/common/actions/actions.h"
#include "aos/common/actions/actor.h"
+#include "aos/common/util/kinematics.h"
#include "frc971/actors/score_action.q.h"
-#include "frc971/actors/fridge_profile_lib.h"
namespace frc971 {
namespace actors {
-class ScoreActor : public FridgeActorBase<ScoreActionQueueGroup> {
+class ScoreActor
+ : public ::aos::common::actions::ActorBase<ScoreActionQueueGroup> {
public:
explicit ScoreActor(ScoreActionQueueGroup *queues);
bool RunAction(const ScoreParams ¶ms) override;
private:
- // Creates and runs a profile action for the fridge. Handles cancelling
- // correctly.
- // height: How high we want the fridge to go.
- // angle: What angle we want the arm to be at.
- // grabbers: Whether we want the grabbers deployed or not.
- void DoProfile(double height, double angle, bool grabbers);
+
+ ::aos::util::ElevatorArmKinematics kinematics_;
+ bool NearGoal(double x, double y);
+ bool PlaceTheStack(const ScoreParams ¶ms);
+ bool MoveStackIntoPosition(const ScoreParams ¶ms);
+ bool SendGoal(double x, double y, bool grabbers_enabled);
+ double CurrentHeight();
+ double CurrentX();
};
typedef aos::common::actions::TypedAction<ScoreActionQueueGroup> ScoreAction;
diff --git a/frc971/actors/score_actor_test.cc b/frc971/actors/score_actor_test.cc
index d8eb041..3d0c6c3 100644
--- a/frc971/actors/score_actor_test.cc
+++ b/frc971/actors/score_actor_test.cc
@@ -39,17 +39,17 @@
// Tests that cancel stops not only the score action, but also the underlying
// profile action.
-TEST_F(ScoreActionTest, ScoreCancel) {
+TEST_F(ScoreActionTest, PlaceTheStackCancel) {
ScoreActor score(&frc971::actors::score_action);
frc971::actors::score_action.goal.MakeWithBuilder().run(true).Send();
// Tell it the fridge is zeroed.
- control_loops::fridge_queue.status.MakeWithBuilder()
- .zeroed(true)
- .angle(0.0)
- .height(0.0)
- .Send();
+ ASSERT_TRUE(control_loops::fridge_queue.status.MakeWithBuilder()
+ .zeroed(true)
+ .angle(0.0)
+ .height(0.0)
+ .Send());
// do the action and it will post to the goal queue
score.WaitForActionRequest();
@@ -59,7 +59,37 @@
frc971::actors::score_action.goal.MakeWithBuilder().run(false).Send();
// let the action start running, if we return from this call it has worked.
- const ScoreParams params = {0.75};
+ bool place_the_stack = true;
+ const ScoreParams params = {0.75, place_the_stack};
+ score.RunAction(params);
+
+ SUCCEED();
+}
+
+// Tests that cancel stops not only the score action, but also the underlying
+// profile action.
+TEST_F(ScoreActionTest, MoveStackIntoPositionCancel) {
+ ScoreActor score(&frc971::actors::score_action);
+
+ frc971::actors::score_action.goal.MakeWithBuilder().run(true).Send();
+
+ // Tell it the fridge is zeroed.
+ ASSERT_TRUE(control_loops::fridge_queue.status.MakeWithBuilder()
+ .zeroed(true)
+ .angle(0.0)
+ .height(0.0)
+ .Send());
+
+ // do the action and it will post to the goal queue
+ score.WaitForActionRequest();
+
+ // the action has started, so now cancel it and it should cancel
+ // the underlying profile
+ frc971::actors::score_action.goal.MakeWithBuilder().run(false).Send();
+
+ // let the action start running, if we return from this call it has worked.
+ bool place_the_stack = false;
+ const ScoreParams params = {0.75, place_the_stack};
score.RunAction(params);
SUCCEED();
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 4b4365a..f9d1db4 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -121,6 +121,9 @@
const int kZeroingSampleSize = 200;
+// The length of the arm.
+const double kArmLength = 0.7366;
+
// TODO(danielp): All these values might need to change.
const double kClawPistonSwitchTime = 0.4;
const double kClawZeroingRange = 0.3;
@@ -186,6 +189,7 @@
3.5 - 0.17 - -0.15,
kArmZeroingHeight,
+ kArmLength,
},
// End "sensor" values.
@@ -255,6 +259,7 @@
3.506927 - 0.170017 - -0.147970,
kArmZeroingHeight,
+ kArmLength,
},
// End "sensor" values.
@@ -326,6 +331,7 @@
3.506927 - 0.170017 - -0.147970 - 0.005045 - -0.026504,
kArmZeroingHeight,
+ kArmLength,
},
// TODO(sensors): End "sensor" values.
diff --git a/frc971/constants.h b/frc971/constants.h
index fe47418..0b6b725 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -111,6 +111,10 @@
// How high the elevator has to be before we start zeroing the arm.
double arm_zeroing_height;
+
+ // The length of the arm, from the axis of the bottom pivot to the axis of
+ // the top pivot.
+ double arm_length;
};
Fridge fridge;
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