Final swerve adjustments

PhyXTGears-programming · Feb 26, 2022 · 56d56d4 · 56d56d4
1 parent aced41c
commit 56d56d4
Show file tree

Hide file tree

Showing 6 changed files with 68 additions and 11 deletions.
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
@@ -5,7 +5,7 @@
 #include <frc/smartdashboard/SmartDashboard.h>
 #include <frc2/command/CommandScheduler.h>
 
-#define DEADZONE(v) (std::abs(v) < 0.05 ? 0 : v)
+#define DEADZONE(v) (std::abs(v) < 0.04 ? 0 : v)
 
 void Robot::RobotInit () {
     std::cout << std::fixed;
@@ -29,9 +29,10 @@ void Robot::TeleopPeriodic () {
     double x = driver.GetX(frc::GenericHID::kLeftHand);
     double y = -1 * driver.GetY(frc::GenericHID::kLeftHand);
     double r = driver.GetX(frc::GenericHID::kRightHand);
-    x = DEADZONE(x) * 0.6;
-    y = DEADZONE(y) * 0.6;
-    r = DEADZONE(r) * 0.35;
+    x = DEADZONE(x) * 1.0;
+    y = DEADZONE(y) * 1.0;
+    r = DEADZONE(r);
+    r = r*r*r * 0.5;
     // std::cout << x << ", " << y << ", " << r << std::endl;
     m_container.swerveDrive.setMotion(x, y, r);
 }

diff --git a/src/main/cpp/subsystems/SwerveDrive.cpp b/src/main/cpp/subsystems/SwerveDrive.cpp
@@ -2,12 +2,23 @@
 
 #include <frc/smartdashboard/SmartDashboard.h>
 
-SwerveDrive::SwerveDrive () {
+constexpr double PI = 3.1415926535897932;
+
+SwerveDrive::SwerveDrive (bool fieldOriented) : fieldOriented(fieldOriented) {
     drive = new swervedrive::drive<double, double, double>({&flWheel, &frWheel, &blWheel, &brWheel});
+
+    gyro.SetYawAxis(frc::ADIS16470_IMU::IMUAxis::kZ);
+    gyro.Reset();
 }
 
 void SwerveDrive::Periodic () {}
 
 void SwerveDrive::setMotion (double x, double y, double r) {
-    drive->set_motion({x, y}, r);
+    double a = 0;
+
+    if (fieldOriented) {
+        a = gyro.GetAngle() * (PI/180.0);
+    }
+
+    drive->set_motion({x, y}, r, a);
 }
diff --git a/src/main/cpp/subsystems/SwerveWheel.cpp b/src/main/cpp/subsystems/SwerveWheel.cpp
@@ -27,11 +27,12 @@ SwerveWheel::SwerveWheel (constants::swerve::WheelConstants constants)
     driveMotor = new rev::CANSparkMax(wheelSettings.drivePin, rev::CANSparkMax::MotorType::kBrushless);
     turnMotor = new rev::CANSparkMax(wheelSettings.turnPin, rev::CANSparkMax::MotorType::kBrushless);
 
+    driveMotor->SetInverted(false);
+    turnMotor->SetInverted(true);
+
     encoder = new ctre::phoenix::sensors::CANCoder(wheelSettings.encoderID);
     encoder->ConfigAbsoluteSensorRange(ctre::phoenix::sensors::AbsoluteSensorRange::Signed_PlusMinus180);
 
-    turnMotor->SetInverted(true);
-
     constexpr double conversionFactor = 2 * M_PI * 0.01816051; // motor rotations to module rad
     turnMotor->GetEncoder().SetPositionConversionFactor(conversionFactor);
     turnMotor->GetEncoder().SetVelocityConversionFactor(conversionFactor / 60.0); // RPM to rad/s
@@ -62,7 +63,14 @@ double SwerveWheel::getAngle () {
 }
 
 void SwerveWheel::setAngle (double rad) {
-    std::pair<double, bool> result = swervedrive::module_rotation::calculate_rotation_target(rad, getAngle());
+    #ifdef TALON_SRX
+    double correction = 0;
+    #endif
+    #ifdef SPARK_MAX
+    double correction = std::copysign(0.04, turnMotor->GetEncoder().GetVelocity()); // add correction to account for lag and momentum
+    #endif
+
+    std::pair<double, bool> result = swervedrive::module_rotation::calculate_rotation_target(rad, getAngle() + correction);
     inverted = result.second;
 
     #ifdef TALON_SRX

diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h
@@ -10,7 +10,7 @@ class RobotContainer {
 
         frc2::Command* GetAutonomousCommand();
 
-        SwerveDrive swerveDrive;
+        SwerveDrive swerveDrive {true};
 
     private:
         void ConfigureButtonBindings();

diff --git a/src/main/include/subsystems/SwerveDrive.h b/src/main/include/subsystems/SwerveDrive.h
@@ -3,6 +3,7 @@
 #include <frc2/command/SubsystemBase.h>
 #include <frc/XboxController.h>
 #include <ctre/phoenix/motorcontrol/can/TalonSRX.h>
+#include <adi/ADIS16470_IMU.h>
 
 #include <swerve/drive.h>
 
@@ -11,7 +12,7 @@
 
 class SwerveDrive : public frc2::SubsystemBase {
     public:
-        SwerveDrive();
+        SwerveDrive(bool fieldOriented = false);
 
         void Periodic() override;
 
@@ -24,4 +25,8 @@ class SwerveDrive : public frc2::SubsystemBase {
         SwerveWheel frWheel {constants::swerve::frontRight};
         SwerveWheel blWheel {constants::swerve::backLeft};
         SwerveWheel brWheel {constants::swerve::backRight};
+
+        frc::ADIS16470_IMU gyro {};
+
+        bool fieldOriented;
 };
diff --git a/vendordeps/ADIS16470.json b/vendordeps/ADIS16470.json
@@ -0,0 +1,32 @@
+{
+    "fileName": "ADIS16470.json",
+    "name": "ADIS16470-IMU",
+    "version": "2020.r4",
+    "uuid": "6606dc7e-fb96-436f-a804-c07a5d841a14",
+    "mavenUrls": [
+        "http://maven.highcurrent.io/maven"
+    ],
+    "jsonUrl": "http://maven.highcurrent.io/vendordeps/ADIS16470.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.github.juchong",
+            "artifactId": "adis16470-java",
+            "version": "2020.r4"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.github.juchong",
+            "artifactId": "adis16470-cpp",
+            "version": "2020.r4",
+            "libName": "libadis16470imu",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena"
+            ]
+        }
+    ]
+}