diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 75703cc..fe6bedf 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -42,6 +42,8 @@ public final class Constants { public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(false, 60, 40, 2); public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops + public static final double COS_MULTIPLIER_LOW = 1.0; + public static final double COS_MULTIPLIER_HIGH = 0.8; /* Drive Train Characteristics */ public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13; @@ -52,16 +54,16 @@ public final class Constants { /* PID Constants Drive*/ public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5); - public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0); - public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55); - public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); - public static final int DRIVE_CRUISE_VELOCITY = 25000; - public static final int DRIVE_ACCELERATION = 21000; + public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.05, 0.0, 1.0, 0.025, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 2.0, 0.0, 0, 0.55); + public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0, 0.025, 0, 1.0); + public static final int DRIVE_CRUISE_VELOCITY = 30000; + public static final int DRIVE_ACCELERATION = 23000; - public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5); - public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); - public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.55); - public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.5); + public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.55); + public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000; public static final int DRIVE_ACCELERATION_HIGH = 7000; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 4f894c9..9e827dd 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -77,7 +77,7 @@ public class Robot extends TimedRobot { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); - //m_robotContainer.setDriveGearState(true); + m_robotContainer.setDriveGearState(false); m_robotContainer.resetOdometry(); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); @@ -105,7 +105,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); - //m_robotContainer.setDriveGearState(true); + m_robotContainer.setDriveGearState(false); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); // This makes sure that the autonomous stops running when diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3afc1a3..6c88f5c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -29,12 +29,15 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.AutoPath1FromCenter; +import frc4388.robot.commands.AutoPath2FromRight; import frc4388.robot.commands.CalibrateShooter; import frc4388.robot.commands.DrivePositionMPAux; import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveStraightToPositionMM; import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveWithJoystick; +import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID; import frc4388.robot.commands.DriveStraightToPositionMM; import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID; @@ -123,7 +126,10 @@ 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 DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController())); + //m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, getDriverController())); + // drives intake with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // runs the turret with joystick @@ -151,15 +157,15 @@ public class RobotContainer { /* Test Buttons */ // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 192)); + .whenPressed(new DriveStraightToPositionMM(m_robotDrive, m_robotPneumatics, 24.0)); // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whileHeld(new DriveStraightAtVelocityPID(m_robotDrive, 6000)); + .whenPressed(new TurnDegrees(m_robotDrive, 90)); // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new GotoCoordinates(m_robotDrive, 12, 12, 0)); + .whenPressed(new Wait(m_robotDrive, 0, 0)); // X driver test button new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) @@ -249,31 +255,9 @@ public class RobotContainer { RamseteCommand ramseteCommand = getRamseteCommand(trajectory); // Run path following command, then stop at the end. //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - - //Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those - //This assumes that we are positioned against the right wall with our shooter facing the target. - return new SequentialCommandGroup(new Wait(0, m_robotDrive), - //add aim command - //add shooter command - //new DriveStraightToPositionMM(m_robotDrive, 48.0), - /*new ParallelCommandGroup( - new StorageIntakeGroup(m_robotIntake, m_robotStorage), - new DriveStraightToPositionMM(m_robotDrive, 36.0)), - new ParallelCommandGroup( - new StorageIntakeGroup(m_robotIntake, m_robotStorage), - new DriveStraightToPositionMM(m_robotDrive, 36.0)), - new StorageIntakeGroup(m_robotIntake, m_robotStorage),*/ - //add aim command - //add shooter command -//Below this would be the picking up additional balls outside of those in the trench directly behind us + return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics); + //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics); - //new GotoCoordinates(m_robotDrive, 36, 36), - new GotoCoordinates(m_robotDrive, 36, 36, -90));//, - //new StorageIntakeGroup(m_robotIntake, m_robotStorage), - //new TurnDegrees(m_robotDrive, 75), - //new DriveStraightToPositionMM(m_robotDrive, 18.0), - //new TurnDegrees(m_robotDrive, -45), - //new DriveStraightToPositionMM(m_robotDrive, 12.0)); } TrajectoryConfig getTrajectoryConfig() { return new TrajectoryConfig( @@ -373,4 +357,4 @@ public class RobotContainer { { return m_driverXbox.getJoyStick(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java new file mode 100644 index 0000000..4ec080a --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* 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.SequentialCommandGroup; +import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class AutoPath1FromCenter extends SequentialCommandGroup { + Drive m_drive; + Pneumatics m_pneumatics; + + /** + * Creates a new AutoPath1FromCenter. + */ + public AutoPath1FromCenter(Drive subsystem, Pneumatics subsystem2) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + m_drive = subsystem; + m_pneumatics = subsystem2; + + addCommands( new Wait(m_drive, 0, 1), + //shoot pre-loaded 3 balls + new GotoCoordinates(m_drive, m_pneumatics, 75, 44, -90), + //Start Intake Ball 1 + new GotoCoordinates(m_drive, m_pneumatics, 0, 12), + new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + //Start Intake Ball 2 + new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + //Start Intake Ball 3 + new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + new Wait(m_drive, 0, 2) + //Shoot 3 Balls + ); + } +} diff --git a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java new file mode 100644 index 0000000..43657c3 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* 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.SequentialCommandGroup; +import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class AutoPath2FromRight extends SequentialCommandGroup { + Drive m_drive; + Pneumatics m_pneumatics; + + /** + * Creates a new AutoPath2FromRight. + */ + public AutoPath2FromRight(Drive subsystem, Pneumatics subsystem2) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + m_drive = subsystem; + m_pneumatics = subsystem2; + + addCommands( new Wait(m_drive, 0, 1), + new GotoCoordinates(m_drive, m_pneumatics, 0, 77), + //Start Intake Ball 1 + new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + //Start Intake Ball 2 + new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + //Shoot 5 Balls + new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + //Start Intake Ball 1 (second round) + new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + //Start Moving to 4th Ball + new GotoCoordinates(m_drive, m_pneumatics, 60, -50), + new Wait(m_drive, 0, 2) + ); + } +} diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java index 925f07a..c9aa927 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java @@ -11,9 +11,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; public class DriveStraightToPositionMM extends CommandBase { Drive m_drive; + Pneumatics m_pneumatics; double m_targetPosIn; double m_targetPosOut; double m_targetGyro; @@ -25,10 +27,19 @@ public class DriveStraightToPositionMM extends CommandBase { * @param subsystem drive subsystem * @param targetPos distance to travel in inches */ - public DriveStraightToPositionMM(Drive subsystem, double targetPos) { + public DriveStraightToPositionMM(Drive subsystem, Pneumatics subsystem2, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2; + m_pneumatics = subsystem2; + try { + if (m_pneumatics.m_isSpeedShiftHigh) { + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2; + } else { + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2; + } + } catch (Exception e) { + System.err.println("Error In Motion Magic Switch Gains."); + } addRequirements(m_drive); //SmartDashboard.putNumber("Distance Target Inches", targetPos); } diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java index c31944e..a457e26 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java @@ -11,9 +11,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; public class DriveStraightToPositionPID extends CommandBase { Drive m_drive; + Pneumatics m_pneumatics; double m_targetPosIn; double m_targetPosOut; double m_targetGyro; @@ -24,10 +26,19 @@ public class DriveStraightToPositionPID extends CommandBase { * @param subsystem drive subsystem * @param targetPos distance to travel in inches */ - public DriveStraightToPositionPID(Drive subsystem, double targetPos) { + public DriveStraightToPositionPID(Drive subsystem, Pneumatics subsystem2, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2; + m_pneumatics = subsystem2; + try { + if (m_pneumatics.m_isSpeedShiftHigh) { + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2; + } else { + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2; + } + } catch (Exception e) { + System.err.println("Error In Motion Magic Switch Gains."); + } addRequirements(m_drive); //SmartDashboard.putNumber("Distance Target Inches", targetPos); } diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index d0bdf96..b7d823f 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -9,12 +9,15 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; import frc4388.utility.controller.IHandController; public class DriveWithJoystick extends CommandBase { private Drive m_drive; private IHandController m_controller; + private Pneumatics m_pneumatics; /** * Creates a new DriveWithJoystick to control the drivetrain with an Xbox controller. @@ -24,9 +27,10 @@ public class DriveWithJoystick extends CommandBase { * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} */ - public DriveWithJoystick(Drive subsystem, IHandController controller) { + public DriveWithJoystick(Drive subsystem, Pneumatics subsystem2, IHandController controller) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; + m_pneumatics = subsystem2; m_controller = controller; addRequirements(m_drive); } @@ -49,8 +53,15 @@ public class DriveWithJoystick extends CommandBase { moveOutput = Math.cos(1.571*moveInput)-1; } - double cosMultiplier = 1.0; + double cosMultiplier; double deadzone = .1; + + if (m_pneumatics.m_isSpeedShiftHigh) { + cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH; + } else { + cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW; + } + if (steerInput > 0){ steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier; } else if (steerInput < 0) { @@ -58,23 +69,31 @@ public class DriveWithJoystick extends CommandBase { } else { steerOutput = 0; } - double tempOutputLimit = 0.8; - boolean isOutputLimited = false; + /* + double outputLimit = 0.8; - if (isOutputLimited) { - if (moveOutput > tempOutputLimit) { - moveOutput = tempOutputLimit; - } else if(moveOutput < -tempOutputLimit) { - moveOutput = -tempOutputLimit; + boolean isMoveOutputLimited = false; + boolean isSteerOutputLimited = false; + + if (m_pneumatics.m_isSpeedShiftHigh) { + if (isMoveOutputLimited) { + if (moveOutput > outputLimit) { + moveOutput = outputLimit; + } else if(moveOutput < -outputLimit) { + moveOutput = -outputLimit; + } } - if (steerOutput > tempOutputLimit) { - steerOutput = tempOutputLimit; - } else if(steerOutput < -tempOutputLimit) { - steerOutput = -tempOutputLimit; + if (isSteerOutputLimited) { + if (steerOutput > outputLimit) { + steerOutput = outputLimit; + } else if(steerOutput < -outputLimit) { + steerOutput = -outputLimit; + } } - } + } + */ m_drive.driveWithInput(moveOutput, steerOutput); } diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java index a298f9c..c69d4e9 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java @@ -7,20 +7,27 @@ package frc4388.robot.commands; +import java.security.PublicKey; + 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.robot.subsystems.Pneumatics; import frc4388.utility.controller.IHandController; public class DriveWithJoystickDriveStraight extends CommandBase { Drive m_drive; + Pneumatics m_pneumatics; double m_targetGyro, m_currentGyro; double m_stopPos; long m_currTime, m_deltaTime; long m_deadTimeSteer, m_deadTimeMove; long m_deadTimeout = 100; IHandController m_controller; + boolean m_isInterrupted; + double highGearMultiplier = 1; + double lowGearMultiplier = 1; /** * Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller. @@ -34,6 +41,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase { public DriveWithJoystickDriveStraight(Drive subsystem, IHandController controller) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; + m_pneumatics = m_drive.m_pneumaticsSubsystem; m_controller = controller; addRequirements(m_drive); } @@ -54,6 +62,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 +91,22 @@ public class DriveWithJoystickDriveStraight extends CommandBase { } private void runDriveWithInput(double move, double steer) { - double cosMultiplier = .45; + + double cosMultiplier; double steerOutput = 0; - double deadzone = .2; - /* Curves the steer output to be similarily gradual */ - if (steer > 0){ - steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone); + double deadzone = .1; + + if (m_pneumatics.m_isSpeedShiftHigh) { + cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH; } else { - steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone); + cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW; + } + + /* Curves the steer output to be similarily gradual */ + 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"); @@ -101,13 +122,19 @@ public class DriveWithJoystickDriveStraight extends CommandBase { */ private void resetGyroTarget() { //m_targetGyro = m_currentGyro; - m_targetGyro = m_currentGyro - + m_drive.getTurnRate(); + if (m_pneumatics.m_isSpeedShiftHigh) { + m_targetGyro = m_currentGyro + + highGearMultiplier * m_drive.getTurnRate(); + } else { + m_targetGyro = m_currentGyro + + lowGearMultiplier * m_drive.getTurnRate(); + } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + m_isInterrupted = interrupted; } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index 5a68a9f..0b154e7 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -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/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java index 5580cfc..6ca3deb 100644 --- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java +++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java @@ -10,12 +10,14 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html public class GotoCoordinates extends SequentialCommandGroup { Drive m_drive; + Pneumatics m_pneumatics; double m_xTarget; double m_yTarget; @@ -26,10 +28,11 @@ public class GotoCoordinates extends SequentialCommandGroup { /** * Creates a new GotoPosition. */ - public GotoCoordinates(Drive subsystem, double xTarget, double yTarget, double endAngle) { + public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); m_drive = subsystem; + m_pneumatics = subsystem2; m_xTarget = xTarget; m_yTarget = yTarget; @@ -38,13 +41,30 @@ public class GotoCoordinates extends SequentialCommandGroup { m_currentAngle = calcAngle(); - SmartDashboard.putNumber("Current Angle Coordinates", m_currentAngle); - m_endAngle = endAngle; + addCommands( new TurnDegrees(m_drive, m_currentAngle), + new Wait(m_drive, 0, 0), + new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), + new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); + } - addCommands( new TurnDegrees(m_drive, m_currentAngle), - new DriveStraightToPositionMM(m_drive, m_hypotDist), + public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) { + m_drive = subsystem; + m_pneumatics = subsystem2; + + m_xTarget = xTarget; + m_yTarget = yTarget; + + m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget)); + + m_currentAngle = calcAngle(); + + m_endAngle = m_currentAngle; + + addCommands( new TurnDegrees(m_drive, m_currentAngle), + new Wait(m_drive, 0, 0), + new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); } diff --git a/src/main/java/frc4388/robot/commands/Wait.java b/src/main/java/frc4388/robot/commands/Wait.java index 934b508..580d279 100644 --- a/src/main/java/frc4388/robot/commands/Wait.java +++ b/src/main/java/frc4388/robot/commands/Wait.java @@ -17,15 +17,19 @@ public class Wait extends CommandBase { long m_waitTime; long m_currentTime; SubsystemBase m_subsystem; + int m_waitNum; + + int counter = 0; /** * Creates a new WaitCommand. */ - public Wait(float seconds, SubsystemBase subsystem) { + public Wait(SubsystemBase subsystem, double seconds, int waitNum) { // Use addRequirements() here to declare subsystem dependencies. m_waitTime = (long) (seconds * 1000); m_subsystem = subsystem; + m_waitNum = waitNum; addRequirements(m_subsystem); } @@ -40,8 +44,15 @@ public class Wait extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + + if (counter == 0) { + SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime); + } + m_currentTime = System.currentTimeMillis(); SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime)); + + counter ++; } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 096feff..599e5e7 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -53,7 +53,7 @@ public class Drive extends SubsystemBase { public Orchestra m_orchestra; /* Pneumatics Subsystem */ - Pneumatics m_pneumaticsSubsystem; + public Pneumatics m_pneumaticsSubsystem; /* Low Gear Gains */ public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW; @@ -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); @@ -127,7 +132,15 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS); /* PID for Front Motor Control in Teleop */ - setRightMotorGains(false); + try { + if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) { + setRightMotorGains(true); + } else { + setRightMotorGains(false); + } + } catch (Exception e) { + System.err.println("Error while trying to switch gains."); + } /* PID for Back Motor Control in Tank Drive Vel */ m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);