diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a431921..22455f3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -18,6 +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.DriveWithJoystickUsingDeadAssistPID; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; @@ -64,7 +65,7 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller - m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, getDriverController())); + m_robotDrive.setDefaultCommand(new DriveWithJoystick(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 0a7938e..352030b 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -86,21 +86,32 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone); } m_drive.driveWithInput(moveOutput, steerOutput); + System.out.println("Driving With Input"); isAuxPIDEnabled = false; } /* If only the move stick is being used */ else { m_drive.driveWithInputAux(moveOutput, m_targetGyro); + System.out.println("Driving with Input Aux with Target: " + m_targetGyro); isAuxPIDEnabled = true; } } /* If the move stick is not being used */ else { m_drive.runDriveStraightVelocityPID(0, m_targetGyro); + System.out.println("Driving with Velocity PID with Target: " + m_targetGyro); isAuxPIDEnabled = true; } } + private void updateGyroTarget() { + + } + + private void resetGyroTarget() { + + } + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) {