From 913f9b8a9e9537df4ced260175e133c63a51b9c6 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sun, 1 Mar 2020 15:13:00 -0700 Subject: [PATCH] Work on dead assist --- .../commands/DriveWithJoystickUsingDeadAssistPID.java | 10 +++++----- src/main/java/frc4388/robot/subsystems/Drive.java | 5 +++++ 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index 5a68a9f..fd2cbe2 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -82,7 +82,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { } /* If move stick is being used */ - if (moveInput != 0) { + if (true) { m_deadTimeMove = m_currTime; m_stopPos = m_drive.m_rightFrontMotor.getSelectedSensorPosition() + (m_drive.m_rightFrontMotor.getSelectedSensorVelocity()); @@ -122,14 +122,14 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { } private void runDriveWithInput(double move, double steerInput) { - double cosMultiplier = .55; + double cosMultiplier = .70; double steerOutput = 0; - double deadzone = .2; + double deadzone = .1; /* Curves the steer output to be similarily gradual */ 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); } m_drive.driveWithInput(move, steerOutput); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index edcbac5..059d3c6 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -114,6 +114,11 @@ public class Drive extends SubsystemBase { m_leftFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); m_leftBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); + /* Config Supply Current Limit (Use only for debugging) */ //m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); //m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);