Read heading and velocity pwm in wpilib_interface
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: Ifeeed94b61ca03972debbba52ae1f36022d901a2
diff --git a/frc971/imu/ADIS16505.cc b/frc971/imu/ADIS16505.cc
index 9ed77a9..d3fbf76 100644
--- a/frc971/imu/ADIS16505.cc
+++ b/frc971/imu/ADIS16505.cc
@@ -373,9 +373,7 @@
yaw += yaw_rate * dt;
// 50% is 0; -2000 deg/sec to 2000 deg/sec
- uint16_t rate_level =
- (std::clamp(yaw_rate, -2000.0, 2000.0) / 4000.0 + 0.5) * PWM_TOP;
- pwm_set_gpio_level(RATE_PWM, rate_level);
+ double scaled_rate = (std::clamp(yaw_rate, -2000.0, 2000.0) / 4000.0 + 0.5);
// 0 to 360
double wrapped_heading = fmod(yaw, 360);
@@ -383,8 +381,20 @@
wrapped_heading = wrapped_heading + 360;
}
- uint16_t heading_level = (int16_t)(wrapped_heading / 360.0 * PWM_TOP);
+ double scaled_heading = wrapped_heading / 360.0;
+
+ constexpr double kScaledRangeLow = 0.1;
+ constexpr double kScaledRangeHigh = 0.9;
+
+ uint16_t heading_level =
+ (scaled_heading * (kScaledRangeHigh - kScaledRangeLow) +
+ kScaledRangeLow) *
+ PWM_TOP;
+ uint16_t rate_level =
+ (scaled_rate * (kScaledRangeHigh - kScaledRangeLow) + kScaledRangeLow) *
+ PWM_TOP;
pwm_set_gpio_level(HEADING_PWM, heading_level);
+ pwm_set_gpio_level(RATE_PWM, rate_level);
}
// if 5 or more consecutive checksums are zero, then something weird is going
diff --git a/y2022/BUILD b/y2022/BUILD
index fd82eb5..cc3aa93 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -225,6 +225,7 @@
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops/drivetrain:drivetrain_position_fbs",
"//frc971/input:robot_state_fbs",
+ "//frc971/queues:gyro_fbs",
"//frc971/wpilib:ADIS16448",
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:drivetrain_writer",
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index b95a4ec..eb70b6f 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -36,6 +36,7 @@
#include "frc971/autonomous/auto_mode_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/input/robot_state_generated.h"
+#include "frc971/queues/gyro_generated.h"
#include "frc971/wpilib/ADIS16448.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/buffered_solenoid.h"
@@ -131,7 +132,9 @@
drivetrain_position_sender_(
event_loop
->MakeSender<::frc971::control_loops::drivetrain::Position>(
- "/drivetrain")) {
+ "/drivetrain")),
+ gyro_sender_(event_loop->MakeSender<::frc971::sensors::GyroReading>(
+ "/drivetrain")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -158,6 +161,14 @@
catapult_encoder_.set_potentiometer(::std::move(potentiometer));
}
+ void set_heading_input(::std::unique_ptr<frc::DigitalInput> sensor) {
+ imu_heading_reader_.set_input(::std::move(sensor));
+ }
+
+ void set_yaw_rate_input(::std::unique_ptr<frc::DigitalInput> sensor) {
+ imu_yaw_rate_reader_.set_input(::std::move(sensor));
+ }
+
void RunIteration() override {
{
auto builder = superstructure_position_sender_.MakeBuilder();
@@ -250,6 +261,38 @@
}
{
+ auto builder = gyro_sender_.MakeBuilder();
+ ::frc971::sensors::GyroReading::Builder gyro_reading_builder =
+ builder.MakeBuilder<::frc971::sensors::GyroReading>();
+ constexpr double kMaxVelocity = 2000; // degrees / second
+ constexpr double kVelocityRadiansPerSecond =
+ kMaxVelocity / 360 * (2.0 * M_PI);
+
+ // Only part of the full range is used to prevent being 100% on or off.
+ constexpr double kScaledRangeLow = 0.1;
+ constexpr double kScaledRangeHigh = 0.9;
+
+ constexpr double kDutyCycleScale =
+ 1 / (kScaledRangeHigh - kScaledRangeLow);
+
+ // scale from 0.1 - 0.9 to 0 - 1
+ double rescaled_heading_duty_cycle =
+ (imu_heading_reader_.Read() - kScaledRangeLow) * kDutyCycleScale;
+ double rescaled_velocity_duty_cycle =
+ (imu_yaw_rate_reader_.Read() - kScaledRangeLow) * kDutyCycleScale;
+
+ if (!std::isnan(rescaled_heading_duty_cycle)) {
+ gyro_reading_builder.add_angle(rescaled_heading_duty_cycle *
+ (2.0 * M_PI));
+ }
+ if (!std::isnan(rescaled_velocity_duty_cycle)) {
+ gyro_reading_builder.add_velocity((rescaled_velocity_duty_cycle - 0.5) *
+ kVelocityRadiansPerSecond);
+ }
+ builder.CheckOk(builder.Send(gyro_reading_builder.Finish()));
+ }
+
+ {
auto builder = auto_mode_sender_.MakeBuilder();
uint32_t mode = 0;
@@ -345,6 +388,7 @@
aos::Sender<superstructure::Position> superstructure_position_sender_;
aos::Sender<frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
+ ::aos::Sender<::frc971::sensors::GyroReading> gyro_sender_;
std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
@@ -354,9 +398,9 @@
std::unique_ptr<frc::AnalogInput> climber_potentiometer_,
flipper_arm_right_potentiometer_, flipper_arm_left_potentiometer_;
frc971::wpilib::AbsoluteEncoderAndPotentiometer intake_encoder_front_,
- intake_encoder_back_, turret_encoder_;
+ intake_encoder_back_, turret_encoder_, catapult_encoder_;
- frc971::wpilib::AbsoluteEncoderAndPotentiometer catapult_encoder_;
+ frc971::wpilib::DutyCycleReader imu_heading_reader_, imu_yaw_rate_reader_;
};
class SuperstructureWriter
@@ -602,6 +646,9 @@
sensor_reader.set_catapult_potentiometer(
std::make_unique<frc::AnalogInput>(2));
+ sensor_reader.set_heading_input(make_unique<frc::DigitalInput>(8));
+ sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(9));
+
AddLoop(&sensor_reader_event_loop);
// Thread 4.