From b2fcc3c9669e14909238e150b2b1d422d4d66dd2 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Wed, 12 Feb 2020 20:30:44 -0700 Subject: [PATCH 1/4] Add dead assist to make the robot stay its course when given no input --- .../DriveWithJoystickUsingDeadAssistPID.java | 108 ++++++++++++++++++ 1 file changed, 108 insertions(+) create mode 100644 src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java new file mode 100644 index 0000000..3d338fb --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -0,0 +1,108 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpiutil.math.MathUtil; +import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.subsystems.Drive; +import frc4388.utility.controller.IHandController; + +public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { + Drive m_drive; + double m_targetGyro; + long lastTime; + IHandController m_controller; + boolean isAuxPIDEnabled = false; + + /** + * Creates a new DriveWithJoystickUsingDeadAssist. + */ + public DriveWithJoystickUsingDeadAssistPID(Drive subsystem, IHandController controller) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = subsystem; + m_controller = controller; + addRequirements(m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + lastTime = System.currentTimeMillis(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); + double moveInput = m_controller.getLeftYAxis(); + double steerInput = m_controller.getRightXAxis(); + double moveOutput = 0; + double steerOutput = 0; + long deltaTime = System.currentTimeMillis() - lastTime; + lastTime = System.currentTimeMillis(); + + /* If AuxPID is enabled, then update using the steer input */ + if (isAuxPIDEnabled) { + m_targetGyro += 2 * steerInput * deltaTime; + + m_targetGyro = MathUtil.clamp( m_targetGyro, + currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4), + currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4)); + } + /* Otherwise set target angle to current angle */ + else { + m_targetGyro = currentGyro; + } + + /* If move stick is being used */ + if (moveInput != 0) { + /* Curves the moveInput to be slightly more gradual at first */ + if (moveInput >= 0) { + moveOutput = -Math.cos(1.571*moveInput)+1; + } else { + moveOutput = Math.cos(1.571*moveInput)-1; + } + + /* If steer stick is being used. */ + if (steerInput != 0) { + double cosMultiplier = .45; + double deadzone = .2; + /* Curves the steer output to be similarily gradual */ + if (steerInput > 0){ + steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone); + } else { + steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone); + } + m_drive.driveWithInput(moveOutput, steerOutput); + isAuxPIDEnabled = false; + } + /* If only the move stick is being used */ + else { + m_drive.driveWithInputAux(moveOutput, m_targetGyro); + isAuxPIDEnabled = true; + } + } + /* If the move stick is not being used */ + else { + m_drive.runDriveStraightVelocityPID(0, m_targetGyro); + isAuxPIDEnabled = true; + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} From cf061c11709adcbe442ba87c2514ef9feecf9991 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Wed, 12 Feb 2020 20:33:16 -0700 Subject: [PATCH 2/4] Update DriveWithJoystickUsingDeadAssist Documentation --- .../commands/DriveWithJoystickUsingDeadAssistPID.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index 3d338fb..11f3261 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -21,7 +21,13 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { boolean isAuxPIDEnabled = false; /** - * Creates a new DriveWithJoystickUsingDeadAssist. + * Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller. + * Applies a curved ramp to the input from the controllers to make the robot less "touchy". + * Also uses PIDs to keep the robot on course when given a "dead" or 0 input. + * @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the + * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in + * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} */ public DriveWithJoystickUsingDeadAssistPID(Drive subsystem, IHandController controller) { // Use addRequirements() here to declare subsystem dependencies. From b9919693cd3533c060d5ea29972323863e095cac Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Wed, 12 Feb 2020 20:37:45 -0700 Subject: [PATCH 3/4] Make Dead Assist default drive mode --- src/main/java/frc4388/robot/RobotContainer.java | 8 ++------ .../commands/DriveWithJoystickUsingDeadAssistPID.java | 2 +- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2585fbc..a431921 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -18,8 +18,7 @@ import frc4388.robot.Constants.*; import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveStraightToPositionMM; import frc4388.robot.commands.DriveStraightToPositionPID; -import frc4388.robot.commands.DriveWithJoystick; -import frc4388.robot.commands.DriveWithJoystickAuxPID; +import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; @@ -30,9 +29,6 @@ import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Leveler; import frc4388.robot.subsystems.Storage; import frc4388.utility.LEDPatterns; @@ -68,7 +64,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, getDriverController())); + m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, getDriverController())); // drives intake with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // drives climber with input from triggers on the opperator controller diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index 11f3261..0a7938e 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -61,7 +61,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4), currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4)); } - /* Otherwise set target angle to current angle */ + /* Otherwise set target angle to current angle (prevents buildup of gyro error) */ else { m_targetGyro = currentGyro; } From ad5125ff4be87593b8e803bbd231961c63d3c1ed Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Wed, 12 Feb 2020 20:42:45 -0700 Subject: [PATCH 4/4] Messing with Constants to make DriveTrain more responsive when in Dead Assist - Added a P to Velocity Control to make robot resist being pushed. - Upped the Output Limit on the Turning PID to allow it to be more aggressive. --- src/main/java/frc4388/robot/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bcd15f8..e05af11 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,8 +29,8 @@ public final class Constants { /* PID Constants Drive*/ public static final int DRIVE_TIMEOUT_MS = 30; public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.3); - public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.0, 0.0, 0.0, 0.1, 0, 1.0); - public static final Gains DRIVE_TURNING_GAINS = new Gains(0.4, 0.0, 0.0, 0.0, 0, 0.3); + public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.0, 0.1, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS = new Gains(0.4, 0.0, 0.0, 0.0, 0, 0.5); public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); public static final int DRIVE_CRUISE_VELOCITY = 20000; public static final int DRIVE_ACCELERATION = 7000;