Turret

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

@@ -16,4 +16,19 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
+
+    public static class Turret {
+        public static final double ERROR_RANGE = 2; // degrees
+        public static final double MAX_TICKS = 2048; // degrees
+        public static final double MIN_TICKS = 0; // degrees
+        public static final double TICKS_PER_DEGREE = 4096.0 / 360.0;
+        public static final double kP = 0.4;
+        //        public static final double kP = 3.5;
+        public static final double kI = 0;
+        //        public static final double kI = 0.01;
+        public static final double kD = 0.04;
+//        public static final double kD = 150;
+        public static final double DEADBAND = 0.1
+        ;
+    }
 }

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

@@ -0,0 +1,10 @@
+package frc.robot;
+
+public final class Ports {
+
+    public static class Turret {
+        public final static boolean INVERTED = false;
+        public final static boolean SENSOR_PHASE = true;
+        public final static int MOTOR = 22;
+    }
+}

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;
+import frc.robot.subsystems.turret.commands.Manual;
+
 /**
  * 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 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 Manual(gunnerMan));
+        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

@@ -22,5 +22,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,124 @@
+package frc.robot.subsystems.turret;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
+import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import edu.wpi.first.wpilibj.GenericHID;
+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. [degrees]
+    private double targetAngle; // The angle the turret needs to get to. [degrees]
+    private UnitModel unitMan = new UnitModel(TICKS_PER_DEGREE); // The unit conversion module.
+    private TalonSRX motor = new TalonSRX(MOTOR); // The motor used to spin the turret.
+
+    //Constructor.
+    public Turret() {
+        motor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder);
+
+        motor.config_kP(0, kP);
+        motor.config_kI(0, kI);
+        motor.config_kD(0, kD);
+
+        motor.setInverted(INVERTED);
+
+        motor.setSensorPhase(SENSOR_PHASE);
+
+        motor.configReverseLimitSwitchSource(LimitSwitchSource.Deactivated, LimitSwitchNormal.Disabled);
+        motor.configForwardLimitSwitchSource(LimitSwitchSource.Deactivated, LimitSwitchNormal.Disabled);
+    }
+
+    private double map(double value, double minimumInput, double maximumInput, double minimumOutput, double maximumOutput) {
+        return (value - minimumInput) * (maximumOutput - minimumOutput) / (maximumInput - minimumInput) + minimumOutput;
+    }
+
+    /**
+     * This function sets the angle the turret needs to be at. [degrees]
+     *
+     * @param targetAngle is the angle the turret needs to be at. [degrees]
+     */
+    public void setTargetAngle(double targetAngle) {
+//        targetAngle = targetAngle < 0 ? targetAngle + 360 : targetAngle;
+//        this.targetAngle = (targetAngle > 180 && targetAngle < 270) ? MAX_ANGLE : ((targetAngle > 270 && targetAngle < 360) ? MIN_ANGLE : targetAngle);
+        targetAngle = targetAngle < 0 ? targetAngle + 360 : targetAngle;
+        targetAngle = targetAngle > 180 ? targetAngle < 270 ? 180 : 0 : targetAngle;
+        int ticks = (int) map(targetAngle, 0, 180, MIN_TICKS, MAX_TICKS);
+        setPosition(ticks);
+    }
+
+    /**
+     * Gets the current angle of the turret. [degrees]
+     *
+     * @return the current angle. [degrees]
+     */
+    public double getCurrAngle() {
+        return currAngle;
+    }
+
+    /**
+     * Gets the target angle of the turret. [degrees]
+     *
+     * @return the target angle. [degrees]
+     */
+    public double getTargetAngle() {
+        return targetAngle;
+    }
+
+    /**
+     * Sets the current angle of the turret. [degrees]
+     *
+     * @param currAngle is the current angle of the turret. [degrees]
+     */
+    public void setCurrAngle(double currAngle) {
+        this.currAngle = currAngle;
+    }
+
+    /**
+     * Changes the position of the motor, hence changing the position of the turret.
+     */
+    public void setPosition() {
+        motor.set(ControlMode.Position, unitMan.toTicks(targetAngle - currAngle));
+    }
+
+    public void setPosition(int position) {
+        motor.set(ControlMode.Position, position);
+    }
+
+    public void setPower(double power) {
+        motor.set(ControlMode.PercentOutput, power);
+    }
+
+    public void setVelocity(double velocity) {
+        motor.set(ControlMode.Velocity, velocity);
+    }
+
+    public int getTicks() {
+        return motor.getSelectedSensorPosition();
+    }
+
+    /**
+     * 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;
+        setPosition();
+    }
+}

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

@@ -0,0 +1,49 @@
+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;
+
+import static frc.robot.Constants.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);
+        if(Math.abs(X) < DEADBAND)
+            X = 0;
+        if(Math.abs(Y) < DEADBAND)
+            Y = 0;
+        targetAngle = Math.toDegrees(Math.atan2(Y, X));
+        gunnerMan.setTargetAngle(targetAngle);
+
+//        if(LT.get())
+    }
+
+    // Resets the turret.
+    @Override
+    public void end(boolean interrupted) {
+        gunnerMan.terminate();
+    }
+
+    // Continues running the program until interrupted.
+    @Override
+    public boolean isFinished() {
+        return !gunnerMan.anglesEqual();
+    }
+}

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

@@ -0,0 +1,48 @@
+package frc.robot.subsystems.turret.commands;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Ports;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.UnitModel;
+import frc.robot.subsystems.turret.Turret;
+
+public class Manual extends CommandBase {
+    private final Turret turret;
+    private final XboxController xbox = new XboxController(1);
+
+
+    public Manual(Turret turret) {
+        this.turret = turret;
+        addRequirements(turret);
+    }
+
+    @Override
+    public void initialize() {
+
+    }
+
+    @Override
+    public void execute() {
+        double x = xbox.getX(GenericHID.Hand.kRight);
+        if (Math.abs(x) < 0.01) x = 0;
+        x /= 4;
+        System.out.println(turret.getTicks());
+        System.out.println(x);
+//        turret.setPower(x);
+//        turret.setVelocity(x * 300);
+//        turret.setPosition(1600);
+        turret.setPosition(0);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        turret.setPower(0);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

src/test/java/TestClass.java

@@ -0,0 +1,17 @@
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import org.junit.Test;
+import frc.robot.subsystems.turret.*;
+
+
+public class TestClass {
+    @Test
+    public void testFunction2() {
+
+//        double targetAngle = 195;
+//        targetAngle = targetAngle < 0 ? targetAngle + 360 : targetAngle;
+//        targetAngle = (targetAngle > 180 && targetAngle < 270) ? MAX_ANGLE : ((targetAngle > 270 && targetAngle < 360) ? MIN_ANGLE : targetAngle);
+//        System.out.println("The corrected angle is: " + targetAngle);
+    }
+}