From 9014d00798ff509333088ac9a2b19e5516b0f8a4 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Thu, 13 Feb 2020 17:08:41 -0700 Subject: [PATCH 1/3] setup code to fix broken dead assist --- src/main/java/frc4388/robot/RobotContainer.java | 3 ++- .../commands/DriveWithJoystickUsingDeadAssistPID.java | 11 +++++++++++ 2 files changed, 13 insertions(+), 1 deletion(-) 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) { From 328380b9e49577066f6b982d0fd8fdd9c01d60d1 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Thu, 13 Feb 2020 17:11:54 -0700 Subject: [PATCH 2/3] invert move input --- src/main/java/frc4388/robot/commands/DriveWithJoystick.java | 2 +- .../robot/commands/DriveWithJoystickUsingDeadAssistPID.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index e663fdb..5387e8d 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -38,7 +38,7 @@ public class DriveWithJoystick extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double moveInput = m_controller.getLeftYAxis(); + double moveInput = -m_controller.getLeftYAxis(); double steerInput = m_controller.getRightXAxis(); double moveOutput = 0; double steerOutput = 0; diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index 352030b..5cfb24d 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -46,7 +46,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { @Override public void execute() { double currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); - double moveInput = m_controller.getLeftYAxis(); + double moveInput = -m_controller.getLeftYAxis(); double steerInput = m_controller.getRightXAxis(); double moveOutput = 0; double steerOutput = 0; From 1aa9037285a8f60a8eeddedfd76fa4bbc5c54c52 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 17 Feb 2020 13:10:10 -0700 Subject: [PATCH 3/3] Cleanup DeadAssist --- .../DriveWithJoystickUsingDeadAssistPID.java | 48 +++++++++---------- 1 file changed, 22 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index 5cfb24d..be92dc7 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -15,10 +15,9 @@ import frc4388.utility.controller.IHandController; public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { Drive m_drive; - double m_targetGyro; - long lastTime; + double m_targetGyro, m_currentGyro; + long m_lastTime, m_deltaTime; IHandController m_controller; - boolean isAuxPIDEnabled = false; /** * Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller. @@ -39,32 +38,19 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - lastTime = System.currentTimeMillis(); + m_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); + m_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 (prevents buildup of gyro error) */ - else { - m_targetGyro = currentGyro; - } + m_deltaTime = System.currentTimeMillis() - m_lastTime; + m_lastTime = System.currentTimeMillis(); /* If move stick is being used */ if (moveInput != 0) { @@ -85,31 +71,41 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { } else { steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone); } + resetGyroTarget(); m_drive.driveWithInput(moveOutput, steerOutput); System.out.println("Driving With Input"); - isAuxPIDEnabled = false; } /* If only the move stick is being used */ else { + updateGyroTarget(steerInput); 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 { + updateGyroTarget(steerInput); m_drive.runDriveStraightVelocityPID(0, m_targetGyro); System.out.println("Driving with Velocity PID with Target: " + m_targetGyro); - isAuxPIDEnabled = true; } } - private void updateGyroTarget() { - + /** + * If AuxPID is enabled, then update using the steer input + */ + private void updateGyroTarget(double steerInput) { + m_targetGyro += 2 * steerInput * m_deltaTime; + + m_targetGyro = MathUtil.clamp( m_targetGyro, + m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4), + m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4)); } + /** + * set target angle to current angle (prevents buildup of gyro error). + */ private void resetGyroTarget() { - + m_targetGyro = m_currentGyro; } // Called once the command ends or is interrupted.