diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2e159df..9e4f64d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -19,6 +19,7 @@ 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.RunIntakeWithTriggers; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; @@ -73,6 +74,9 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500)) .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); + + new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + .whileHeld(new DriveWithJoystickAuxPID(m_robotDrive, getDriverController())); // resets the yaw of the pigeon new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 72)); diff --git a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java deleted file mode 100644 index 625e522..0000000 --- a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java +++ /dev/null @@ -1,60 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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 frc4388.robot.Constants.DriveConstants; -import frc4388.robot.subsystems.Drive; - -public class DriveToDistanceMM extends CommandBase { - Drive m_drive; - double m_distance; - double m_leftTarget; - double m_rightTarget; - - /** - * Creates a new DriveToDistancePID. - * @param subsystem drive subsystem - * @param distance distance to travel in inches - */ - public DriveToDistanceMM(Drive subsystem, double distance) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; - m_distance = distance * DriveConstants.TICKS_PER_INCH; - addRequirements(m_drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance; - m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - //m_drive.runMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget); - //m_drive.runMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget); - } - - // 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() { - if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){ - return true; - } else { - return false; - } - } -} diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java new file mode 100644 index 0000000..2531847 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* 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 com.ctre.phoenix.motorcontrol.TalonFXControlMode; + +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 DriveWithJoystickAuxPID extends CommandBase { + Drive m_drive; + double m_targetGyro; + long lastTime; + IHandController m_controller; + + /** + * Creates a new DriveWithJoystickAuxPID. + */ + public DriveWithJoystickAuxPID(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() { + m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); + 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; + long deltaTime = System.currentTimeMillis() - lastTime; + lastTime = System.currentTimeMillis(); + if (moveInput >= 0){ + moveOutput = -Math.cos(1.571*moveInput)+1; + } else { + moveOutput = Math.cos(1.571*moveInput)-1; + } + + 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)); + + m_drive.driveWithInputAux(moveOutput, m_targetGyro); + + System.err.println("Target: " + m_targetGyro); + System.err.println("Current: " + currentGyro); + System.err.println(); + } + + // 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; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 2f4c2e0..e629ab8 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -330,6 +330,15 @@ public class Drive extends SubsystemBase { m_driveTrain.arcadeDrive(move, steer); } + public void driveWithInputAux(double move, double targetGyro) { + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + + m_rightFrontMotor.set(TalonFXControlMode.PercentOutput, move, DemandType.AuxPID, targetGyro); + m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); + + m_driveTrain.feedWatchdog(); + } + /** * Runs a position PID while driving straight * @param targetPos The position to drive to in units