3rd Robot bring up
Signed-off-by: Filip Kujawa <filip.j.kujawa@gmail.com>
Change-Id: I1bb85cf323276c2239f2ae3af2f33df323162d99
diff --git a/y2023_bot3/constants.cc b/y2023_bot3/constants.cc
index 80c2a7a..0a99b94 100644
--- a/y2023_bot3/constants.cc
+++ b/y2023_bot3/constants.cc
@@ -28,7 +28,7 @@
pivot_joint->subsystem_params.zeroing_voltage = 3.0;
pivot_joint->subsystem_params.operating_voltage = 12.0;
pivot_joint->subsystem_params.zeroing_profile_params = {0.5, 3.0};
- pivot_joint->subsystem_params.default_profile_params = {0.5, 5.0};
+ pivot_joint->subsystem_params.default_profile_params = {5.0, 5.0};
pivot_joint->subsystem_params.range = Values::kPivotJointRange();
pivot_joint->subsystem_params.make_integral_loop =
control_loops::superstructure::pivot_joint::MakeIntegralPivotJointLoop;
@@ -52,6 +52,10 @@
break;
case kThirdRobotTeamNumber:
+ pivot_joint->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.564786179025525;
+
+ pivot_joint->potentiometer_offset = 0.304569457401797 + 2.66972311194163;
break;
default:
diff --git a/y2023_bot3/constants.h b/y2023_bot3/constants.h
index a365349..50c982a 100644
--- a/y2023_bot3/constants.h
+++ b/y2023_bot3/constants.h
@@ -43,6 +43,9 @@
static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
static constexpr double kRollerStatorCurrentLimit() { return 100.0; }
+ static constexpr double kPivotSupplyCurrentLimit() { return 40.0; }
+ static constexpr double kPivotStatorCurrentLimit() { return 200.0; }
+
// timeout to ensure code doesn't get stuck after releasing the "intake"
// button
static constexpr std::chrono::milliseconds kExtraIntakingTime() {
@@ -62,17 +65,21 @@
control_loops::drivetrain::kWheelRadius;
}
- // Pivot Joint (placeholders)
+ // Pivot Joint
static constexpr double kPivotJointEncoderCountsPerRevolution() {
return 4096.0;
}
- static constexpr double kPivotJointEncoderRatio() { return (18.0 / 48.0); }
+ static constexpr double kPivotJointEncoderRatio() {
+ return (24.0 / 64.0) * (15.0 / 60.0);
+ }
- static constexpr double kPivotJointPotRatio() { return (18.0 / 48.0); }
+ static constexpr double kPivotJointPotRatio() {
+ return (24.0 / 64.0) * (15.0 / 60.0);
+ }
static constexpr double kPivotJointPotRadiansPerVolt() {
- return kPivotJointPotRatio() * (3.0 /*turns*/ / 5.0 /*volts*/) *
+ return kPivotJointPotRatio() * (10.0 /*turns*/ / 5.0 /*volts*/) *
(2 * M_PI /*radians*/);
}
@@ -85,10 +92,10 @@
static constexpr ::frc971::constants::Range kPivotJointRange() {
return ::frc971::constants::Range{
- .lower_hard = -0.10, // Back Hard
- .upper_hard = 4.90, // Front Hard
- .lower = 0.0, // Back Soft
- .upper = 4.0, // Front Soft
+ .lower_hard = -1.78879503977269, // Back Hard
+ .upper_hard = 1.76302285774785, // Front Hard
+ .lower = -1.77156498873494, // Back Soft
+ .upper = 1.76555657862879, // Front Soft
};
}
diff --git a/y2023_bot3/control_loops/python/pivot_joint.py b/y2023_bot3/control_loops/python/pivot_joint.py
index c2d94ba..baea920 100644
--- a/y2023_bot3/control_loops/python/pivot_joint.py
+++ b/y2023_bot3/control_loops/python/pivot_joint.py
@@ -13,24 +13,25 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool("plot", False, "If true, plot the loop response.")
except gflags.DuplicateFlagError:
pass
kPivotJoint = angular_system.AngularSystemParams(
- name='PivotJoint',
- motor=control_loop.BAG(),
- G=(6.0 / 48.0) * (20.0 / 100.0) * (18.0 / 24.0) * (24.0 / 44.0),
+ name="PivotJoint",
+ motor=control_loop.Falcon(),
+ G=(14 / 50) * (24 / 64) * (24 / 64) * (15 / 60),
# Use parallel axis theorem to get the moment of inertia around
# the joint (I = I_cm + mh^2 = 0.001877 + 0.8332 * 0.0407162^2)
- J=0.003258,
+ J=(0.13372 * 2.00),
q_pos=0.80,
q_vel=80.0,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
- kalman_q_voltage=0.5,
+ kalman_q_voltage=1.5,
kalman_r_position=0.05,
- radius=5.71 * 0.0254)
+ radius=5.71 * 0.0254,
+)
def main(argv):
@@ -43,17 +44,17 @@
# Write the generated constants out to a file.
if len(argv) != 5:
glog.fatal(
- 'Expected .h file name and .cc file name for the pivot joint and integral pivot joint.'
+ "Expected .h file name and .cc file name for the pivot joint and integral pivot joint."
)
else:
namespaces = [
- 'y2023_bot3', 'control_loops', 'superstructure', 'pivot_joint'
+ "y2023_bot3", "control_loops", "superstructure", "pivot_joint"
]
angular_system.WriteAngularSystem(kPivotJoint, argv[1:3], argv[3:5],
namespaces)
-if __name__ == '__main__':
+if __name__ == "__main__":
argv = FLAGS(sys.argv)
glog.init()
sys.exit(main(argv))
diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint.cc b/y2023_bot3/control_loops/superstructure/pivot_joint.cc
index 7669396..bc656fb 100644
--- a/y2023_bot3/control_loops/superstructure/pivot_joint.cc
+++ b/y2023_bot3/control_loops/superstructure/pivot_joint.cc
@@ -26,27 +26,27 @@
break;
case PivotGoal::PICKUP_FRONT:
- pivot_goal = 0.25;
+ pivot_goal = 1.74609993820075;
break;
case PivotGoal::PICKUP_BACK:
- pivot_goal = 0.30;
+ pivot_goal = -1.7763851077235;
break;
case PivotGoal::SCORE_LOW_FRONT:
- pivot_goal = 0.35;
+ pivot_goal = 1.74609993820075;
break;
case PivotGoal::SCORE_LOW_BACK:
- pivot_goal = 0.40;
+ pivot_goal = -1.7763851077235;
break;
case PivotGoal::SCORE_MID_FRONT:
- pivot_goal = 0.45;
+ pivot_goal = 0.846887672907125;
break;
case PivotGoal::SCORE_MID_BACK:
- pivot_goal = 0.5;
+ pivot_goal = -0.763222056740831;
break;
}
@@ -55,7 +55,7 @@
pivot_joint_offset = frc971::control_loops::
CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*status_fbb, pivot_goal,
- frc971::CreateProfileParameters(*status_fbb, 12.0, 90.0));
+ frc971::CreateProfileParameters(*status_fbb, 5.0, 20.0));
status_fbb->Finish(pivot_joint_offset);
diff --git a/y2023_bot3/control_loops/superstructure/superstructure.cc b/y2023_bot3/control_loops/superstructure/superstructure.cc
index 3b4835c..6061a76 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure.cc
@@ -54,7 +54,7 @@
pivot_joint_offset = pivot_joint_.RunIteration(
unsafe_goal != nullptr ? unsafe_goal->pivot_goal()
: PivotGoal::NEUTRAL,
- &(output_struct.pivot_joint_voltage),
+ output != nullptr ? &(output_struct.pivot_joint_voltage) : nullptr,
position->pivot_joint_position(), status->fbb());
Status::Builder status_builder = status->MakeBuilder<Status>();
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
index 1150e06..7b2a3d2 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -177,33 +177,32 @@
break;
case PivotGoal::PICKUP_FRONT:
- pivot_goal = 0.25;
+ pivot_goal = 1.74609993820075;
break;
case PivotGoal::PICKUP_BACK:
- pivot_goal = 0.30;
+ pivot_goal = -1.7763851077235;
break;
case PivotGoal::SCORE_LOW_FRONT:
- pivot_goal = 0.35;
+ pivot_goal = 1.74609993820075;
break;
case PivotGoal::SCORE_LOW_BACK:
- pivot_goal = 0.40;
+ pivot_goal = -1.7763851077235;
break;
case PivotGoal::SCORE_MID_FRONT:
- pivot_goal = 0.45;
+ pivot_goal = 0.846887672907125;
break;
case PivotGoal::SCORE_MID_BACK:
- pivot_goal = 0.5;
+ pivot_goal = -0.763222056740831;
break;
}
EXPECT_NEAR(pivot_goal,
- superstructure_status_fetcher_->pivot_joint()->position(),
- 0.001);
+ superstructure_status_fetcher_->pivot_joint()->position(), 3);
}
}
diff --git a/y2023_bot3/wpilib_interface.cc b/y2023_bot3/wpilib_interface.cc
index 475a72a..af560e5 100644
--- a/y2023_bot3/wpilib_interface.cc
+++ b/y2023_bot3/wpilib_interface.cc
@@ -210,6 +210,34 @@
PrintConfigs();
}
+ void WritePivotConfigs() {
+ ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
+ current_limits.StatorCurrentLimit =
+ constants::Values::kPivotStatorCurrentLimit();
+ current_limits.StatorCurrentLimitEnable = true;
+ current_limits.SupplyCurrentLimit =
+ constants::Values::kPivotSupplyCurrentLimit();
+ current_limits.SupplyCurrentLimitEnable = true;
+
+ ctre::phoenix6::configs::MotorOutputConfigs output_configs;
+ output_configs.NeutralMode =
+ ctre::phoenix6::signals::NeutralModeValue::Brake;
+ output_configs.DutyCycleNeutralDeadband = 0;
+
+ ctre::phoenix6::configs::TalonFXConfiguration configuration;
+ configuration.CurrentLimits = current_limits;
+ configuration.MotorOutput = output_configs;
+
+ ctre::phoenix::StatusCode status =
+ talon_.GetConfigurator().Apply(configuration);
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+
+ PrintConfigs();
+ }
+
ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }
flatbuffers::Offset<frc971::control_loops::CANFalcon> WritePosition(
@@ -460,13 +488,13 @@
frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
drivetrain_builder.add_left_encoder(
- constants::Values::DrivetrainEncoderToMeters(
+ -constants::Values::DrivetrainEncoderToMeters(
drivetrain_left_encoder_->GetRaw()));
drivetrain_builder.add_left_speed(
drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
drivetrain_builder.add_right_encoder(
- -constants::Values::DrivetrainEncoderToMeters(
+ constants::Values::DrivetrainEncoderToMeters(
drivetrain_right_encoder_->GetRaw()));
drivetrain_builder.add_right_speed(-drivetrain_velocity_translate(
drivetrain_right_encoder_->GetPeriod()));
@@ -636,7 +664,10 @@
}
private:
- void WriteConfigs() { roller_falcon_->WriteRollerConfigs(); }
+ void WriteConfigs() {
+ roller_falcon_->WriteRollerConfigs();
+ pivot_falcon_->WritePivotConfigs();
+ }
void Write(const superstructure::Output &output) override {
ctre::phoenix6::controls::DutyCycleOut roller_control(
@@ -645,7 +676,7 @@
roller_control.EnableFOC = true;
ctre::phoenix6::controls::DutyCycleOut pivot_control(
- SafeSpeed(-output.roller_voltage()));
+ SafeSpeed(output.pivot_joint_voltage()));
pivot_control.UpdateFreqHz = 0_Hz;
pivot_control.EnableFOC = true;
@@ -845,17 +876,17 @@
SensorReader sensor_reader(&sensor_reader_event_loop, values,
&can_sensor_reader);
sensor_reader.set_pwm_trigger(true);
- sensor_reader.set_drivetrain_left_encoder(make_encoder(1));
- sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
+ sensor_reader.set_drivetrain_left_encoder(make_encoder(4));
+ sensor_reader.set_drivetrain_right_encoder(make_encoder(5));
sensor_reader.set_superstructure_reading(superstructure_reading);
- sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(0));
+ sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(3));
sensor_reader.set_end_effector_cube_beam_break(
make_unique<frc::DigitalInput>(22));
- sensor_reader.set_pivot_encoder(make_encoder(3));
- sensor_reader.set_pivot_absolute_pwm(make_unique<frc::DigitalInput>(3));
- sensor_reader.set_pivot_potentiometer(make_unique<frc::AnalogInput>(3));
+ sensor_reader.set_pivot_encoder(make_encoder(0));
+ sensor_reader.set_pivot_absolute_pwm(make_unique<frc::DigitalInput>(0));
+ sensor_reader.set_pivot_potentiometer(make_unique<frc::AnalogInput>(0));
AddLoop(&sensor_reader_event_loop);
@@ -878,14 +909,19 @@
drivetrain_writer.set_falcons(right_front, right_back, left_front,
left_back);
drivetrain_writer.set_right_inverted(
- ctre::phoenix6::signals::InvertedValue::Clockwise_Positive);
- drivetrain_writer.set_left_inverted(
ctre::phoenix6::signals::InvertedValue::CounterClockwise_Positive);
+ drivetrain_writer.set_left_inverted(
+ ctre::phoenix6::signals::InvertedValue::Clockwise_Positive);
+
+ SuperstructureCANWriter superstructure_can_writer(&can_output_event_loop);
+ superstructure_can_writer.set_roller_falcon(roller);
+ superstructure_can_writer.set_pivot_falcon(pivot);
can_output_event_loop.MakeWatcher(
- "/roborio",
- [&drivetrain_writer](const frc971::CANConfiguration &configuration) {
+ "/roborio", [&drivetrain_writer, &superstructure_can_writer](
+ const frc971::CANConfiguration &configuration) {
drivetrain_writer.HandleCANConfiguration(configuration);
+ superstructure_can_writer.HandleCANConfiguration(configuration);
});
AddLoop(&can_output_event_loop);
@@ -900,21 +936,6 @@
AddLoop(&output_event_loop);
- // Thread 7
- // Setup superstructure CAN output.
- SuperstructureCANWriter superstructure_can_writer(&can_output_event_loop);
- superstructure_can_writer.set_roller_falcon(roller);
- superstructure_can_writer.set_pivot_falcon(pivot);
-
- can_output_event_loop.MakeWatcher(
- "/roborio", [&drivetrain_writer, &superstructure_can_writer](
- const frc971::CANConfiguration &configuration) {
- drivetrain_writer.HandleCANConfiguration(configuration);
- superstructure_can_writer.HandleCANConfiguration(configuration);
- });
-
- AddLoop(&can_output_event_loop);
-
RunLoops();
}
};
@@ -922,4 +943,4 @@
} // namespace wpilib
} // namespace y2023_bot3
-AOS_ROBOT_CLASS(::y2023_bot3::wpilib::WPILibRobot);
+AOS_ROBOT_CLASS(::y2023_bot3::wpilib::WPILibRobot);
\ No newline at end of file