From 913f9b8a9e9537df4ced260175e133c63a51b9c6 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sun, 1 Mar 2020 15:13:00 -0700 Subject: [PATCH 1/3] 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); From e0966aa65b90f33b14752f79d98245fac6e36de5 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sun, 1 Mar 2020 15:34:40 -0700 Subject: [PATCH 2/3] Implements Drive Straight --- .../java/frc4388/robot/RobotContainer.java | 2 +- .../DriveWithJoystickDriveStraight.java | 19 +++++++++++++------ 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4793fc2..b488752 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -127,7 +127,7 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller - m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController())); + m_robotDrive.setDefaultCommand(new DriveWithJoystickDriveStraight(m_robotDrive, getDriverController())); //m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, getDriverController())); // drives intake with input from triggers on the opperator controller diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java index a298f9c..ef9c1b9 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java @@ -21,6 +21,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase { long m_deadTimeSteer, m_deadTimeMove; long m_deadTimeout = 100; IHandController m_controller; + boolean m_isInterrupted; /** * Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller. @@ -54,6 +55,11 @@ public class DriveWithJoystickDriveStraight extends CommandBase { m_deltaTime = System.currentTimeMillis() - m_currTime; m_currTime = System.currentTimeMillis(); + if (m_isInterrupted) { + resetGyroTarget(); + m_isInterrupted = false; + } + /* If steer stick is being used */ if (steerInput != 0) { m_deadTimeSteer = m_currTime; @@ -78,14 +84,14 @@ public class DriveWithJoystickDriveStraight extends CommandBase { } private void runDriveWithInput(double move, double steer) { - double cosMultiplier = .45; + double cosMultiplier = .7; double steerOutput = 0; - double deadzone = .2; + double deadzone = .1; /* Curves the steer output to be similarily gradual */ - if (steer > 0){ - steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone); - } else { - steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone); + if (steer > 0) { + steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steer) + cosMultiplier; + } else if (steer < 0) { + steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steer) - cosMultiplier; } m_drive.driveWithInput(move, steerOutput); System.out.println("Driving With Input"); @@ -108,6 +114,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + m_isInterrupted = interrupted; } // Returns true when the command should end. From 60b67a89ddee10d8cbe07f49b391913052e8f18d Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 2 Mar 2020 23:37:45 +0000 Subject: [PATCH 3/3] Remove true thing and replace with good thing --- .../robot/commands/DriveWithJoystickUsingDeadAssistPID.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index fd2cbe2..0b154e7 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 (true) { + if (moveInput != 0) { m_deadTimeMove = m_currTime; m_stopPos = m_drive.m_rightFrontMotor.getSelectedSensorPosition() + (m_drive.m_rightFrontMotor.getSelectedSensorVelocity());