Feat/field oriented drive (#57)

* add field oriented drive

* fix the limiting of drive speed to steer error

* fix the gyro readings (read the navx docs, had to to the Omnimount thing)

* actual field oriented drive

* fix field-oriented drive, and thus far it works pretty well (not entirely well, but that's more a matter for feat/swerve-drive-tuning or something)

* put the inversion logic for teleop back in

* second driver camera

* make default speed and acceleration faster

* add drive deadband, remove duplicated code

Author: danielbrownmsm

shuffleboard.json

@@ -497,6 +497,62 @@
           }
         }
       }
+    },
+    {
+      "title": "camera",
+      "autoPopulate": false,
+      "autoPopulatePrefix": "",
+      "widgetPane": {
+        "gridSize": 128.0,
+        "showGrid": true,
+        "hgap": 16.0,
+        "vgap": 16.0,
+        "titleType": 0,
+        "tiles": {
+          "0,0": {
+            "size": [
+              6,
+              5
+            ],
+            "content": {
+              "_type": "Camera Stream",
+              "_source0": "camera_server://HD_USB_Camera-output",
+              "_title": "HD_USB_Camera-output",
+              "_glyph": 148,
+              "_showGlyph": false,
+              "Crosshair/Show crosshair": false,
+              "Crosshair/Crosshair color": "#FFFFFFFF",
+              "Controls/Show controls": true,
+              "Controls/Rotation": "NONE",
+              "compression": -1.0,
+              "fps": -1,
+              "imageWidth": 240,
+              "imageHeight": 120
+            }
+          },
+          "6,0": {
+            "size": [
+              7,
+              5
+            ],
+            "content": {
+              "_type": "Camera Stream",
+              "_source0": "camera_server://USB Camera 0",
+              "_title": "USB Camera 0",
+              "_glyph": 148,
+              "_showGlyph": false,
+              "Crosshair/Show crosshair": false,
+              "Crosshair/Crosshair color": "#FFFFFFFF",
+              "Controls/Show controls": true,
+              "Controls/Rotation": "NONE",
+              "compression": -1.0,
+              "fps": -1,
+              "imageWidth": 240,
+              "imageHeight": 120
+            }
+          }
+        }
+      }
     }
   ],
   "windowGeometry": {

src/main/cpp/RobotContainer.cpp

@@ -310,7 +310,7 @@ bool RobotContainer::InitCommands() {
     score_preload_balance_auto_ = std::make_shared<ScorePreloadedBalanceAuto>(swerve_drive_, arm_, intake_);
 
     // swerve commands
-    swerve_teleop_command_ = std::make_shared<TeleOpSwerveCommand>(swerve_drive_, gamepad1_, 0.9, 0.6, false); // speed mod, open loop
+    swerve_teleop_command_ = std::make_shared<TeleOpSwerveCommand>(swerve_drive_, gamepad1_, 1, 0.4, false); // speed mod, open loop
     slow_swerve_teleop_command_ = std::make_shared<TeleOpSwerveCommand>(swerve_drive_, gamepad1_, 0.5, 1, true); // brake mode
     fast_swerve_teleop_command_ = std::make_shared<TeleOpSwerveCommand>(swerve_drive_, gamepad1_, 1.5, 0.1, false); // BOOOOOOOOOST
     OKC_CHECK(swerve_teleop_command_ != nullptr);

src/main/cpp/Utils.cpp

@@ -1,5 +1,6 @@
 #include "Utils.h"
 
+
 namespace TeamOKC {
     // TODO: make this a templated function.
     bool Clamp(const double &lower, const double &upper, double *value) {
@@ -27,5 +28,9 @@ namespace TeamOKC {
         }
         return true;
     }
+
+    double Radians(double degrees) {
+        return degrees * M_PI / 180.0;
+    }
 } // namespace TeamOKC
 

src/main/cpp/io/SwerveDriveIO.cpp

@@ -154,7 +154,7 @@ bool SwerveDriveIO::ProcessInputs() {
     // navX IMU:
     OKC_CHECK(hw_interface_->ahrs != nullptr);
     #ifdef __FRC_ROBORIO__
-        sw_interface_->imu_yaw = hw_interface_->ahrs->GetAngle();
+        sw_interface_->imu_yaw = hw_interface_->ahrs->GetYaw();
         sw_interface_->imu_pitch = hw_interface_->ahrs->GetPitch();
     #endif
     #ifndef __FRC_ROBORIO__

src/main/cpp/subsystems/SwerveDrive.cpp

@@ -140,27 +140,42 @@ bool SwerveDrive::SetIdleMode(rev::CANSparkMax::IdleMode mode) {
 }
 
 bool SwerveDrive::VectorTeleOpDrive(const double &drive, const double &strafe, const double &turn) {
-    double final_drive = drive * control_decay + last_drive * (1 - control_decay);
-    double final_strafe = strafe * control_decay + last_strafe * (1 - control_decay);
-    double final_turn = turn * control_decay  + last_turn * (1 - control_decay);
+    double drive_ = drive;
+    double strafe_ = strafe;
+    double turn_ = turn;
 
     // if turn is very small
-    if (abs(final_turn) < 0.3) {
-        final_turn = 0.0; // then zero it
+    if (abs(turn) < 0.3) {
+        turn_ = 0.0; // then zero it
     // } else if (final_turn)
     }
 
     // if strafe is very small
-    if (abs(final_strafe) < 0.15) {
-        final_strafe = 0.0; // then zero it
+    if (abs(strafe) < 0.15) {
+        strafe_ = 0.0; // then zero it
     }
 
+    if (abs(drive) < 0.1) {
+        drive_ = 0.0;
+    }
+
+    double heading = 0.0;
+    OKC_CALL(this->GetHeading(&heading));
+    heading *= -1;
+
+    double temp_drive = drive_;
+    double temp_strafe = strafe_;
+
+    // field oriented drive (page 10 of https://www.first1684.com/uploads/2/0/1/6/20161347/chimiswerve_whitepaper__2_.pdf)
+    drive_ = temp_drive * cos(TeamOKC::Radians(heading))  +  temp_strafe * sin(TeamOKC::Radians(heading));
+    strafe_ = temp_strafe * cos(TeamOKC::Radians(heading))  -  temp_drive * sin(TeamOKC::Radians(heading));
+
     // copied from ChiefDelphi thread
     //TODO post link here
-    double A = final_strafe - final_turn * tracklength_/2;
-    double B = final_strafe + final_turn * tracklength_/2;
-    double C = final_drive - final_turn * trackwidth_/2;
-    double D = final_drive + final_turn * trackwidth_/2;
+    double A = strafe_ - turn_ * tracklength_/2;
+    double B = strafe_ + turn_ * tracklength_/2;
+    double C = drive_ - turn_ * trackwidth_/2;
+    double D = drive_ + turn_ * trackwidth_/2;
 
     // speed
     double left_front_speed = sqrt(pow(B, 2) + pow(D, 2));
@@ -236,14 +251,14 @@ bool SwerveDrive::VectorTeleOpDrive(const double &drive, const double &strafe, c
     // really nice convoluted deadband
     // this is to stop the swerve modules from immediately trying to center themselves instead of
     // coasting until receiving another instruction so we don't tip
-    if (abs(final_drive) > 0.05 || abs(final_strafe) > 0.05 || abs(final_turn) > 0.3) {
+    if (abs(drive_) > 0.05 || abs(strafe_) > 0.05 || abs(turn_) > 0.3) {
         OKC_CALL(this->left_front_module_->SetAngle(left_front_turn));
         OKC_CALL(this->left_back_module_->SetAngle(left_back_turn));
         OKC_CALL(this->right_front_module_->SetAngle(right_front_turn));
         OKC_CALL(this->right_back_module_->SetAngle(right_back_turn));
     }
 
-    if (abs(final_drive) < 0.05 && abs(final_strafe) < 0.05 && abs(final_turn) < 0.3) {
+    if (abs(drive_) < 0.05 && abs(strafe_) < 0.05 && abs(turn_) < 0.3) {
         this->interface_->left_front_drive_motor_output = 0.0;
         this->interface_->left_back_drive_motor_output = 0.0;
         this->interface_->right_front_drive_motor_output = 0.0;
@@ -254,10 +269,10 @@ bool SwerveDrive::VectorTeleOpDrive(const double &drive, const double &strafe, c
         double right_front_steer_error = 0.0;
         double right_back_steer_error = 0.0;
 
-        this->interface_->left_front_drive_motor_output = cos(left_front_module_->GetSteerError(&left_front_steer_error)) * left_front_speed;
-        this->interface_->left_back_drive_motor_output = cos(left_front_module_->GetSteerError(&left_back_steer_error)) * left_back_speed;
-        this->interface_->right_front_drive_motor_output = cos(left_front_module_->GetSteerError(&right_front_steer_error)) * right_front_speed;
-        this->interface_->right_back_drive_motor_output = cos(left_front_module_->GetSteerError(&right_back_steer_error)) * right_back_speed;
+        this->interface_->left_front_drive_motor_output = cos(TeamOKC::Radians(left_front_module_->GetSteerError(&left_front_steer_error))) * left_front_speed;
+        this->interface_->left_back_drive_motor_output = cos(TeamOKC::Radians(left_back_module_->GetSteerError(&left_back_steer_error))) * left_back_speed;
+        this->interface_->right_front_drive_motor_output = cos(TeamOKC::Radians(right_front_module_->GetSteerError(&right_front_steer_error))) * right_front_speed;
+        this->interface_->right_back_drive_motor_output = cos(TeamOKC::Radians(right_back_module_->GetSteerError(&right_back_steer_error))) * right_back_speed;
     }
 
     OKC_CALL(this->left_front_module_->GetSteerOutput(&this->interface_->left_front_steer_motor_output));

src/main/include/Utils.h

@@ -2,6 +2,9 @@
 #pragma once
 
 #include <iostream>
+#define _USE_MATH_DEFINES
+#include <math.h>
+
 
 #define OKC_CALL(res)                                                          \
     if (!(res)) {                                                              \
@@ -58,9 +61,11 @@ namespace TeamOKC {
     } Pose;
 
     bool WrapAngle(double *angle);
+    double Radians(double degrees);
 
     typedef struct arm_state_t {
         double extension;
         double rotation;
+
     } ArmState;
 }

src/main/include/io/SwerveDriveIO.h

@@ -59,7 +59,7 @@ typedef struct swerve_drive_hardware_interface_t {
 typedef struct swerve_drive_software_interface_t {
     // SW INPUTS
     // IMU yaw angle
-    double imu_yaw;
+    float imu_yaw;
     float imu_pitch;
 
     // Encoders