Add roll joint to superstructure and arm UI
Arm UI changes:
- Update robot dimensions
- Support visualizing roll joint
- Add roll joint collision detection
Superstructure changes:
- Adding roll joint feedback loop and zeroing estimator
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I422e343890248940bba98ba3cabac94e68723a3e
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index f5eff79..52254e4 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -117,6 +117,7 @@
"//frc971/control_loops:subsystem_simulator",
"//frc971/control_loops:team_number_test_environment",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//y2023/control_loops/superstructure/roll:roll_plants",
],
)
diff --git a/y2023/control_loops/superstructure/arm/BUILD b/y2023/control_loops/superstructure/arm/BUILD
index 0ef2cf9..032c654 100644
--- a/y2023/control_loops/superstructure/arm/BUILD
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -10,15 +10,15 @@
visibility = ["//visibility:public"],
deps = [
":generated_graph",
- "//frc971/control_loops/double_jointed_arm:demo_path",
"//frc971/control_loops/double_jointed_arm:ekf",
"//frc971/control_loops/double_jointed_arm:graph",
- "//frc971/control_loops/double_jointed_arm:trajectory",
"//frc971/zeroing",
"//y2023:constants",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
"//y2023/control_loops/superstructure:superstructure_status_fbs",
"//y2023/control_loops/superstructure/arm:arm_constants",
+ "//y2023/control_loops/superstructure/arm:trajectory",
+ "//y2023/control_loops/superstructure/roll:roll_plants",
],
)
@@ -49,7 +49,8 @@
deps = [
":arm_constants",
"//frc971/control_loops/double_jointed_arm:graph",
- "//frc971/control_loops/double_jointed_arm:trajectory",
+ "//y2023/control_loops/superstructure/arm:trajectory",
+ "//y2023/control_loops/superstructure/roll:roll_plants",
],
)
diff --git a/y2023/control_loops/superstructure/arm/arm.cc b/y2023/control_loops/superstructure/arm/arm.cc
index 73781d2..5417dc8 100644
--- a/y2023/control_loops/superstructure/arm/arm.cc
+++ b/y2023/control_loops/superstructure/arm/arm.cc
@@ -1,5 +1,8 @@
#include "y2023/control_loops/superstructure/arm/arm.h"
+#include "y2023/control_loops/superstructure/roll/integral_hybrid_roll_plant.h"
+#include "y2023/control_loops/superstructure/roll/integral_roll_plant.h"
+
namespace y2023 {
namespace control_loops {
namespace superstructure {
@@ -18,20 +21,24 @@
state_(ArmState::UNINITIALIZED),
proximal_zeroing_estimator_(values_->arm_proximal.zeroing),
distal_zeroing_estimator_(values_->arm_distal.zeroing),
+ roll_joint_zeroing_estimator_(values_->roll_joint.zeroing),
proximal_offset_(0.0),
distal_offset_(0.0),
- max_intake_override_(1000.0),
- alpha_unitizer_((::Eigen::Matrix<double, 2, 2>() << 1.0 / kAlpha0Max(),
- 0.0, 0.0, 1.0 / kAlpha1Max())
+ roll_joint_offset_(0.0),
+ alpha_unitizer_((::Eigen::DiagonalMatrix<double, 3>().diagonal()
+ << (1.0 / kAlpha0Max()),
+ (1.0 / kAlpha1Max()), (1.0 / kAlpha2Max()))
.finished()),
dynamics_(kArmConstants),
- search_graph_(MakeSearchGraph(&dynamics_, &trajectories_, alpha_unitizer_,
- kVMax())),
close_enough_for_full_power_(false),
brownout_count_(0),
+ roll_joint_loop_(roll::MakeIntegralRollLoop()),
+ hybrid_roll_joint_loop_(roll::MakeIntegralHybridRollLoop()),
arm_ekf_(&dynamics_),
+ search_graph_(MakeSearchGraph(&dynamics_, &trajectories_, alpha_unitizer_,
+ kVMax(), &hybrid_roll_joint_loop_)),
// Go to the start of the first trajectory.
- follower_(&dynamics_, NeutralPosPoint()),
+ follower_(&dynamics_, &hybrid_roll_joint_loop_, NeutralPosPoint()),
points_(PointList()),
current_node_(0) {
int i = 0;
@@ -48,26 +55,32 @@
const ::aos::monotonic_clock::time_point /*monotonic_now*/,
const uint32_t *unsafe_goal, const superstructure::ArmPosition *position,
bool trajectory_override, double *proximal_output, double *distal_output,
- bool /*intake*/, bool /*spit*/, flatbuffers::FlatBufferBuilder *fbb) {
- ::Eigen::Matrix<double, 2, 1> Y;
+ double *roll_joint_output, bool /*intake*/, bool /*spit*/,
+ flatbuffers::FlatBufferBuilder *fbb) {
const bool outputs_disabled =
- ((proximal_output == nullptr) || (distal_output == nullptr));
+ ((proximal_output == nullptr) || (distal_output == nullptr) ||
+ (roll_joint_output == nullptr));
if (outputs_disabled) {
++brownout_count_;
} else {
brownout_count_ = 0;
}
- uint32_t filtered_goal = 0;
+ // TODO(milind): should we default to the closest position?
+ uint32_t filtered_goal = arm::NeutralPosIndex();
if (unsafe_goal != nullptr) {
filtered_goal = *unsafe_goal;
}
- Y << position->proximal()->encoder() + proximal_offset_,
+ ::Eigen::Matrix<double, 2, 1> Y_arm;
+ Y_arm << position->proximal()->encoder() + proximal_offset_,
position->distal()->encoder() + distal_offset_;
+ ::Eigen::Matrix<double, 1, 1> Y_roll_joint;
+ Y_roll_joint << position->roll_joint()->encoder() + roll_joint_offset_;
proximal_zeroing_estimator_.UpdateEstimate(*position->proximal());
distal_zeroing_estimator_.UpdateEstimate(*position->distal());
+ roll_joint_zeroing_estimator_.UpdateEstimate(*position->roll_joint());
if (proximal_output != nullptr) {
*proximal_output = 0.0;
@@ -75,15 +88,21 @@
if (distal_output != nullptr) {
*distal_output = 0.0;
}
+ if (roll_joint_output != nullptr) {
+ *roll_joint_output = 0.0;
+ }
- arm_ekf_.Correct(Y, kDt());
+ arm_ekf_.Correct(Y_arm, kDt());
+ roll_joint_loop_.Correct(Y_roll_joint);
if (::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) <= 0.05 &&
- ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) <= 0.05) {
+ ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) <= 0.05 &&
+ ::std::abs(roll_joint_loop_.X_hat(0) - follower_.theta(2)) <= 0.05) {
close_enough_for_full_power_ = true;
}
if (::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) >= 1.10 ||
- ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) >= 1.10) {
+ ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) >= 1.10 ||
+ ::std::abs(roll_joint_loop_.X_hat(0) - follower_.theta(2)) >= 0.50) {
close_enough_for_full_power_ = false;
}
@@ -94,25 +113,31 @@
state_ = ArmState::ZEROING;
proximal_zeroing_estimator_.Reset();
distal_zeroing_estimator_.Reset();
+ roll_joint_zeroing_estimator_.Reset();
break;
case ArmState::ZEROING:
// Zero by not moving.
- if (proximal_zeroing_estimator_.zeroed() &&
- distal_zeroing_estimator_.zeroed()) {
+ if (zeroed()) {
state_ = ArmState::DISABLED;
proximal_offset_ = proximal_zeroing_estimator_.offset();
distal_offset_ = distal_zeroing_estimator_.offset();
+ roll_joint_offset_ = roll_joint_zeroing_estimator_.offset();
- Y << position->proximal()->encoder() + proximal_offset_,
+ Y_arm << position->proximal()->encoder() + proximal_offset_,
position->distal()->encoder() + distal_offset_;
+ Y_roll_joint << position->roll_joint()->encoder() + roll_joint_offset_;
// TODO(austin): Offset ekf rather than reset it. Since we aren't
// moving at this point, it's pretty safe to do this.
- ::Eigen::Matrix<double, 4, 1> X;
- X << Y(0), 0.0, Y(1), 0.0;
- arm_ekf_.Reset(X);
+ ::Eigen::Matrix<double, 4, 1> X_arm;
+ X_arm << Y_arm(0), 0.0, Y_arm(1), 0.0;
+ arm_ekf_.Reset(X_arm);
+
+ ::Eigen::Matrix<double, 3, 1> X_roll_joint;
+ X_roll_joint << Y_roll_joint(0), 0.0, 0.0;
+ roll_joint_loop_.mutable_X_hat() = X_roll_joint;
} else {
break;
}
@@ -122,14 +147,14 @@
follower_.SwitchTrajectory(nullptr);
close_enough_for_full_power_ = false;
- const ::Eigen::Matrix<double, 2, 1> current_theta =
- (::Eigen::Matrix<double, 2, 1>() << arm_ekf_.X_hat(0),
- arm_ekf_.X_hat(2))
+ const ::Eigen::Matrix<double, 3, 1> current_theta =
+ (::Eigen::Matrix<double, 3, 1>() << arm_ekf_.X_hat(0),
+ arm_ekf_.X_hat(2), roll_joint_loop_.X_hat(0))
.finished();
uint32_t best_index = 0;
double best_distance = (points_[0] - current_theta).norm();
uint32_t current_index = 0;
- for (const ::Eigen::Matrix<double, 2, 1> &point : points_) {
+ for (const ::Eigen::Matrix<double, 3, 1> &point : points_) {
const double new_distance = (point - current_theta).norm();
if (new_distance < best_distance) {
best_distance = new_distance;
@@ -165,7 +190,8 @@
// ESTOP if we hit the hard limits.
// TODO(austin): Pick some sane limits.
if (proximal_zeroing_estimator_.error() ||
- distal_zeroing_estimator_.error()) {
+ distal_zeroing_estimator_.error() ||
+ roll_joint_zeroing_estimator_.error()) {
AOS_LOG(ERROR, "Zeroing error ESTOP\n");
state_ = ArmState::ESTOP;
} else if (outputs_disabled && brownout_count_ > kMaxBrownoutCount) {
@@ -227,38 +253,56 @@
close_enough_for_full_power_
? kOperatingVoltage()
: (state_ == ArmState::GOTO_PATH ? kGotoPathVMax() : kPathlessVMax());
- follower_.Update(arm_ekf_.X_hat(), disable, kDt(), vmax_,
- max_operating_voltage);
+ ::Eigen::Matrix<double, 9, 1> X_hat;
+ X_hat.block<6, 1>(0, 0) = arm_ekf_.X_hat();
+ X_hat.block<3, 1>(6, 0) = roll_joint_loop_.X_hat();
+
+ follower_.Update(X_hat, disable, kDt(), vmax_, max_operating_voltage);
AOS_LOG(INFO, "Max voltage: %f\n", max_operating_voltage);
+ arm_ekf_.Predict(follower_.U().head<2>(), kDt());
+ roll_joint_loop_.UpdateObserver(follower_.U().tail<1>(), kDtDuration());
+
flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
proximal_estimator_state_offset =
proximal_zeroing_estimator_.GetEstimatorState(fbb);
flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
distal_estimator_state_offset =
distal_zeroing_estimator_.GetEstimatorState(fbb);
+ flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ roll_joint_estimator_state_offset =
+ roll_joint_zeroing_estimator_.GetEstimatorState(fbb);
superstructure::ArmStatus::Builder status_builder(*fbb);
status_builder.add_proximal_estimator_state(proximal_estimator_state_offset);
status_builder.add_distal_estimator_state(distal_estimator_state_offset);
+ status_builder.add_roll_joint_estimator_state(
+ roll_joint_estimator_state_offset);
status_builder.add_goal_theta0(follower_.theta(0));
status_builder.add_goal_theta1(follower_.theta(1));
+ status_builder.add_goal_theta2(follower_.theta(2));
status_builder.add_goal_omega0(follower_.omega(0));
status_builder.add_goal_omega1(follower_.omega(1));
+ status_builder.add_goal_omega2(follower_.omega(2));
status_builder.add_theta0(arm_ekf_.X_hat(0));
status_builder.add_theta1(arm_ekf_.X_hat(2));
+ status_builder.add_theta2(roll_joint_loop_.X_hat(0));
status_builder.add_omega0(arm_ekf_.X_hat(1));
status_builder.add_omega1(arm_ekf_.X_hat(3));
+ status_builder.add_omega2(roll_joint_loop_.X_hat(1));
status_builder.add_voltage_error0(arm_ekf_.X_hat(4));
status_builder.add_voltage_error1(arm_ekf_.X_hat(5));
+ status_builder.add_voltage_error2(roll_joint_loop_.X_hat(2));
if (!disable) {
*proximal_output = ::std::max(
-kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(0)));
*distal_output = ::std::max(
-kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(1)));
+ *roll_joint_output = ::std::max(
+ -kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(2)));
}
status_builder.add_path_distance_to_go(follower_.path_distance_to_go());
@@ -269,7 +313,6 @@
status_builder.add_state(state_);
status_builder.add_failed_solutions(follower_.failed_solutions());
- arm_ekf_.Predict(follower_.U(), kDt());
return status_builder.Finish();
}
diff --git a/y2023/control_loops/superstructure/arm/arm.h b/y2023/control_loops/superstructure/arm/arm.h
index 27588f1..9cda7fc 100644
--- a/y2023/control_loops/superstructure/arm/arm.h
+++ b/y2023/control_loops/superstructure/arm/arm.h
@@ -5,15 +5,14 @@
#include "frc971/control_loops/double_jointed_arm/dynamics.h"
#include "frc971/control_loops/double_jointed_arm/ekf.h"
#include "frc971/control_loops/double_jointed_arm/graph.h"
-#include "frc971/control_loops/double_jointed_arm/trajectory.h"
#include "frc971/zeroing/zeroing.h"
#include "y2023/constants.h"
#include "y2023/control_loops/superstructure/arm/generated_graph.h"
+#include "y2023/control_loops/superstructure/arm/trajectory.h"
#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
using frc971::control_loops::arm::EKF;
-using frc971::control_loops::arm::TrajectoryFollower;
namespace y2023 {
namespace control_loops {
@@ -32,8 +31,13 @@
return kGrannyMode() ? 5.0 : 12.0;
}
static constexpr double kDt() { return 0.00505; }
+ static constexpr std::chrono::nanoseconds kDtDuration() {
+ return std::chrono::duration_cast<std::chrono::nanoseconds>(
+ std::chrono::duration<double>(kDt()));
+ }
static constexpr double kAlpha0Max() { return kGrannyMode() ? 5.0 : 15.0; }
static constexpr double kAlpha1Max() { return kGrannyMode() ? 5.0 : 15.0; }
+ static constexpr double kAlpha2Max() { return kGrannyMode() ? 5.0 : 15.0; }
static constexpr double kVMax() { return kGrannyMode() ? 5.0 : 11.5; }
static constexpr double kPathlessVMax() { return 5.0; }
@@ -43,7 +47,8 @@
const ::aos::monotonic_clock::time_point /*monotonic_now*/,
const uint32_t *unsafe_goal, const superstructure::ArmPosition *position,
bool trajectory_override, double *proximal_output, double *distal_output,
- bool /*intake*/, bool /*spit*/, flatbuffers::FlatBufferBuilder *fbb);
+ double *roll_joint_output, bool /*intake*/, bool /*spit*/,
+ flatbuffers::FlatBufferBuilder *fbb);
void Reset();
@@ -52,13 +57,10 @@
bool estopped() const { return state_ == ArmState::ESTOP; }
bool zeroed() const {
return (proximal_zeroing_estimator_.zeroed() &&
- distal_zeroing_estimator_.zeroed());
+ distal_zeroing_estimator_.zeroed() &&
+ roll_joint_zeroing_estimator_.zeroed());
}
- // Returns the maximum position for the intake. This is used to override the
- // intake position to release the box when the state machine is lifting.
- double max_intake_override() const { return max_intake_override_; }
-
uint32_t current_node() const { return current_node_; }
double path_distance_to_go() { return follower_.path_distance_to_go(); }
@@ -79,29 +81,36 @@
proximal_zeroing_estimator_;
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator
distal_zeroing_estimator_;
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator
+ roll_joint_zeroing_estimator_;
double proximal_offset_;
double distal_offset_;
+ double roll_joint_offset_;
- double max_intake_override_;
-
- const ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
+ const ::Eigen::DiagonalMatrix<double, 3> alpha_unitizer_;
double vmax_ = kVMax();
frc971::control_loops::arm::Dynamics dynamics_;
::std::vector<TrajectoryAndParams> trajectories_;
- SearchGraph search_graph_;
bool close_enough_for_full_power_;
size_t brownout_count_;
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackPlant<3, 1, 1>,
+ StateFeedbackObserver<3, 1, 1>>
+ roll_joint_loop_;
+ const StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>
+ hybrid_roll_joint_loop_;
EKF arm_ekf_;
+ SearchGraph search_graph_;
TrajectoryFollower follower_;
- const ::std::vector<::Eigen::Matrix<double, 2, 1>> points_;
+ const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
// Start at the 0th index.
uint32_t current_node_;
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index fa83d20..1ad2625 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -60,6 +60,7 @@
unsafe_goal != nullptr ? unsafe_goal->trajectory_override() : false,
output != nullptr ? &output_struct.proximal_voltage : nullptr,
output != nullptr ? &output_struct.distal_voltage : nullptr,
+ output != nullptr ? &output_struct.roll_joint_voltage : nullptr,
unsafe_goal != nullptr ? unsafe_goal->intake() : false,
unsafe_goal != nullptr ? unsafe_goal->spit() : false,
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index ae01f0b..f3f0471 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -9,6 +9,7 @@
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2023/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2023/control_loops/superstructure/roll/integral_roll_plant.h"
#include "y2023/control_loops/superstructure/superstructure.h"
DEFINE_string(output_folder, "",
@@ -50,6 +51,8 @@
&proximal_zeroing_constants,
const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
&distal_zeroing_constants,
+ const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ &roll_joint_zeroing_constants,
std::chrono::nanoseconds dt)
: proximal_zeroing_constants_(proximal_zeroing_constants),
proximal_pot_encoder_(M_PI * 2.0 *
@@ -57,12 +60,17 @@
distal_zeroing_constants_(distal_zeroing_constants),
distal_pot_encoder_(M_PI * 2.0 *
constants::Values::kDistalEncoderRatio()),
+ roll_joint_zeroing_constants_(roll_joint_zeroing_constants),
+ roll_joint_pot_encoder_(M_PI * 2.0 *
+ constants::Values::kDistalEncoderRatio()),
+ roll_joint_loop_(roll::MakeIntegralRollLoop()),
dynamics_(arm::kArmConstants),
dt_(dt) {
X_.setZero();
+ roll_joint_loop_.Reset();
}
- void InitializePosition(::Eigen::Matrix<double, 2, 1> position) {
+ void InitializePosition(::Eigen::Matrix<double, 3, 1> position) {
X_ << position(0), 0.0, position(1), 0.0;
proximal_pot_encoder_.Initialize(
@@ -71,6 +79,13 @@
distal_pot_encoder_.Initialize(
X_(2), kNoiseScalar, 0.0,
distal_zeroing_constants_.measured_absolute_position);
+
+ Eigen::Matrix<double, 3, 1> X_roll_joint;
+ X_roll_joint << position(2), 0.0, 0.0;
+ roll_joint_loop_.mutable_X_hat() = X_roll_joint;
+ roll_joint_pot_encoder_.Initialize(
+ X_roll_joint(0), kNoiseScalar, 0.0,
+ roll_joint_zeroing_constants_.measured_absolute_position);
}
flatbuffers::Offset<ArmPosition> GetSensorValues(
@@ -83,9 +98,14 @@
flatbuffers::Offset<frc971::PotAndAbsolutePosition> distal_offset =
distal_pot_encoder_.GetSensorValues(&distal_builder);
+ frc971::PotAndAbsolutePosition::Builder roll_joint_builder(*fbb);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> roll_joint_offset =
+ roll_joint_pot_encoder_.GetSensorValues(&roll_joint_builder);
+
ArmPosition::Builder arm_position_builder(*fbb);
arm_position_builder.add_proximal(proximal_offset);
arm_position_builder.add_distal(distal_offset);
+ arm_position_builder.add_roll_joint(roll_joint_offset);
return arm_position_builder.Finish();
}
@@ -94,21 +114,26 @@
double proximal_velocity() const { return X_(1, 0); }
double distal_position() const { return X_(2, 0); }
double distal_velocity() const { return X_(3, 0); }
+ double roll_joint_position() const { return roll_joint_loop_.X_hat(0, 0); }
+ double roll_joint_velocity() const { return roll_joint_loop_.X_hat(1, 0); }
- void Simulate(::Eigen::Matrix<double, 2, 1> U) {
+ void Simulate(::Eigen::Matrix<double, 3, 1> U) {
constexpr double voltage_check =
superstructure::arm::Arm::kOperatingVoltage();
AOS_CHECK_LE(::std::abs(U(0)), voltage_check);
AOS_CHECK_LE(::std::abs(U(1)), voltage_check);
+ AOS_CHECK_LE(::std::abs(U(2)), voltage_check);
X_ = dynamics_.UnboundedDiscreteDynamics(
- X_, U,
+ X_, U.head<2>(),
std::chrono::duration_cast<std::chrono::duration<double>>(dt_).count());
+ roll_joint_loop_.UpdateObserver(U.tail<1>(), dt_);
// TODO(austin): Estop on grose out of bounds.
proximal_pot_encoder_.MoveTo(X_(0));
distal_pot_encoder_.MoveTo(X_(2));
+ roll_joint_pot_encoder_.MoveTo(roll_joint_loop_.X_hat(0));
}
private:
@@ -122,6 +147,13 @@
distal_zeroing_constants_;
PositionSensorSimulator distal_pot_encoder_;
+ const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ roll_joint_zeroing_constants_;
+ PositionSensorSimulator roll_joint_pot_encoder_;
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackPlant<3, 1, 1>,
+ StateFeedbackObserver<3, 1, 1>>
+ roll_joint_loop_;
+
::frc971::control_loops::arm::Dynamics dynamics_;
std::chrono::nanoseconds dt_;
@@ -135,7 +167,8 @@
chrono::nanoseconds dt)
: event_loop_(event_loop),
dt_(dt),
- arm_(values->arm_proximal.zeroing, values->arm_distal.zeroing, dt_),
+ arm_(values->arm_proximal.zeroing, values->arm_distal.zeroing,
+ values->roll_joint.zeroing, dt_),
superstructure_position_sender_(
event_loop_->MakeSender<Position>("/superstructure")),
superstructure_status_fetcher_(
@@ -151,9 +184,10 @@
EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
arm_.Simulate(
- (::Eigen::Matrix<double, 2, 1>()
+ (::Eigen::Matrix<double, 3, 1>()
<< superstructure_output_fetcher_->proximal_voltage(),
- superstructure_output_fetcher_->distal_voltage())
+ superstructure_output_fetcher_->distal_voltage(),
+ superstructure_output_fetcher_->roll_joint_voltage())
.finished());
}
first_ = false;
@@ -162,7 +196,7 @@
dt);
}
- void InitializeArmPosition(::Eigen::Matrix<double, 2, 1> position) {
+ void InitializeArmPosition(::Eigen::Matrix<double, 3, 1> position) {
arm_.InitializePosition(position);
}
@@ -224,8 +258,8 @@
test_event_loop_->MakeSender<DrivetrainStatus>("/drivetrain")),
superstructure_plant_event_loop_(MakeEventLoop("plant", roborio_)),
superstructure_plant_(superstructure_plant_event_loop_.get(), values_,
- dt()) {
- (void)values_;
+ dt()),
+ points_(arm::PointList()) {
set_team_id(frc971::control_loops::testing::kTeamNumber);
SetEnabled(true);
@@ -245,6 +279,48 @@
ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr)
<< ": No status";
+
+ constexpr double kEpsTheta = 0.01;
+ constexpr double kEpsOmega = 0.01;
+
+ // Check that the status had the right goal
+ ASSERT_NEAR(points_[superstructure_goal_fetcher_->arm_goal_position()](0),
+ superstructure_status_fetcher_->arm()->goal_theta0(),
+ kEpsTheta);
+ ASSERT_NEAR(points_[superstructure_goal_fetcher_->arm_goal_position()](1),
+ superstructure_status_fetcher_->arm()->goal_theta1(),
+ kEpsTheta);
+ ASSERT_NEAR(points_[superstructure_goal_fetcher_->arm_goal_position()](2),
+ superstructure_status_fetcher_->arm()->goal_theta2(),
+ kEpsTheta);
+
+ // Check that the status met the goal
+ EXPECT_NEAR(superstructure_status_fetcher_->arm()->goal_theta0(),
+ superstructure_status_fetcher_->arm()->theta0(), kEpsTheta);
+ EXPECT_NEAR(superstructure_status_fetcher_->arm()->goal_theta1(),
+ superstructure_status_fetcher_->arm()->theta1(), kEpsTheta);
+ EXPECT_NEAR(superstructure_status_fetcher_->arm()->goal_theta2(),
+ superstructure_status_fetcher_->arm()->theta2(), kEpsTheta);
+ EXPECT_NEAR(superstructure_status_fetcher_->arm()->goal_omega0(),
+ superstructure_status_fetcher_->arm()->omega0(), kEpsOmega);
+ EXPECT_NEAR(superstructure_status_fetcher_->arm()->goal_omega1(),
+ superstructure_status_fetcher_->arm()->omega1(), kEpsOmega);
+ EXPECT_NEAR(superstructure_status_fetcher_->arm()->goal_omega2(),
+ superstructure_status_fetcher_->arm()->omega2(), kEpsOmega);
+
+ // Check that our simulator matches the status
+ EXPECT_NEAR(superstructure_plant_.arm().proximal_position(),
+ superstructure_status_fetcher_->arm()->theta0(), kEpsTheta);
+ EXPECT_NEAR(superstructure_plant_.arm().distal_position(),
+ superstructure_status_fetcher_->arm()->theta1(), kEpsTheta);
+ EXPECT_NEAR(superstructure_plant_.arm().roll_joint_position(),
+ superstructure_status_fetcher_->arm()->theta2(), kEpsTheta);
+ EXPECT_NEAR(superstructure_plant_.arm().proximal_velocity(),
+ superstructure_status_fetcher_->arm()->omega0(), kEpsOmega);
+ EXPECT_NEAR(superstructure_plant_.arm().distal_velocity(),
+ superstructure_status_fetcher_->arm()->omega1(), kEpsOmega);
+ EXPECT_NEAR(superstructure_plant_.arm().roll_joint_velocity(),
+ superstructure_status_fetcher_->arm()->omega2(), kEpsOmega);
}
void CheckIfZeroed() {
@@ -308,6 +384,8 @@
std::unique_ptr<aos::EventLoop> logger_event_loop_;
std::unique_ptr<aos::logger::Logger> logger_;
+
+ const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
}; // namespace testing
// Tests that the superstructure does nothing when the goal is to remain
@@ -378,7 +456,6 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- // Intake needs over 9 seconds to reach the goal
// TODO(Milo): Make this a sane time
RunFor(chrono::seconds(20));
VerifyNearGoal();
@@ -406,7 +483,7 @@
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::NeutralToConePos1Index());
+ goal_builder.add_arm_goal_position(arm::PickupPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -420,31 +497,6 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::NeutralToConePos1Index());
- ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
- }
-
- RunFor(chrono::seconds(10));
-
- VerifyNearGoal();
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::NeutralToConePos2Index());
- ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
- }
-
- RunFor(chrono::seconds(10));
- VerifyNearGoal();
-}
-
-// Tests that we can can execute a move which moves through multiple nodes.
-TEST_F(SuperstructureTest, ArmMultistepMove) {
- SetEnabled(true);
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -456,7 +508,34 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::ConePosIndex());
+ goal_builder.add_arm_goal_position(arm::PickupPosIndex());
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal();
+}
+
+// Tests that we can can execute a move which moves through multiple nodes.
+TEST_F(SuperstructureTest, ArmMultistepMove) {
+ SetEnabled(true);
+ superstructure_plant_.InitializeArmPosition(arm::NeutralPosPoint());
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_arm_goal_position(arm::PickupPosIndex());
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(10));
+
+ VerifyNearGoal();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_arm_goal_position(arm::ScorePosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
diff --git a/y2023/control_loops/superstructure/superstructure_position.fbs b/y2023/control_loops/superstructure/superstructure_position.fbs
index 8549943..bd3d607 100644
--- a/y2023/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023/control_loops/superstructure/superstructure_position.fbs
@@ -12,18 +12,18 @@
// (connected to proximal) arm in radians.
// Zero is straight up, positive is a forwards rotation
distal:frc971.PotAndAbsolutePosition (id: 1);
+
+ // Zero for roll joint is up vertically
+ // Positive position would be rotated counterclockwise relative to the robot
+ roll_joint:frc971.PotAndAbsolutePosition (id: 2);
}
table Position {
arm:ArmPosition (id: 0);
- // Zero for roll joint is up vertically
- // Positive position would be rotated counterclockwise relative to the robot
- roll_joint:frc971.PotAndAbsolutePosition (id: 1);
-
// Zero for wrist is facing staright outward.
// Positive position would be upwards
- wrist:frc971.AbsolutePosition (id: 2);
+ wrist:frc971.AbsolutePosition (id: 1);
}
root_type Position;
diff --git a/y2023/control_loops/superstructure/superstructure_status.fbs b/y2023/control_loops/superstructure/superstructure_status.fbs
index 9b21cc3..db86687 100644
--- a/y2023/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023/control_loops/superstructure/superstructure_status.fbs
@@ -16,6 +16,7 @@
// State of the estimators.
proximal_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState (id: 0);
distal_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState (id: 1);
+ roll_joint_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState (id: 18);
// The node we are currently going to.
current_node:uint32 (id: 2);
@@ -24,19 +25,24 @@
// Goal position and velocity (radians)
goal_theta0:float (id: 4);
goal_theta1:float (id: 5);
+ goal_theta2:float (id: 19);
goal_omega0:float (id: 6);
goal_omega1:float (id: 7);
+ goal_omega2:float (id: 20);
// Current position and velocity (radians)
theta0:float (id: 8);
theta1:float (id: 9);
+ theta2:float (id: 21);
omega0:float (id: 10);
omega1:float (id: 11);
+ omega2:float (id: 22);
// Estimated voltage error for the two joints.
voltage_error0:float (id: 12);
voltage_error1:float (id: 13);
+ voltage_error2:float (id: 23);
// True if we are zeroed.
zeroed:bool (id: 14);
@@ -60,9 +66,7 @@
arm:ArmStatus (id: 2);
- roll_joint:frc971.PotAndAbsoluteEncoderEstimatorState (id: 3);
-
- wrist:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 4);
+ wrist:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);
}
root_type Status;