Drivetrain::faceDirection method implemented

build.gradle

@@ -41,7 +41,7 @@ deploy {
 def includeSrcInIncludeRoot = false
 
 // Set this to true to enable desktop support.
-def includeDesktopSupport = true
+def includeDesktopSupport = false
 
 // Enable simulation gui support. Must check the box in vscode to enable support
 // upon debugging

src/main/cpp/Drivetrain.cpp

@@ -2,7 +2,15 @@
 #include <frc/kinematics/ChassisSpeeds.h>
 #include <frc/kinematics/SwerveModuleState.h>
 
+#include "Twist.hpp"
+#include "Wheel.hpp"
+#include <AHRS.h>
+#include <array>
+#include <frc/geometry/Translation2d.h>
 #include <thread>
+#include <iostream>
+#include <memory>
+
 // #include <cmath>
 
 inline static std::array<std::unique_ptr<Wheel>, 4> wheels {
@@ -38,7 +46,7 @@ void Drivetrain::reset_gyro()
 
 double Drivetrain::get_angle()
 {
-    return -navx->GetAngle();
+    return -navx->GetAngle() + 90;
 }
 
 void Drivetrain::drive(frc::ChassisSpeeds const& feild_speeds)
@@ -47,7 +55,7 @@ void Drivetrain::drive(frc::ChassisSpeeds const& feild_speeds)
         feild_speeds.vx,
         feild_speeds.vy,
         feild_speeds.omega,
-        frc::Rotation2d { units::degree_t { get_angle() + 90 } });
+        frc::Rotation2d { units::degree_t { get_angle() } });
     auto const module_states = m_kinematics.ToSwerveModuleStates(speeds);
     drive(module_states);
 }
@@ -63,6 +71,27 @@ void Drivetrain::drive(wpi::array<frc::SwerveModuleState, 4> const& module_state
         ts.join();
 }
 
+void Drivetrain::face_direction(units::meters_per_second_t dx, units::meters_per_second_t dy, units::degree_t theta)
+{
+    auto const currentRotation = units::degree_t { get_angle() };
+    auto const errorTheta      = currentRotation - theta;
+    auto const rotateP         = 1.5;
+    auto       pRotation       = errorTheta * rotateP / 1_s;
+    if(pRotation > 90_deg / 1_s)
+        pRotation = 90_deg / 1_s;
+    drive({ dx, dy, pRotation });
+}
+
+void Drivetrain::face_closest(units::meters_per_second_t dx, units::meters_per_second_t dy)
+{
+    auto const currentRotation = get_angle();
+    auto const errorTheta      = (ngr::fabs(currentRotation) <= 90)
+                                ? currentRotation
+                                : currentRotation - 180;
+    face_direction(dx, dy, units::degree_t {errorTheta});
+}
+
+
 void Drivetrain::goto180()
 {
     wpi::array<frc::SwerveModuleState, 4> states {

src/main/cpp/Robot.cpp

@@ -196,7 +196,7 @@ void Robot::EightBall()
 
     // Go to traverse
     while(IsAutonomous() && IsEnabled() &&
-          ! (Hood::goToPosition(HOOD::POSITION::BOTTOM, fabs(HOOD::POSITION::SAFE_TO_TURN)) && Turret::goToPosition(TURRET::POSITION::ZERO)))
+          ! (Hood::goToPosition(HOOD::POSITION::BOTTOM, ngr::fabs(HOOD::POSITION::SAFE_TO_TURN)) && Turret::goToPosition(TURRET::POSITION::ZERO)))
         std::this_thread::sleep_for(10ms);
 
     ///////////////////////
@@ -364,6 +364,26 @@ void Robot::TestPeriodic()
     // Turret::visionTrack(TURRET::POSITION::BACK);
     //Drivetrain::print();
     // ShooterWheel::bangbang();
+
+    double x = BUTTON::ps5.GetX() * WHEELS::speed_mult;
+
+    if(fabs(x) < .1)
+        x = 0;
+    double y = -BUTTON::ps5.GetY() * WHEELS::speed_mult;
+
+    if(fabs(y) < .1)
+        y = 0;
+
+    double rotate = BUTTON::ps5.GetZ() * 2;
+    if(fabs(rotate) < .1)
+        rotate = 0;
+
+    if(BUTTON::DRIVETRAIN::ROTATE_FRONT)
+        Drivetrain::face_direction(units::meters_per_second_t { x }, units::meters_per_second_t { y }, 0_deg);
+    if(BUTTON::DRIVETRAIN::ROTATE_BACK)
+        Drivetrain::face_direction(units::meters_per_second_t { x }, units::meters_per_second_t { y }, 180_deg);
+    if(BUTTON::DRIVETRAIN::ROTATE_TO_CLOSEST)
+        Drivetrain::face_closest(units::meters_per_second_t { x }, units::meters_per_second_t { y });
 }
 
 void Robot::DisabledInit()
