Turret

src/main/java/frc/robot/Constants.java

@@ -16,4 +16,14 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
+
+    public static class Turret{
+        public static double ERROR_RANGE = 2;
+        public static double MAX_ANGLE = 0;
+        public static double MIN_ANGLE = 0;
+        public static double TICKS_PER_DEGREE = 4096 / 360;
+        public static double kP = 0;
+        public static double kI = 0;
+        public static double kD = 0;
+    }
 }

src/main/java/frc/robot/Ports.java

@@ -0,0 +1,8 @@
+package frc.robot;
+
+public final class Ports {
+
+    public static class Turret{
+        public static int motorPort = 0;
+    }
+}

src/main/java/frc/robot/RobotContainer.java

@@ -9,45 +9,56 @@
 
 import com.revrobotics.ColorSensorV3;
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.subsystems.ExampleSubsystem.ExampleSubsystem;
+import frc.robot.subsystems.turret.*;
+import frc.robot.subsystems.turret.commands.Gunner;
+
 /**
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
  * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
  * (including subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-  // The robot's subsystems and commands are defined here...
-
-
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
-  public RobotContainer() {
-    // Configure the button bindings
-    configureButtonBindings();
-  }
-
-  /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
-   */
-  private void configureButtonBindings() {
-  }
-
-
-  /**
-   * Use this to pass the autonomous command to the main {@link Robot} class.
-   *
-   * @return the command to run in autonomous
-   */
-  public Command getAutonomousCommand() {
-
-    // An ExampleCommand will run in autonomous
-    return null;
-  }
+    private final XboxController xbox = new XboxController(1);
+    Trigger LT = new Trigger(() -> xbox.getRawAxis(XboxController.Axis.kLeftTrigger.value) > 0.3);
+    private final Turret gunnerMan = new Turret();
+    // The robot's subsystems and commands are defined here...
+
+
+    /**
+     * The container for the robot.  Contains subsystems, OI devices, and commands.
+     */
+    public RobotContainer() {
+        gunnerMan.setDefaultCommand(new Gunner(gunnerMan));
+        // Configure the button bindings
+        configureButtonBindings();
+    }
+
+    /**
+     * Use this method to define your button->command mappings.  Buttons can be created by
+     * instantiating a {@link GenericHID} or one of its subclasses ({@link
+     * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+     * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+     */
+    private void configureButtonBindings() {
+
+    }
+
+
+    /**
+     * Use this to pass the autonomous command to the main {@link Robot} class.
+     *
+     * @return the command to run in autonomous
+     */
+    public Command getAutonomousCommand() {
+
+        // An ExampleCommand will run in autonomous
+        return null;
+    }
 }

src/main/java/frc/robot/subsystems/UnitModel.java

@@ -1,5 +1,7 @@
 package frc.robot.subsystems;
 
+import static frc.robot.Constants.Turret.*;
+
 public class UnitModel {
     double ticksPerUnit;
 
@@ -22,5 +24,4 @@ public double toVelocity(double ticks100ms){
     public int toTicks100ms(double velocity){
         return (int) (velocity * ticksPerUnit / 10);
     }
-
 }

src/main/java/frc/robot/subsystems/turret/Turret.java

@@ -0,0 +1,89 @@
+package frc.robot.subsystems.turret;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.UnitModel;
+
+import static frc.robot.Constants.Turret.*;
+import static frc.robot.Ports.Turret.*;
+
+public class Turret extends SubsystemBase {
+    private double currAngle = 90; // The current angle of the turret.
+    private double targetAngle; // The angle the turret needs to get to.
+    private UnitModel unitMan = new UnitModel(TICKS_PER_DEGREE); // The unit conversion module.
+    private TalonSRX motor = new TalonSRX(motorPort); // The motor used to spin the turret.
+
+    //Constructor.
+    public Turret(){
+        motor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
+        motor.config_kP(0, kP);
+        motor.config_kI(0, kI);
+        motor.config_kD(0, kD);
+        motor.setSensorPhase(true);
+    }
+
+    /**
+     * This function sets the angle the turret needs to be at.
+     * @param targetAngle is the angle the turret needs to be at.
+     */
+    public void setTargetAngle(double targetAngle) {
+        if(targetAngle > MAX_ANGLE)
+            this.targetAngle = MAX_ANGLE;
+        else if(targetAngle < MIN_ANGLE)
+            this.targetAngle = MIN_ANGLE;
+        else
+            this.targetAngle = targetAngle;
+    }
+
+    /**
+     * Gets the current angle of the turret
+     * @return the current angle
+     */
+    public double getCurrAngle() {
+        return currAngle;
+    }
+
+    /**
+     * Gets the target angle of the turret
+     * @return the target angle
+     */
+    public double getTargetAngle() {
+        return targetAngle;
+    }
+
+    /**
+     * Sets the current angle of the turret.
+     * @param currAngle is the current angle of the turret.
+     */
+    public void setCurrAngle(double currAngle){
+        this.currAngle = currAngle;
+    }
+
+    /**
+     * Changes the position of the motor, hence changing the position of the turret.
+     */
+    public void setAngle(){
+        motor.set(ControlMode.Position, unitMan.toTicks(targetAngle - currAngle));
+    }
+
+    /**
+     * Checks whether or not the target angle and the current angle are equal in a certain error range.
+     * @return whether the angles are equal.
+     */
+    public boolean anglesEqual(){
+        if(currAngle == targetAngle + ERROR_RANGE || currAngle == targetAngle - ERROR_RANGE){
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * Sets the turret back to 0.
+     */
+    public void terminate(){
+        targetAngle = 0;
+        setAngle();
+    }
+}

src/main/java/frc/robot/subsystems/turret/commands/Gunner.java

@@ -0,0 +1,53 @@
+package frc.robot.subsystems.turret.commands;
+
+import edu.wpi.first.wpilibj.*;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.button.*;
+import frc.robot.subsystems.turret.Turret;
+
+public class Gunner extends CommandBase {
+    private double X;
+    private double Y;
+    private double targetAngle; // Angle the turret needs to be at.
+    private Turret gunnerMan; // Turret class object (in order to move the motor).
+    private final XboxController xbox = new XboxController(1);
+    Trigger LT = new Trigger(() -> xbox.getRawAxis(XboxController.Axis.kLeftTrigger.value) > 0.3);
+
+    public Gunner(Turret gunnerMan){
+        this.gunnerMan = gunnerMan;
+        addRequirements(gunnerMan);
+    }
+
+    // Executes all the commands in a loop.
+    @Override
+    public void execute() {
+        X = xbox.getX(GenericHID.Hand.kRight);
+        Y = xbox.getY(GenericHID.Hand.kRight);
+        targetAngle = Math.atan2(Y, X);
+
+
+        gunnerMan.setTargetAngle(targetAngle);
+
+        if(LT.get())
+            gunnerMan.setAngle();
+
+        if (isFinished()) {
+            gunnerMan.setCurrAngle(targetAngle);
+        }
+    }
+
+    // Resets the turret.
+    @Override
+    public void end(boolean interrupted) {
+        gunnerMan.terminate();
+    }
+
+    // Continues running the program until interrupted.
+    @Override
+    public boolean isFinished() {
+        if (!gunnerMan.anglesEqual())
+            return false;
+        else
+            return true;
+    }
+}