diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index 734b9c2..a284d8a 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -31,17 +31,13 @@ public class AimToCenter extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - m_targetAngle = m_turret.getBoomBoomAngleDegrees() + m_drive.gyro.getAngle() + Math.atan(y/x); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - aimTurret(m_targetAngle); - } - - public void aimTurret(double targetAngle) { //Split into configure and run - m_turret.runshooterRotatePID(targetAngle); + m_targetAngle = m_turret.getBoomBoomAngleDegrees() + m_drive.gyro.getAngle() + Math.atan(y/x); + m_turret.runshooterRotatePID(m_targetAngle); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 1abcfa1..5f785c5 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -9,6 +9,7 @@ import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANEncoder; import com.revrobotics.CANSparkMax; +import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.SparkMaxPIDController; @@ -95,7 +96,6 @@ public class Turret extends SubsystemBase { m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; - m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition); }