task_1

README.md

@@ -164,24 +164,33 @@ To undo the going back:
 
 ## CppCheck Warnings
 ```
-src/frc846/cpp/frc846/other/trajectory_generator.cc:68:18: warning: Consider using std::transform algorithm instead of a raw loop. [useStlAlgorithm]
-src/y2024/cpp/commands/teleop/drive_command.cc:67:8: warning: Condition 'is_robot_centric' is always false [knownConditionTrueFalse]
-src/frc846/cpp/frc846/util/math.cc:19:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
-src/frc846/cpp/frc846/util/math.cc:46:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
+src/frc846/cpp/frc846/ntinf/fstore.cc:287:27: warning: Consider using std::any_of algorithm instead of a raw loop. [useStlAlgorithm]
+src/y2024/cpp/commands/teleop/drive_command.cc:69:8: warning: Condition 'is_robot_centric' is always false [knownConditionTrueFalse]
+src/y2024/cpp/commands/teleop/drive_command.cc:33:17: warning: Variable 'intaking' is assigned a value that is never used. [unreadVariable]
+src/y2024/cpp/field.cc:9:16: warning: Variable 'point' can be declared as reference to const [constVariableReference]
+src/y2024/cpp/field.cc:21:14: warning: Variable 'path' can be declared as reference to const [constVariableReference]
+src/y2024/cpp/field.cc:10:32: warning: Consider using std::find_if algorithm instead of a raw loop. [useStlAlgorithm]
+src/y2024/cpp/field.cc:22:29: warning: Consider using std::find_if algorithm instead of a raw loop. [useStlAlgorithm]
+src/y2024/cpp/field.cc:67:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm]
+src/y2024/cpp/field.cc:77:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm]
+src/y2024/cpp/field.cc:60:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm]
+src/frc846/cpp/frc846/math/collection.cc:7:0: warning: The function 'DEquals' is never used. [unusedFunction]
+src/frc846/cpp/frc846/math/collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
+src/frc846/cpp/frc846/math/collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
 ```
 ## CppCheck Warnings
 ```
-srcfrc846cppfrc846ntinffstore.cc:287:27: warning: Consider using std::any_of algorithm instead of a raw loop. [useStlAlgorithm]
-srcy2024cppcommandsteleopdrive_command.cc:69:8: warning: Condition 'is_robot_centric' is always false [knownConditionTrueFalse]
-srcy2024cppcommandsteleopdrive_command.cc:33:17: warning: Variable 'intaking' is assigned a value that is never used. [unreadVariable]
-srcy2024cppfield.cc:9:16: warning: Variable 'point' can be declared as reference to const [constVariableReference]
-srcy2024cppfield.cc:21:14: warning: Variable 'path' can be declared as reference to const [constVariableReference]
-srcy2024cppfield.cc:10:32: warning: Consider using std::find_if algorithm instead of a raw loop. [useStlAlgorithm]
-srcy2024cppfield.cc:22:29: warning: Consider using std::find_if algorithm instead of a raw loop. [useStlAlgorithm]
-srcy2024cppfield.cc:67:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm]
-srcy2024cppfield.cc:77:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm]
-srcy2024cppfield.cc:60:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm]
-srcfrc846cppfrc846mathcollection.cc:7:0: warning: The function 'DEquals' is never used. [unusedFunction]
-srcfrc846cppfrc846mathcollection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
-srcfrc846cppfrc846mathcollection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
+src/frc846/cpp/frc846/ntinf/fstore.cc:287:27: warning: Consider using std::any_of algorithm instead of a raw loop. [useStlAlgorithm]
+src/y2024/cpp/commands/teleop/drive_command.cc:69:8: warning: Condition 'is_robot_centric' is always false [knownConditionTrueFalse]
+src/y2024/cpp/commands/teleop/drive_command.cc:33:17: warning: Variable 'intaking' is assigned a value that is never used. [unreadVariable]
+src/y2024/cpp/field.cc:9:16: warning: Variable 'point' can be declared as reference to const [constVariableReference]
+src/y2024/cpp/field.cc:21:14: warning: Variable 'path' can be declared as reference to const [constVariableReference]
+src/y2024/cpp/field.cc:10:32: warning: Consider using std::find_if algorithm instead of a raw loop. [useStlAlgorithm]
+src/y2024/cpp/field.cc:22:29: warning: Consider using std::find_if algorithm instead of a raw loop. [useStlAlgorithm]
+src/y2024/cpp/field.cc:67:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm]
+src/y2024/cpp/field.cc:77:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm]
+src/y2024/cpp/field.cc:60:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm]
+src/frc846/cpp/frc846/math/collection.cc:7:0: warning: The function 'DEquals' is never used. [unusedFunction]
+src/frc846/cpp/frc846/math/collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
+src/frc846/cpp/frc846/math/collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
 ```

build.gradle

@@ -111,7 +111,7 @@ spotless {
             include '**/*.cpp', '**/*.cc', '**/*.h', '**/*.hpp'
             exclude '**/build/**', '**/build-*/**'
         }
-        def selectedClangVersion = project.hasProperty('fromCI') ? '14.0.0-1ubuntu1.1' : '18.1.8'
+        def selectedClangVersion = project.hasProperty('fromCI') ? '14.0.0-1ubuntu1.1' : '18.1.8'//18.1.8
         clangFormat(selectedClangVersion).style('Google')
     }
     groovyGradle {

src/y2024/cpp/subsystems/abstract/vision.cc

@@ -1,11 +1,14 @@
 #include "subsystems/abstract/vision.h"
 
 #include <frc/DriverStation.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
 #include <units/math.h>
 
 #include "field.h"
 #include "frc846/math/vectors.h"
 #include "frc846/util/share_tables.h"
+#include "frc846/wpilib/time.h"
 
 VisionSubsystem::VisionSubsystem(bool init)
     : frc846::robot::GenericSubsystem<VisionReadings, VisionTarget>("vision",
@@ -49,6 +52,14 @@ VisionReadings VisionSubsystem::ReadFromHardware() {
   units::degree_t bearing_ =
       units::degree_t(frc846::util::ShareTables::GetDouble("robot_bearing_"));
 
+  units::angular_velocity::degrees_per_second_t bearing_velocity =
+      units::angular_velocity::degrees_per_second_t(
+          frc846::util::ShareTables::GetDouble("bearing_velocity"));
+
+  units::second_t latency_seconds{latency};
+
+  bearing_ = bearing_ - bearing_velocity * latency_seconds;
+
   units::inch_t robot_x =
       units::foot_t(frc846::util::ShareTables::GetDouble("odometry_x_"));
 

src/y2024/cpp/subsystems/hardware/drivetrain.cc

@@ -239,6 +239,8 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
                                        readings.pose.velocity[0].to<double>());
   frc846::util::ShareTables::SetDouble("velocity_y_",
                                        readings.pose.velocity[1].to<double>());
+  frc846::util::ShareTables::SetDouble("bearing_velocity",
+                                       readings.angular_velocity.to<double>());
 
   SetMap();