@@ -408,7 +428,7 @@ void Robot::ButtonManager()
     else
     {
         deployIntake = false;
-        if(Hood::goToPosition(HOOD::POSITION::BOTTOM, fabs(HOOD::POSITION::SAFE_TO_TURN)))
+        if(Hood::goToPosition(HOOD::POSITION::BOTTOM, ngr::fabs(HOOD::POSITION::SAFE_TO_TURN)))
             Turret::goToPosition(TURRET::POSITION::ZERO);
     }
 
@@ -452,9 +472,16 @@ void Robot::ButtonManager()
     // else if(BUTTON::DRIVETRAIN::REVERSE)
     //     Drivetrain::goto180();
     // else
-    Drivetrain::drive(frc::ChassisSpeeds { units::meters_per_second_t { x },
-                                           units::meters_per_second_t { y },
-                                           units::radians_per_second_t { rotate } });
+    if(BUTTON::DRIVETRAIN::ROTATE_FRONT)
+        Drivetrain::face_direction(units::meters_per_second_t { x }, units::meters_per_second_t { y }, 0_deg);
+    else if(BUTTON::DRIVETRAIN::ROTATE_BACK)
+        Drivetrain::face_direction(units::meters_per_second_t { x }, units::meters_per_second_t { y }, 180_deg);
+    else if(BUTTON::DRIVETRAIN::ROTATE_TO_CLOSEST)
+        Drivetrain::face_closest(units::meters_per_second_t { x }, units::meters_per_second_t { y });
+    else
+        Drivetrain::drive(frc::ChassisSpeeds { units::meters_per_second_t { x },
+                                               units::meters_per_second_t { y },
+                                               units::radians_per_second_t { rotate } });
 }
 
 bool Robot::aim(TURRET::POSITION direction)

src/main/include/Constants.hpp

@@ -41,6 +41,9 @@ namespace BUTTON
     {
         // inline JoystickButton ZERO { BUTTON::lStick, 10 };
         // inline JoystickButton REVERSE { BUTTON::lStick, 11 };
+        inline JoystickButton ROTATE_FRONT { BUTTON::ps5, 6 };
+        inline JoystickButton ROTATE_BACK { BUTTON::ps5, 5 };
+        inline JoystickButton ROTATE_TO_CLOSEST { BUTTON::ps5, 7 };
     } // namespace DRIVETRAIN
 } // namespace BUTTON
 

src/main/include/Drivetrain.hpp

@@ -1,15 +1,7 @@
 #ifndef __DRIVETRAIN_H__
 #define __DRIVETRAIN_H__
 
-#include "Twist.hpp"
-#include "Wheel.hpp"
-#include <AHRS.h>
-#include <array>
-#include <frc/geometry/Translation2d.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
-#include <iostream>
-#include <memory>
-#include <thread>
 
 namespace Drivetrain
 {
@@ -22,6 +14,8 @@ namespace Drivetrain
     void   goto180();
     void   PrintWheelAngle(int);
     void   reset_gyro();
+    void   face_direction(units::meters_per_second_t dx, units::meters_per_second_t dy, units::degree_t theta);
+    void   face_closest(units::meters_per_second_t dx, units::meters_per_second_t dy);
 } // namespace Drivetrain
 
 #endif // __DRIVETRAIN_H__