diff --git a/simgui.json b/simgui.json index d2b6273..2d09ed5 100644 --- a/simgui.json +++ b/simgui.json @@ -28,9 +28,6 @@ "NetworkTables": { "transitory": { "SmartDashboard": { - "Shooter": { - "open": true - }, "open": true } } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index b721cde..ba8063f 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/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 diff --git a/src/main/java/com/stuypulse/robot/commands/TurretPoint.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretPoint.java similarity index 56% rename from src/main/java/com/stuypulse/robot/commands/TurretPoint.java rename to src/main/java/com/stuypulse/robot/commands/turret/TurretPoint.java index c818cc8..090e33a 100644 --- a/src/main/java/com/stuypulse/robot/commands/TurretPoint.java +++ b/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,12 +17,17 @@ 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); } @@ -27,17 +35,11 @@ public TurretPoint(Translation2d target) { 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 diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 2c76186..b69b9d5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/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; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 28b7dbc..ffa9f27 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/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); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index eb6b2a0..2c85375 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/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()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java index c5aaeaa..e467eba 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java +++ b/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( - LinearSystemId.identifyVelocitySystem(Settings.Shooter.Feedforward.kV, Settings.Shooter.Feedforward.kA) + LinearSystemId.identifyVelocitySystem(Feedforward.kV, Feedforward.kA) ); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index ba1c9be..c886378 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/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), diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java index 8626f81..71edd14 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java +++ b/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,34 +29,39 @@ 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( @@ -62,19 +69,15 @@ public final void periodic() { 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(); - } diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java index 20afafe..be4b81b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/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); diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java index 407a8cc..24f801d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java +++ b/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 turretSim; + private final LinearSystemSim turretSim; private final FieldObject2d turretPose2d; public TurretSim() { turretSim = new LinearSystemSim( - 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())