Send 0.95 from TOF controller if there is no cone.
This lets us detect cone presence separately from unplugged sensor.
Change-Id: I0f70c1229bd88b5c50ed2fdf043361d072c41402
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/wpilib/dma_edge_counting.cc b/frc971/wpilib/dma_edge_counting.cc
index ba8fdf4..4e8a158 100644
--- a/frc971/wpilib/dma_edge_counting.cc
+++ b/frc971/wpilib/dma_edge_counting.cc
@@ -28,11 +28,11 @@
if (have_prev_sample_ && high_time_ != 0 && prev_sample_.Get(input_) &&
!sample.Get(input_)) {
last_width_ = (sample.GetTime() - high_time_) * 1e-6;
- high_time_ = 0;
poll_count_ = 0;
} else if (have_prev_sample_ && !prev_sample_.Get(input_) &&
sample.Get(input_)) {
- high_time_ = prev_sample_.GetTime();
+ last_period_ = (sample.GetTime() - high_time_) * 1e-6;
+ high_time_ = sample.GetTime();
poll_count_ = 0;
}
have_prev_sample_ = true;
@@ -46,6 +46,7 @@
high_time_ = 0;
have_prev_sample_ = false;
last_width_ = ::std::numeric_limits<double>::quiet_NaN();
+ last_period_ = ::std::numeric_limits<double>::quiet_NaN();
}
poll_count_++;
}
diff --git a/frc971/wpilib/dma_edge_counting.h b/frc971/wpilib/dma_edge_counting.h
index 4d1246c..f51318d 100644
--- a/frc971/wpilib/dma_edge_counting.h
+++ b/frc971/wpilib/dma_edge_counting.h
@@ -47,6 +47,7 @@
// Last pulse width in seconds
double last_width() const { return last_width_; }
+ double last_period() const { return last_period_; }
private:
void UpdateFromSample(const DMASample & /*sample*/) override;
@@ -70,6 +71,7 @@
size_t poll_count_ = 0;
double last_width_ = ::std::numeric_limits<double>::quiet_NaN();
+ double last_period_ = ::std::numeric_limits<double>::quiet_NaN();
DISALLOW_COPY_AND_ASSIGN(DMAPulseWidthReader);
};
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index 9d5d564..617464b 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -79,7 +79,7 @@
EndEffectorState end_effector_state = end_effector_.RunIteration(
timestamp,
unsafe_goal != nullptr ? unsafe_goal->roller_goal() : RollerGoal::IDLE,
- position->end_effector_cone_beam_break(),
+ false,
position->end_effector_cube_beam_break(), &output_struct.roller_voltage);
if (output) {
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index b94054d..f9b8548 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -244,18 +244,14 @@
Position::Builder position_builder = builder.MakeBuilder<Position>();
position_builder.add_arm(arm_offset);
position_builder.add_wrist(wrist_offset);
- position_builder.add_end_effector_cone_beam_break(
- end_effector_cone_beam_break_);
position_builder.add_end_effector_cube_beam_break(
end_effector_cube_beam_break_);
+ // TODO(milind): put into our state
+ position_builder.add_cone_position(0.95);
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
- void set_end_effector_cone_beam_break(bool triggered) {
- end_effector_cone_beam_break_ = triggered;
- }
-
void set_end_effector_cube_beam_break(bool triggered) {
end_effector_cube_beam_break_ = triggered;
}
@@ -268,7 +264,6 @@
ArmSimulation arm_;
AbsoluteEncoderSimulator wrist_;
- bool end_effector_cone_beam_break_;
bool end_effector_cube_beam_break_;
::aos::Sender<Position> superstructure_position_sender_;
@@ -569,7 +564,7 @@
public:
void SetBeambreak(GamePiece game_piece, bool status) {
if (game_piece == GamePiece::kCone) {
- superstructure_plant_.set_end_effector_cone_beam_break(status);
+ // TODO(milind): handle cone
} else {
superstructure_plant_.set_end_effector_cube_beam_break(status);
}
@@ -807,8 +802,9 @@
VerifyNearGoal();
}
+// TODO(milind): add cone
INSTANTIATE_TEST_SUITE_P(EndEffectorGoal, SuperstructureBeambreakTest,
- ::testing::Values(GamePiece::kCone, GamePiece::kCube));
+ ::testing::Values(GamePiece::kCube));
} // namespace testing
} // namespace superstructure
diff --git a/y2023/control_loops/superstructure/superstructure_position.fbs b/y2023/control_loops/superstructure/superstructure_position.fbs
index 927fe10..cf8a843 100644
--- a/y2023/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023/control_loops/superstructure/superstructure_position.fbs
@@ -26,7 +26,7 @@
wrist:frc971.AbsolutePosition (id: 1);
// If this is true, the cone beam break is triggered.
- end_effector_cone_beam_break:bool (id: 2);
+ cone_position:double (id: 2);
// If this is true, the cube beam break is triggered.
end_effector_cube_beam_break:bool (id: 3);
diff --git a/y2023/tof_controller/tof_controller.cc b/y2023/tof_controller/tof_controller.cc
index 645eb95..f264fb6 100644
--- a/y2023/tof_controller/tof_controller.cc
+++ b/y2023/tof_controller/tof_controller.cc
@@ -63,8 +63,8 @@
class SignalWriter {
public:
- static constexpr double kScaledRangeLow = 0.1;
- static constexpr double kScaledRangeHigh = 0.9;
+ static constexpr double kScaledRangeLow = 0.0;
+ static constexpr double kScaledRangeHigh = 1.0;
// PWM counts to this before wrapping
static constexpr uint16_t kPWMTop = 62499;
@@ -427,9 +427,15 @@
sensor2.errors == 0 && result2.Status == 0 &&
width_of_obstruction > 0;
- output_writer.SetEnabled(data_good);
- output_writer.SetValue(averaged_estimate);
- output_indicator.SetValue(data_good? averaged_estimate: 0);
+ output_writer.SetEnabled(sensor1.errors == 0 && result1.Status == 0 &&
+ sensor2.errors == 0 && result2.Status == 0);
+
+ const double output =
+ data_good ? std::max(0.05, std::min(averaged_estimate * 2.0, 0.9))
+ : 0.95;
+ output_writer.SetValue(output);
+
+ output_indicator.SetValue(data_good ? averaged_estimate : 0);
/*Temporary */ if (data_good) {
n += 1;
@@ -456,12 +462,13 @@
"dist = %5d mm, "
"Ambient = %3d, Signal = %5d, "
"#ofSpads = %3d\nPeriod: %f Errors: %d %d %d %d\nx: %f, width: %f "
- "data: %s\n",
+ "data: %s, sending: %f\n",
result1.Status, result1.Distance, result1.Ambient, result1.SigPerSPAD,
result1.NumSPADs, result2.Status, result2.Distance, result2.Ambient,
result2.SigPerSPAD, result2.NumSPADs, period_ms, sensor1.errors,
sensor2.errors, sensor1.consecutive_errors, sensor2.consecutive_errors,
- averaged_estimate, width_of_obstruction, data_good ? "good" : "bad");
+ averaged_estimate, width_of_obstruction, data_good ? "good" : "bad",
+ output);
// Try to reinitialize the sensor if it is in a bad state (eg. lost power
// but is now reconnected)
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index 4dedd1d..8b68fb6 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -138,6 +138,7 @@
void Start() override {
AddToDMA(&imu_heading_reader_);
AddToDMA(&imu_yaw_rate_reader_);
+ AddToDMA(&cone_position_sensor_);
}
// Auto mode switches.
@@ -196,10 +197,10 @@
position_builder.add_arm(arm_offset);
position_builder.add_wrist(wrist_offset);
- position_builder.add_end_effector_cone_beam_break(
- end_effector_cone_beam_break_->Get());
position_builder.add_end_effector_cube_beam_break(
end_effector_cube_beam_break_->Get());
+ position_builder.add_cone_position(cone_position_sensor_.last_width() /
+ cone_position_sensor_.last_period());
builder.CheckOk(builder.Send(position_builder.Finish()));
}
@@ -341,15 +342,16 @@
wrist_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
}
- void set_end_effector_cone_beam_break(
- ::std::unique_ptr<frc::DigitalInput> sensor) {
- end_effector_cone_beam_break_ = ::std::move(sensor);
- }
void set_end_effector_cube_beam_break(
::std::unique_ptr<frc::DigitalInput> sensor) {
end_effector_cube_beam_break_ = ::std::move(sensor);
}
+ void set_cone_position_sensor(::std::unique_ptr<frc::DigitalInput> sensor) {
+ cone_position_input_ = ::std::move(sensor);
+ cone_position_sensor_.set_input(cone_position_input_.get());
+ }
+
private:
std::shared_ptr<const Values> values_;
@@ -362,13 +364,16 @@
std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
std::unique_ptr<frc::DigitalInput> imu_heading_input_, imu_yaw_rate_input_,
- end_effector_cone_beam_break_, end_effector_cube_beam_break_;
+ end_effector_cube_beam_break_;
frc971::wpilib::DMAPulseWidthReader imu_heading_reader_, imu_yaw_rate_reader_;
frc971::wpilib::AbsoluteEncoderAndPotentiometer proximal_encoder_,
distal_encoder_, roll_joint_encoder_;
frc971::wpilib::AbsoluteEncoder wrist_encoder_;
+
+ frc971::wpilib::DMAPulseWidthReader cone_position_sensor_;
+ std::unique_ptr<frc::DigitalInput> cone_position_input_;
};
class SuperstructureWriter
@@ -831,11 +836,9 @@
sensor_reader.set_wrist_encoder(make_encoder(4));
sensor_reader.set_wrist_absolute_pwm(make_unique<frc::DigitalInput>(4));
- // TODO(Max): Make the DigitalInput values the accurate robot values.
- sensor_reader.set_end_effector_cone_beam_break(
- make_unique<frc::DigitalInput>(6));
sensor_reader.set_end_effector_cube_beam_break(
make_unique<frc::DigitalInput>(7));
+ sensor_reader.set_cone_position_sensor(make_unique<frc::DigitalInput>(8));
AddLoop(&sensor_reader_event_loop);