Merge "Nest all namespaces"
diff --git a/frc971/orin/cuda_april_tag_test.cc b/frc971/orin/cuda_april_tag_test.cc
index 9e5eb5f..b30943f 100644
--- a/frc971/orin/cuda_april_tag_test.cc
+++ b/frc971/orin/cuda_april_tag_test.cc
@@ -1198,6 +1198,7 @@
// search through indixes j - a to j + a to see if they're swapped
// also only doesn't count if the precision needed to differentiate is
// less than max_allowed_imprecision
+ bool is_allowable = false;
for (size_t k = j - allowable_swapped_indices;
k <= j + allowable_swapped_indices; k++) {
if (cuda_grouped_blob[j].x() == slope_sorted_points[k].x() &&
@@ -1205,9 +1206,12 @@
abs(ComputeTheta(slope_sorted_points[k]) -
ComputeTheta(slope_sorted_points[j])) <
max_allowed_imprecision) {
- continue;
+ is_allowable = true;
}
}
+ if (is_allowable) {
+ continue;
+ }
++missmatched_points;
++missmatched_runs;
// We shouldn't see a lot of points in a row which don't match.
@@ -1699,8 +1703,25 @@
}
}
+ static inline int DetectionCompareFunction(const void *_a, const void *_b) {
+ apriltag_detection_t *a = *(apriltag_detection_t **)_a;
+ apriltag_detection_t *b = *(apriltag_detection_t **)_b;
+
+ if (a->id != b->id) {
+ return a->id - b->id;
+ } else if (a->hamming != b->hamming) {
+ return a->hamming - b->hamming;
+ } else {
+ return b->decision_margin - a->decision_margin;
+ }
+ }
+
void CheckDetections(zarray_t *aprilrobotics_detections,
- const zarray_t *gpu_detections) {
+ const zarray_t *_gpu_detections) {
+ zarray_t *gpu_detections = zarray_copy(_gpu_detections);
+ zarray_sort(gpu_detections, DetectionCompareFunction);
+ zarray_sort(aprilrobotics_detections, DetectionCompareFunction);
+
CHECK_EQ(zarray_size(aprilrobotics_detections),
zarray_size(gpu_detections));
LOG(INFO) << "Found " << zarray_size(gpu_detections) << " tags";
diff --git a/tools/ci/clean-disk.sh b/tools/ci/clean-disk.sh
index 213a134..0c2ca92 100755
--- a/tools/ci/clean-disk.sh
+++ b/tools/ci/clean-disk.sh
@@ -3,13 +3,19 @@
# Set default for disk utilisation
readonly DMAX="${1:-80%}"
# Cache folder
-readonly CACHE_FOLDER="${HOME}/.cache/bazel/disk_cache/"
+readonly CACHE_FOLDER="${HOME}/.cache/bazel/"
+readonly BUILDS_FOLDER="${HOME}/builds/${BUILDKITE_AGENT_NAME}/spartan-robotics/"
# Retrieve disk usages in percentage
readonly DSIZE="$(df -hlP "${CACHE_FOLDER}" | sed 1d | awk '{print $5}')"
if [[ ${DSIZE} > ${DMAX} ]]; then
echo "$(hostname): Disk over ${DMAX} Clean up needed on node."
+ # Ensure that everything has permissions such that it can be deleted.
+ chmod --recursive 777 ${CACHE_FOLDER}
+ chmod --recursive 777 "${BUILDS_FOLDER}"/*_output_base
rm -rf "${CACHE_FOLDER}"
+ # Don't delete the git checkout.
+ rm -rf "${BUILDS_FOLDER}"/*_output_base
else
echo "$(hostname): No clean up needed. Disk usage is at: ${DSIZE}"
fi
diff --git a/y2024/BUILD b/y2024/BUILD
index 8e91107..7e5761c 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -239,6 +239,8 @@
"//frc971/queues:gyro_fbs",
"//frc971/wpilib:ADIS16448",
"//frc971/wpilib:buffered_pcm",
+ "//frc971/wpilib:can_drivetrain_writer",
+ "//frc971/wpilib:can_sensor_reader",
"//frc971/wpilib:dma",
"//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:encoder_and_potentiometer",
@@ -246,6 +248,7 @@
"//frc971/wpilib:logging_fbs",
"//frc971/wpilib:pdp_fetcher",
"//frc971/wpilib:sensor_reader",
+ "//frc971/wpilib:talonfx",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:phoenix",
"//third_party:phoenix6",
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index 94d62ce..672667a 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -3,7 +3,6 @@
#include <array>
#include <chrono>
#include <cinttypes>
-#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
@@ -11,20 +10,14 @@
#include <mutex>
#include <thread>
-#include "ctre/phoenix/CANifier.h"
-
#include "frc971/wpilib/ahal/AnalogInput.h"
-#include "frc971/wpilib/ahal/Counter.h"
-#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
#include "frc971/wpilib/ahal/DriverStation.h"
#include "frc971/wpilib/ahal/Encoder.h"
-#include "frc971/wpilib/ahal/Servo.h"
#include "frc971/wpilib/ahal/TalonFX.h"
#include "frc971/wpilib/ahal/VictorSP.h"
#undef ERROR
#include "ctre/phoenix/cci/Diagnostics_CCI.h"
-#include "ctre/phoenix6/TalonFX.hpp"
#include "aos/commonmath.h"
#include "aos/containers/sized_array.h"
@@ -39,21 +32,22 @@
#include "aos/util/wrapping_counter.h"
#include "frc971/autonomous/auto_mode_generated.h"
#include "frc971/can_configuration_generated.h"
-#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_can_position_static.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"
+#include "frc971/wpilib/can_drivetrain_writer.h"
+#include "frc971/wpilib/can_sensor_reader.h"
#include "frc971/wpilib/dma.h"
-#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/logging_generated.h"
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
+#include "frc971/wpilib/talonfx.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2024/constants.h"
#include "y2024/control_loops/superstructure/superstructure_output_generated.h"
@@ -65,6 +59,8 @@
using ::aos::monotonic_clock;
using ::frc971::CANConfiguration;
+using ::frc971::control_loops::drivetrain::CANPositionStatic;
+using ::frc971::wpilib::TalonFX;
using ::y2024::constants::Values;
namespace superstructure = ::y2024::control_loops::superstructure;
namespace drivetrain = ::y2024::control_loops::drivetrain;
@@ -95,9 +91,6 @@
} // namespace
-static constexpr int kCANFalconCount = 6;
-static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
-
// Class to send position messages with sensor readings to our loops.
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
@@ -112,9 +105,9 @@
event_loop->MakeSender<superstructure::Position>(
"/superstructure")),
drivetrain_position_sender_(
- event_loop
- ->MakeSender<::frc971::control_loops::drivetrain::Position>(
- "/drivetrain")),
+ event_loop->MakeSender<
+ ::frc971::control_loops::drivetrain::PositionStatic>(
+ "/drivetrain")),
gyro_sender_(event_loop->MakeSender<::frc971::sensors::GyroReading>(
"/drivetrain")){};
void Start() override { AddToDMA(&imu_yaw_rate_reader_); }
@@ -138,24 +131,10 @@
builder.CheckOk(builder.Send(position_builder.Finish()));
}
- {
- auto builder = drivetrain_position_sender_.MakeBuilder();
- frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
- builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
- drivetrain_builder.add_left_encoder(
- 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(
- drivetrain_right_encoder_->GetRaw()));
- drivetrain_builder.add_right_speed(-drivetrain_velocity_translate(
- drivetrain_right_encoder_->GetPeriod()));
-
- builder.CheckOk(builder.Send(drivetrain_builder.Finish()));
- }
+ SendDrivetrainPosition(drivetrain_position_sender_.MakeStaticBuilder(),
+ drivetrain_velocity_translate,
+ constants::Values::DrivetrainEncoderToMeters, false,
+ false);
{
auto builder = gyro_sender_.MakeBuilder();
@@ -211,7 +190,7 @@
aos::Sender<frc971::autonomous::AutonomousMode> auto_mode_sender_;
aos::Sender<superstructure::Position> superstructure_position_sender_;
- aos::Sender<frc971::control_loops::drivetrain::Position>
+ aos::Sender<frc971::control_loops::drivetrain::PositionStatic>
drivetrain_position_sender_;
::aos::Sender<::frc971::sensors::GyroReading> gyro_sender_;
@@ -264,20 +243,82 @@
c_Phoenix_Diagnostics_Dispose();
}
+ std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry;
+
+ std::shared_ptr<TalonFX> right_front = std::make_shared<TalonFX>(
+ 0, false, "Drivetrain Bus", &signals_registry,
+ constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+ std::shared_ptr<TalonFX> right_back = std::make_shared<TalonFX>(
+ 1, false, "Drivetrain Bus", &signals_registry,
+ constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+ std::shared_ptr<TalonFX> left_front = std::make_shared<TalonFX>(
+ 2, false, "Drivetrain Bus", &signals_registry,
+ constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+ std::shared_ptr<TalonFX> left_back = std::make_shared<TalonFX>(
+ 3, false, "Drivetrain Bus", &signals_registry,
+ constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+
ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
ctre::phoenix::platform::can::CANComm_SetTxSchedPriority(
constants::Values::kDrivetrainTxPriority, true, "Drivetrain Bus");
+ ::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
+ can_sensor_reader_event_loop.set_name("CANSensorReader");
+
+ // Creating list of talonfx for CANSensorReader
+ std::vector<std::shared_ptr<TalonFX>> talonfxs;
+ for (auto talonfx : {right_front, right_back, left_front, left_back}) {
+ talonfxs.push_back(talonfx);
+ }
+
+ aos::Sender<CANPositionStatic> can_position_sender =
+ can_sensor_reader_event_loop.MakeSender<CANPositionStatic>(
+ "/drivetrain");
+
+ frc971::wpilib::CANSensorReader can_sensor_reader(
+ &can_sensor_reader_event_loop, std::move(signals_registry), talonfxs,
+ [talonfxs, &can_position_sender](ctre::phoenix::StatusCode status) {
+ aos::Sender<CANPositionStatic>::StaticBuilder builder =
+ can_position_sender.MakeStaticBuilder();
+
+ auto falcon_vector = builder->add_talonfxs();
+
+ for (auto talonfx : talonfxs) {
+ talonfx->SerializePosition(
+ falcon_vector->emplace_back(),
+ control_loops::drivetrain::kHighOutputRatio);
+ }
+
+ builder->set_timestamp(talonfxs.front()->GetTimestamp());
+ builder->set_status(static_cast<int>(status));
+
+ builder.CheckOk(builder.Send());
+ });
+
+ AddLoop(&can_sensor_reader_event_loop);
+
+ // Thread 5.
::aos::ShmEventLoop can_output_event_loop(&config.message());
can_output_event_loop.set_name("CANOutputWriter");
- // Thread 5
- // Set up superstructure output.
- ::aos::ShmEventLoop output_event_loop(&config.message());
- output_event_loop.set_name("PWMOutputWriter");
+ frc971::wpilib::CANDrivetrainWriter can_drivetrain_writer(
+ &can_output_event_loop);
- AddLoop(&output_event_loop);
+ can_drivetrain_writer.set_talonfxs({right_front, right_back},
+ {left_front, left_back});
+
+ can_output_event_loop.MakeWatcher(
+ "/roborio", [&can_drivetrain_writer](
+ const frc971::CANConfiguration &configuration) {
+ can_drivetrain_writer.HandleCANConfiguration(configuration);
+ });
+
+ AddLoop(&can_output_event_loop);
// Thread 6