From 7339658077b70d486beb1edcb2d10d6197660de8 Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Tue, 25 Feb 2020 19:15:15 -0700
Subject: [PATCH] Fix watchdog feeds and steer output curving

---
 .../java/frc4388/robot/commands/DriveWithJoystick.java |  4 ++--
 src/main/java/frc4388/robot/subsystems/Drive.java      | 10 +++++-----
 2 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index 42aedfc..fe1c8e5 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -51,9 +51,9 @@ public class DriveWithJoystick extends CommandBase {
     double cosMultiplier = .55;
     double deadzone = .2;
     if (steerInput > 0){
-      steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
+      steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
     } else if (steerInput < 0) {
-      steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
+      steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier;
     } else {
       steerOutput = 0;
     }
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index d0e7601..9f2639b 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -119,8 +119,8 @@ public class Drive extends SubsystemBase {
     m_leftFrontMotor.setInverted(false);
     m_rightFrontMotor.setInverted(false);
     //m_driveTrain.setRightSideInverted(false);
-    //m_leftBackMotor.setInverted(InvertType.FollowMaster);
-    //m_rightBackMotor.setInverted(InvertType.FollowMaster);
+    m_leftBackMotor.setInverted(InvertType.FollowMaster);
+    m_rightBackMotor.setInverted(InvertType.FollowMaster);
 
     float rampRate = 0.1f;
     m_rightFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -128,9 +128,9 @@ public class Drive extends SubsystemBase {
     m_leftFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
     m_leftBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
 
-    SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 20, 25, 0.01);
-    m_rightFrontMotor.configSupplyCurrentLimit(c);
-    m_leftFrontMotor.configSupplyCurrentLimit(c);
+    //SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 40, 35, 0.01);
+    //m_rightFrontMotor.configSupplyCurrentLimit(c);
+    //m_leftFrontMotor.configSupplyCurrentLimit(c);
 
     setDriveTrainNeutralMode(NeutralMode.Coast);