diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 096f898..3da8fc8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -138,12 +138,16 @@ public final class Constants { public static final int TURRET_LEFT_SOFT_LIMIT = -55; public static final double TURRET_SPEED_MULTIPLIER = 0.3; public static final double TURRET_CALIBRATE_SPEED = 0.075; + public static final double TURRET_MOTOR_ROTS_PER_ROT = 1; //TODO: Find + public static final double TURRET_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find public static final int HOOD_UP_SOFT_LIMIT = 33; public static final int HOOD_DOWN_SOFT_LIMIT = 3; public static final double HOOD_CONVERT_SLOPE = 0.47; public static final double HOOD_CONVERT_B = 40.5; public static final double HOOD_CALIBRATE_SPEED = 0.2; + public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find + public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find public static final double DRUM_RAMP_LIMIT = 1000; public static final double DRUM_VELOCITY_BOUND = 300; diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index 3709d90..ae3f5c5 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -69,40 +69,6 @@ public class ShooterAim extends SubsystemBase { SmartDashboard.putData("Turret Angle", m_turretGyro); } - public GyroBase getGyroInterface() { - return new GyroBase(){ - - @Override - public void close() throws Exception { - // TODO Auto-generated method stub - - } - - @Override - public void reset() { - // TODO Auto-generated method stub - - } - - @Override - public double getRate() { - // TODO Auto-generated method stub - return 0; - } - - @Override - public double getAngle() { - // TODO Auto-generated method stub - return getShooterRotatePosition(); - } - - @Override - public void calibrate() { - // TODO Auto-generated method stub - - } - }; - } /** * Passes subsystem needed. * @param subsystem Subsystem needed. @@ -140,6 +106,46 @@ public class ShooterAim extends SubsystemBase { public double getShooterRotatePosition() { - return m_shooterRotateMotor.getEncoder().getPosition(); + return m_shooterRotateEncoder.getPosition(); + } + + public double getShooterRotatePositionDegrees() + { + return (m_shooterRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; + } + + public GyroBase getGyroInterface() { + return new GyroBase(){ + + @Override + public void close() throws Exception { + // TODO Auto-generated method stub + + } + + @Override + public void reset() { + // TODO Auto-generated method stub + + } + + @Override + public double getRate() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public double getAngle() { + // TODO Auto-generated method stub + return getShooterRotatePositionDegrees(); + } + + @Override + public void calibrate() { + // TODO Auto-generated method stub + + } + }; } } diff --git a/src/main/java/frc4388/robot/subsystems/ShooterHood.java b/src/main/java/frc4388/robot/subsystems/ShooterHood.java index 1cb3b9b..5cd3ac3 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterHood.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterHood.java @@ -59,7 +59,7 @@ public class ShooterHood extends SubsystemBase { // This method will be called once per scheduler run SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle); - SmartDashboard.putNumber("Hood Angle Raw", getAnglePosition()); + SmartDashboard.putNumber("Hood Angle Raw", getAnglePositionDegrees()); } /** @@ -95,4 +95,8 @@ public class ShooterHood extends SubsystemBase { public double getAnglePosition(){ return m_angleEncoder.getPosition(); } + + public double getAnglePositionDegrees(){ + return (m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT; + } }