diff --git a/src/main/java/frc/robot/commands/AimCommand.java b/src/main/java/frc/robot/commands/AimCommand.java index 1a01d94..5f9bdd3 100644 --- a/src/main/java/frc/robot/commands/AimCommand.java +++ b/src/main/java/frc/robot/commands/AimCommand.java @@ -62,7 +62,9 @@ public void execute() { Units.degreesToRadians(CamResult.getBestTarget().getPitch())) - GOAL_RANGE_METERS; // turn and move towards target. - m_driveSubsystem.driveAndTurn(m_driveSubsystem.getYaw(), angleGoal, distanceFromTarget); + //m_driveSubsystem.driveAndTurn(m_driveSubsystem.getYaw(), angleGoal, distanceFromTarget); + m_driveSubsystem.turnToAngle(m_driveSubsystem.getYaw(), angleGoal); + m_driveSubsystem.driveToDistance(distanceFromTarget); // we reset the angle everytime as the target could change / move. m_driveSubsystem.turnSetGoal(angleGoal); m_driveSubsystem.distanceSetGoal(distanceFromTarget); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 5d9d4f2..822196a 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -218,6 +218,21 @@ public void driveToDistance(double targetDistance) { } } + public void turnToAngle(double gyroYawAngle, double TargetAngleDegrees) { + /* + * When this function is activated, execute another command to rotate to target angle. Since a Tank drive + * system cannot move forward simultaneously while rotating, all joystick input + * is ignored until this button is released. + */ + this.calcuateAngleRate(gyroYawAngle, TargetAngleDegrees); + double leftStickValue = turnRotateToAngleRate; + double rightStickValue = -turnRotateToAngleRate; + if (!m_turnController.atGoal()) { + this.tankDrive(leftStickValue, rightStickValue); + } + } + + public void driveAndTurn(double gyroYawAngle, double TargetAngleDegrees, double targetDistance) { /* This lets you set a gyro angle and a distance you need to travel. @@ -251,19 +266,6 @@ private void calcuateAngleRate(double gyroYawAngle, double targetAngleDegrees) { turnRotateToAngleRate = MathUtil.clamp(m_turnController.calculate(gyroYawAngle), -1.0, 1.0); } - public void turnToAngle(double gyroYawAngle, double TargetAngleDegrees) { - /* - * When this function is activated, execute another command to rotate to target angle. Since a Tank drive - * system cannot move forward simultaneously while rotating, all joystick input - * is ignored until this button is released. - */ - this.calcuateAngleRate(gyroYawAngle, TargetAngleDegrees); - double leftStickValue = turnRotateToAngleRate; - double rightStickValue = -turnRotateToAngleRate; - if (!m_turnController.atGoal()) { - this.tankDrive(leftStickValue, rightStickValue); - } - } // magnitude = (joystickL + joystickR) / 2; public void driveStraight(