Tbr

simgui.json

@@ -28,9 +28,6 @@
   "NetworkTables": {
     "transitory": {
       "SmartDashboard": {
-        "Shooter": {
-          "open": true
-        },
         "open": true
       }
     }

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -5,29 +5,23 @@
 
 package com.stuypulse.robot;
 
-import com.stuypulse.robot.commands.TurretPoint;
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.commands.odometry.OdometryRealign;
 import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
+import com.stuypulse.robot.commands.turret.TurretPoint;
 import com.stuypulse.robot.constants.Ports;
-import com.stuypulse.robot.constants.Motors.Swerve;
 import com.stuypulse.robot.subsystems.shooter.Shooter;
 import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 import com.stuypulse.robot.subsystems.swerve.evenMoreSwerve.*;
 import com.stuypulse.robot.subsystems.turret.Turret;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
-import com.stuypulse.stuylib.network.SmartBoolean;
 
-import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 public class RobotContainer {
@@ -57,7 +51,7 @@ public RobotContainer() {
     /****************/
 
     private void configureDefaultCommands() {
-        turret.setDefaultCommand(new TurretPoint (new Translation2d()));
+        turret.setDefaultCommand(new TurretPoint(new Translation2d(5, 5)));
         swerve.setDefaultCommand(new SwerveDriveDrive(driver));
     }
 
@@ -73,6 +67,9 @@ private void configureDriverBindings() {
 
         // swerve
 
+        // turret
+        // driver.getLeftButton().whileTrue(new TurretPoint(new Translation2d(0, 0))); // change the button later lol
+
         // left bumper -> robot relative
 
         // odometry

src/main/java/com/stuypulse/robot/commands/TurretPoint.java → src/main/java/com/stuypulse/robot/commands/turret/TurretPoint.java

@@ -1,10 +1,13 @@
-package com.stuypulse.robot.commands;
+package com.stuypulse.robot.commands.turret;
 
+import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.subsystems.odometry.Odometry;
 import com.stuypulse.robot.subsystems.turret.Turret;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
 public class TurretPoint extends CommandBase {
@@ -14,30 +17,29 @@ public class TurretPoint extends CommandBase {
 
     private Translation2d target;
 
+    private FieldObject2d targetPose;
+
     public TurretPoint(Translation2d target) {
         this.target = target;
 
         turret = Turret.getInstance();
         odometry = Odometry.getInstance();
 
+        targetPose = Odometry.getInstance().getField().getObject("Turret Target Pose");
+        targetPose.setPose(new Pose2d(target, new Rotation2d()));
+
         addRequirements(turret);
     }
 
     @Override
     public final void execute() {
         Pose2d robotPose = odometry.getPose();
 
-        if ((Math.abs(robotPose.getX()) < .001) && (Math.abs(target.getX()) < .001)) {
-            turret.setTargetAngle(0, 360, -360);
-        }
-
-        else {
-            turret.setTargetAngle(
-            (180 + Math.toDegrees(
-                Math.atan((robotPose.getY() - target.getY()) / (robotPose.getX() - target.getX()))
-            )), 180, -180
-            );
-        }
+        turret.setTargetAngle(
+            Math.toDegrees(Math.atan2(target.getY() - robotPose.getY() , target.getX() - robotPose.getX())),
+            Settings.Turret.MIN_ANGLE,
+            Settings.Turret.MAX_ANGLE
+        );
     }
 
     @Override

src/main/java/com/stuypulse/robot/constants/Ports.java

@@ -18,6 +18,10 @@ public interface Turret {
         int TURN = -1;
     }
 
+    public interface Shooter {
+        int MOTOR = -1;
+    }
+
     public interface Swerve {
         public interface FrontRight {
             int DRIVE = 10;

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -6,13 +6,10 @@
 package com.stuypulse.robot.constants;
 
 import com.pathplanner.lib.auto.PIDConstants;
-import com.stuypulse.stuylib.control.feedback.PIDController;
-// import com.stuypulse.stuylib.control.Controller;
 import com.stuypulse.stuylib.math.Vector2D;
 import com.stuypulse.stuylib.network.SmartBoolean;
 import com.stuypulse.stuylib.network.SmartNumber;
 
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
@@ -28,9 +25,18 @@ public interface Settings {
     double DT = 0.02;
 
     public interface Turret {
+        int MIN_ANGLE = -135;
+        int MAX_ANGLE = +135;
+
+        public interface Feedback {
+            SmartNumber kP = new SmartNumber("Turret/kP", 3.0);
+            SmartNumber kI = new SmartNumber("Turret/kI", 0.0);
+            SmartNumber kD = new SmartNumber("Turret/kD", 0.0);
+        }
+
         public interface Feedforward {
-            SmartNumber kV = new SmartNumber("Turret kV", 0.1);
-            SmartNumber kA = new SmartNumber("Turret kA", 0.01);
+            SmartNumber kV = new SmartNumber("Turret/kV", 0.1);
+            SmartNumber kA = new SmartNumber("Turret/kA", 0.01);
         }
     }
 

src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java

@@ -4,6 +4,7 @@
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
+import com.stuypulse.robot.constants.Ports;
 
 /**
  * ShooterImpl class contains the hardware logic for the Shooter class.
@@ -14,13 +15,14 @@
  * @author Richie Xue 
  */
 public class ShooterImpl extends Shooter {
+
     private final CANSparkMax motor;
     private final RelativeEncoder encoder;
 
     public ShooterImpl() {
         super();
 
-        this.motor = new CANSparkMax(1, CANSparkMax.MotorType.kBrushless);
+        this.motor = new CANSparkMax(Ports.Shooter.MOTOR, CANSparkMax.MotorType.kBrushless);
         this.encoder = motor.getEncoder();
     }
 
@@ -35,7 +37,7 @@ public void setVoltage(double voltage) {
 
     @Override
     public void periodicallyCalled() {
-        SmartDashboard.putNumber("Shooter/Velocity", getVelocity());
+        SmartDashboard.putNumber("Shooter/RPM", getVelocity());
         SmartDashboard.putNumber("Shooter/Error", getTargetRPM() - getVelocity());
     }   
 }

src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java

@@ -1,6 +1,7 @@
 package com.stuypulse.robot.subsystems.shooter;
 
 import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.constants.Settings.Shooter.Feedforward;
 
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.system.plant.LinearSystemId;
@@ -14,7 +15,7 @@ public class ShooterSim extends Shooter {
 
     public ShooterSim() {
         shooterSim = new LinearSystemSim<N1, N1, N1>(
-            LinearSystemId.identifyVelocitySystem(Settings.Shooter.Feedforward.kV, Settings.Shooter.Feedforward.kA)
+            LinearSystemId.identifyVelocitySystem(Feedforward.kV, Feedforward.kA)
         );
     }
 

src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java

@@ -42,12 +42,6 @@ public class SwerveDrive extends SubsystemBase {
 
     static {
         if (RobotBase.isReal()) {
-            // instance = new SwerveDrive(
-            //     new MAX_SwerveModule(FrontRight.ID, FrontRight.MODULE_OFFSET, Ports.Swerve.FrontRight.TURN, FrontRight.ABSOLUTE_OFFSET, Ports.Swerve.FrontRight.DRIVE),
-            //     new MAX_SwerveModule(FrontLeft.ID, FrontLeft.MODULE_OFFSET, Ports.Swerve.FrontLeft.TURN, FrontLeft.ABSOLUTE_OFFSET, Ports.Swerve.FrontLeft.DRIVE),
-            //     new MAX_SwerveModule(BackLeft.ID, BackLeft.MODULE_OFFSET, Ports.Swerve.BackLeft.TURN, BackLeft.ABSOLUTE_OFFSET, Ports.Swerve.BackLeft.DRIVE),
-            //     new MAX_SwerveModule(BackRight.ID, BackRight.MODULE_OFFSET, Ports.Swerve.BackRight.TURN, BackRight.ABSOLUTE_OFFSET, Ports.Swerve.BackRight.DRIVE)
-            // );
             instance = new SwerveDrive(
                 new SL_SwerveModule(FrontRight.ID, FrontRight.MODULE_OFFSET, Ports.Swerve.FrontRight.TURN, FrontRight.ABSOLUTE_OFFSET, Ports.Swerve.FrontRight.DRIVE),
                 new SL_SwerveModule(FrontLeft.ID, FrontLeft.MODULE_OFFSET, Ports.Swerve.FrontLeft.TURN, FrontLeft.ABSOLUTE_OFFSET, Ports.Swerve.FrontLeft.DRIVE),

src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java

@@ -3,6 +3,8 @@
 import com.stuypulse.robot.Robot;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.constants.Settings.Turret.Feedback;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 import com.stuypulse.stuylib.control.Controller;
 import com.stuypulse.stuylib.control.feedback.PIDController;
 import com.stuypulse.stuylib.network.SmartNumber;
@@ -15,7 +17,7 @@ public abstract class Turret extends SubsystemBase {
     public static final Turret instance;
 
     static {
-        if(Robot.isReal()) {
+        if (Robot.isReal()) {
             instance = new TurretImpl(Ports.Turret.TURN);
         }
         else {
@@ -27,54 +29,55 @@ public static Turret getInstance() {
         return instance;
     }
 
-    private Controller controller;
-    private SmartNumber targetAngle = new SmartNumber("Target Angle", 120);
-    private SmartNumber fakeTargetAngle = new SmartNumber("Fake Target Angle", 0);
-
-    public void stop() {
-        setTurretVoltage(0);
-    }
+    private final SmartNumber targetAngle;
+    private final Controller controller;
 
     public Turret() {
-        controller = new PIDController(3, 0, 0); 
+        controller = new PIDController(Feedback.kP, Feedback.kI, Feedback.kD); 
+
+        targetAngle = new SmartNumber("Turret/Target Angle", 0);
     }
 
     public abstract double getTurretAngle();
     public abstract void setTurretVoltage(double voltage);
 
-    public void setTargetAngle(double angle, double max, double min) {
-        // closest to current angle as possible
-        double currentAngle = getTurretAngle() + (angle % 360);
-
-        if (currentAngle > max) {
-            targetAngle.set(currentAngle - (360 * (Math.ceil(angle / 360.0))));
-        } else if (currentAngle < min) {
-            targetAngle.set(currentAngle + (360 * (Math.ceil(angle / 360))));
-        } else {
-            targetAngle.set(angle % 360);
-        }
+    public void stop() {
+        setTurretVoltage(0);
     }
 
+    public void setTargetAngle(double angle, double minTarget, double maxTarget) {
+        // keep in mind that this assumes that the minimum angle is 360, when it very
+        // much could be less, in which case I'll change it in like 5 minutes lol
+
+        // // number of rotations needed to bring angle back in (min, max) range
+        double deltaRotations = 0;
+
+        if (angle > maxTarget) {
+            deltaRotations = -Math.ceil((angle - maxTarget) / 180.0); // minimum number of turns possible to reach it
+        }
+        else if (angle < minTarget) {
+            deltaRotations = +Math.ceil(-(angle - minTarget) / 180.0);
+        }
+
+        targetAngle.set(angle + deltaRotations * 360); // 360 makes sure that right side of robot is tracking
+    }
+
     @Override
     public final void periodic() {
         controller.update(
             targetAngle.get(), 
             getTurretAngle()
         );
 
-        setTargetAngle(fakeTargetAngle.get(), 300, -300);
-
         double output = controller.getOutput();
         setTurretVoltage(output);
-
-        SmartDashboard.putNumber("Calculated Voltage", 0);
-        SmartDashboard.putNumber("Turret Angle", 0);
-        SmartDashboard.putNumber("Target Angle", targetAngle.get());
+
+        SmartDashboard.putNumber("Turret/Calculated Voltage (V)", output);
+        SmartDashboard.putNumber("Turret/Angle (deg)", getTurretAngle());
 
         periodic2();
     }
 
     public abstract void periodic2();
-
 }
 

src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java

@@ -7,8 +7,8 @@
 
 public class TurretImpl extends Turret {
 
-    private CANSparkMax motor;
-    private SparkMaxAbsoluteEncoder encoder;
+    private final CANSparkMax motor;
+    private final SparkMaxAbsoluteEncoder encoder;
 
     public TurretImpl(int port) {
         motor = new CANSparkMax(port, MotorType.kBrushless);

src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java

@@ -1,6 +1,7 @@
 package com.stuypulse.robot.subsystems.turret;
 
 import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.constants.Settings.Turret.Feedforward;
 import com.stuypulse.robot.subsystems.odometry.Odometry;
 
 import edu.wpi.first.math.MathUtil;
@@ -15,18 +16,16 @@
 
 public class TurretSim extends Turret {
 
-    private LinearSystemSim<N2, N1, N1> turretSim;
+    private final LinearSystemSim<N2, N1, N1> turretSim;
     private final FieldObject2d turretPose2d;
 
     public TurretSim() {
         turretSim = new LinearSystemSim<N2, N1, N1>(
-            LinearSystemId.identifyPositionSystem(Settings.Turret.Feedforward.kV.get(), Settings.Turret.Feedforward.kA.get())
+            LinearSystemId.identifyPositionSystem(Feedforward.kV.get(), Feedforward.kA.get())
         );
 
         turretPose2d = Odometry.getInstance().getField().getObject("Turret Pose 2d");
 
-        // turretPose2d.setPose(new Pose2d(4, 4, Rotation2d.fromDegrees(0)));
-        // turretPose2d.setPose(Odometry.getInstance().getPose());
         turretPose2d.setPose(new Pose2d(
             Odometry.getInstance().getTranslation(),
             Rotation2d.fromDegrees(getTurretAngle())