From a09d6fadfec3d909e255d9eb9dddf617dd375adc Mon Sep 17 00:00:00 2001 From: Jonathan Date: Thu, 8 Jun 2023 16:38:06 -0600 Subject: [PATCH 1/7] working to get rid of sonarlint code smells --- src/main/java/team3176/robot/Constants.java | 10 +-- src/main/java/team3176/robot/Robot.java | 89 +++++++++++-------- .../java/team3176/robot/RobotContainer.java | 26 ++---- .../subsystems/drivetrain/SwervePod.java | 2 - .../subsystems/drivetrain/SwervePodIOSim.java | 32 ++----- .../robot/subsystems/superstructure/Arm.java | 59 ++++-------- .../subsystems/superstructure/ArmIO.java | 6 -- .../subsystems/superstructure/ArmIOSim.java | 18 +--- .../subsystems/superstructure/ArmIOSpark.java | 13 +-- .../robot/subsystems/superstructure/Claw.java | 14 ++- .../subsystems/superstructure/IntakeCone.java | 21 +---- .../subsystems/superstructure/IntakeCube.java | 6 -- .../superstructure/Superstructure.java | 78 ++++++++-------- .../robot/subsystems/vision/VisionDual.java | 4 +- 14 files changed, 147 insertions(+), 231 deletions(-) diff --git a/src/main/java/team3176/robot/Constants.java b/src/main/java/team3176/robot/Constants.java index 7420365..a1be3e2 100644 --- a/src/main/java/team3176/robot/Constants.java +++ b/src/main/java/team3176/robot/Constants.java @@ -12,13 +12,13 @@ public final class Constants { private static final RobotType robot = RobotType.ROBOT_SIMBOT; - public static final double loopPeriodSecs = 0.02; - public static final boolean tuningMode = false; + public static final double LOOP_PERIODIC_SECS = 0.02; + public static final boolean TUNING_MODE = false; public static boolean invalidRobotAlertSent = false; public static RobotType getRobot() { - if (!disableHAL && RobotBase.isReal()) { + if (!isHALdisable && RobotBase.isReal()) { if (robot == RobotType.ROBOT_SIMBOT) { // Invalid robot selected if (!invalidRobotAlertSent) { invalidRobotAlertSent = true; @@ -62,10 +62,10 @@ public static enum Mode { } // Function to disable HAL interaction when running without native libs - public static boolean disableHAL = false; + public static boolean isHALdisable = false; public static void disableHAL() { - disableHAL = true; + isHALdisable = true; } /** Checks whether the robot the correct robot is selected when deploying. */ diff --git a/src/main/java/team3176/robot/Robot.java b/src/main/java/team3176/robot/Robot.java index 971a4a3..f5e6093 100644 --- a/src/main/java/team3176/robot/Robot.java +++ b/src/main/java/team3176/robot/Robot.java @@ -17,10 +17,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import team3176.robot.Constants.RobotType; -import team3176.robot.subsystems.superstructure.Arm; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.UsbCamera; -import edu.wpi.first.cscore.CvSource; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -29,11 +27,11 @@ * project. */ public class Robot extends LoggedRobot{ - private Command m_autonomousCommand; - - private RobotContainer m_robotContainer; - Thread m_fisheyeThread; + private Command autonomousCommand; + private RobotContainer robotContainer; + Thread fisheyeThread; + private static final boolean FISHEYE_CAMERA = false; /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -43,23 +41,24 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. Logger logger = Logger.getInstance(); - logger.recordMetadata("Robot", Constants.getRobot().toString()); System.out.println("[Init] Starting AdvantageKit"); + logger.recordMetadata("Robot", Constants.getRobot().toString()); logger.recordMetadata("RuntimeType", getRuntimeType().toString()); logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + final String GitDirty = "GitDirty"; switch (BuildConstants.DIRTY) { case 0: - logger.recordMetadata("GitDirty", "All changes committed"); + logger.recordMetadata(GitDirty, "All changes committed"); break; case 1: - logger.recordMetadata("GitDirty", "Uncomitted changes"); + logger.recordMetadata(GitDirty, "Uncomitted changes"); break; default: - logger.recordMetadata("GitDirty", "Unknown"); + logger.recordMetadata(GitDirty, "Unknown"); break; } switch (Constants.getMode()) { @@ -90,15 +89,21 @@ public void robotInit() { logger.start(); - m_robotContainer = new RobotContainer(); + robotContainer = new RobotContainer(); SmartDashboard.putData(CommandScheduler.getInstance()); - // m_fisheyeThread = new Thread( () -> { - // UsbCamera fisheye = CameraServer.startAutomaticCapture(); - // fisheye.setResolution(640,480); - // CvSource outputStream = CameraServer.putVideo("fisheye", 640, 480); - // }); - // m_fisheyeThread.setDaemon(true); - // m_fisheyeThread.start(); + + if(FISHEYE_CAMERA) + { + fisheyeThread = new Thread( () -> { + UsbCamera fisheye = CameraServer.startAutomaticCapture(); + fisheye.setResolution(640,480); + //not using this at the return so commenting for linting: CvSource outputStream = + CameraServer.putVideo("fisheye", 640, 480); + }); + fisheyeThread.setDaemon(true); + fisheyeThread.start(); + } + } @@ -121,29 +126,33 @@ public void robotPeriodic() { /** This function is called once each time the robot enters Disabled mode. */ @Override public void disabledInit() { - m_robotContainer.setArmCoast(); + robotContainer.setArmCoast(); } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + //nan + } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - m_robotContainer.clearCanFaults(); - m_robotContainer.setArmBrake(); - m_robotContainer.setThrustBrake(); - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + robotContainer.clearCanFaults(); + robotContainer.setArmBrake(); + robotContainer.setThrustBrake(); + autonomousCommand = robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); + if (autonomousCommand != null) { + autonomousCommand.schedule(); } } /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + //scheduled command executes from init() + } @Override public void teleopInit() { @@ -151,17 +160,19 @@ public void teleopInit() { // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - m_robotContainer.clearCanFaults(); - m_robotContainer.setArmBrake(); - m_robotContainer.setThrustCoast(); - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); + robotContainer.clearCanFaults(); + robotContainer.setArmBrake(); + robotContainer.setThrustCoast(); + if (autonomousCommand != null) { + autonomousCommand.cancel(); } } /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + //command scheduler is responsible for actions + } @Override public void testInit() { @@ -171,13 +182,19 @@ public void testInit() { /** This function is called periodically during test mode. */ @Override - public void testPeriodic() {} + public void testPeriodic() { + //nan + } /** This function is called once when the robot is first started up. */ @Override - public void simulationInit() {} + public void simulationInit() { + //nan + } /** This function is called periodically whilst in simulation. */ @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + //nan + } } diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index 2af390a..b50ec68 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -6,34 +6,21 @@ import java.io.File; -import edu.wpi.first.hal.PowerDistributionStickyFaults; + import edu.wpi.first.wpilibj.Filesystem; 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.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -import edu.wpi.first.hal.PowerDistributionStickyFaults; -import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; import team3176.robot.commands.*; -import team3176.robot.commands.autons.*; import team3176.robot.commands.drivetrain.*; -import team3176.robot.commands.superstructure.*; -import team3176.robot.commands.superstructure.arm.*; -import team3176.robot.commands.superstructure.claw.*; import team3176.robot.commands.superstructure.claw.ClawIdle; import team3176.robot.commands.superstructure.intakecube.*; -import team3176.robot.commands.vision.*; import team3176.robot.constants.Hardwaremap; -import team3176.robot.constants.SuperStructureConstants; import team3176.robot.subsystems.controller.Controller; import team3176.robot.subsystems.drivetrain.Drivetrain; -import team3176.robot.subsystems.drivetrain.Drivetrain.coordType; -import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; -import team3176.robot.subsystems.RobotState; import team3176.robot.subsystems.superstructure.Arm; import team3176.robot.subsystems.superstructure.Claw; import team3176.robot.subsystems.superstructure.IntakeCube; @@ -41,7 +28,6 @@ import team3176.robot.subsystems.superstructure.Superstructure; import team3176.robot.subsystems.vision.VisionDual; -import team3176.robot.subsystems.vision.VisionDualIOLime; /** * This class is where the bulk of the robot should be declared. Since @@ -62,8 +48,8 @@ public class RobotContainer { private final IntakeCone m_IntakeCone; private PowerDistribution m_PDH; - - // private final Compressor m_Compressor; + + // is this why we don't have a compressor? private final Compressor m_Compressor private final Drivetrain m_Drivetrain; private final VisionDual m_Vision; private final Superstructure m_Superstructure; @@ -85,9 +71,9 @@ public RobotContainer() { m_Vision = VisionDual.getInstance(); m_Superstructure = Superstructure.getInstance(); m_Drivetrain.setDefaultCommand(new SwerveDrive( - () -> m_Controller.getForward(), - () -> m_Controller.getStrafe(), - () -> m_Controller.getSpin())); + m_Controller::getForward, + m_Controller::getStrafe, + m_Controller::getSpin)); m_Arm.setDefaultCommand(m_Arm.armFineTune( () -> m_Controller.operator.getLeftY())); m_autonChooser = new SendableChooser<>(); File paths = new File(Filesystem.getDeployDirectory(), "pathplanner"); diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java index 6a5e3c4..7d16ae1 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java @@ -6,11 +6,9 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java index 916018d..a6ee21b 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java @@ -1,12 +1,5 @@ package team3176.robot.subsystems.drivetrain; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonFX; -import com.ctre.phoenix.sensors.AbsoluteSensorRange; -import com.ctre.phoenix.sensors.CANCoder; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; @@ -14,28 +7,22 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import team3176.robot.Constants; -import team3176.robot.Robot; -import team3176.robot.constants.DrivetrainConstants; import team3176.robot.constants.DrivetrainConstants; -import team3176.robot.constants.SwervePodHardwareID; public class SwervePodIOSim implements SwervePodIO{ private FlywheelSim driveSim = new FlywheelSim(DCMotor.getFalcon500(1), 4.714, 0.025); private FlywheelSim turnSim = new FlywheelSim(DCMotor.getNeo550(1), 70.0, 0.0005); - private PIDController drivePID = new PIDController(.03, 0, 0.0,.045); + //private PIDController drivePID = new PIDController(.03, 0, 0.0,.045); private double turnRelativePositionRad = 0.0; private double turnAbsolutePositionRad = Math.random() * 2.0 * Math.PI; private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; private double currentDriveSpeed = 0.0; - public SwervePodIOSim() { - - - } + @Override public void updateInputs(SwervePodIOInputs inputs) { - driveSim.update(Constants.loopPeriodSecs); - turnSim.update(Constants.loopPeriodSecs); - double angleDiffRad = Units.radiansToDegrees(turnSim.getAngularVelocityRadPerSec() * Constants.loopPeriodSecs); + driveSim.update(Constants.LOOP_PERIODIC_SECS); + turnSim.update(Constants.LOOP_PERIODIC_SECS); + double angleDiffRad = Units.radiansToDegrees(turnSim.getAngularVelocityRadPerSec() * Constants.LOOP_PERIODIC_SECS); turnRelativePositionRad += angleDiffRad; turnAbsolutePositionRad += angleDiffRad; while (turnAbsolutePositionRad < -180) { @@ -47,7 +34,7 @@ public void updateInputs(SwervePodIOInputs inputs) { inputs.drivePositionRad = inputs.drivePositionRad - + (driveSim.getAngularVelocityRadPerSec() * Constants.loopPeriodSecs); + + (driveSim.getAngularVelocityRadPerSec() * Constants.LOOP_PERIODIC_SECS); inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; inputs.driveCurrentAmpsStator = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; @@ -77,6 +64,7 @@ public void setDrive(double velMetersPerSecond) { } /** Run the turn motor at the specified voltage. */ + @Override public void setTurn(double volts) { if(DriverStation.isEnabled()){ turnAppliedVolts = MathUtil.clamp(volts * 12, -12.0, 12.0); @@ -86,10 +74,4 @@ public void setTurn(double volts) { } } - - /** Enable or disable brake mode on the drive motor. */ - public void setDriveBrakeMode(boolean enable) {} - - /** Enable or disable brake mode on the turn motor. */ - public void setTurnBrakeMode(boolean enable) {} } diff --git a/src/main/java/team3176/robot/subsystems/superstructure/Arm.java b/src/main/java/team3176/robot/subsystems/superstructure/Arm.java index 0fd2200..5346969 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Arm.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Arm.java @@ -1,19 +1,9 @@ package team3176.robot.subsystems.superstructure; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import com.ctre.phoenix.sensors.AbsoluteSensorRange; -import com.ctre.phoenix.sensors.CANCoder; import java.util.function.DoubleSupplier; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; @@ -23,28 +13,18 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand; import team3176.robot.constants.SuperStructureConstants; import team3176.robot.Constants; import team3176.robot.Constants.Mode; -import team3176.robot.constants.Hardwaremap; -import team3176.robot.constants.SuperStructureConstants; - -import team3176.robot.subsystems.superstructure.ArmIO; import org.littletonrobotics.junction.Logger; public class Arm extends SubsystemBase { - private static final double MAX_ENCODER_ANGLE_VALUE = SuperStructureConstants.ARM_HIGH_POS; - private static final double MIN_ENCODER_ANGLE_VALUE = SuperStructureConstants.ARM_ZERO_POS; private static Arm instance; private final ArmIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); - - private double armEncoderAbsPosition; - private double lastEncoderPos; - private final PIDController m_turningPIDController; - private int counter; - public enum States {OPEN_LOOP,CLOSED_LOOP}; + + private final PIDController turningPIDController; + public enum States {OPEN_LOOP,CLOSED_LOOP} private States currentState = States.OPEN_LOOP; private double armSetpointAngleRaw = SuperStructureConstants.ARM_ZERO_POS; private Mechanism2d mech = new Mechanism2d(10,10); @@ -54,7 +34,7 @@ public enum States {OPEN_LOOP,CLOSED_LOOP}; private MechanismLigament2d armElbow = armSholder.append(new MechanismLigament2d("armELigament",2,90)); private Arm(ArmIO io) { this.io = io; - this.m_turningPIDController = new PIDController(SuperStructureConstants.ARM_kP, SuperStructureConstants.ARM_kI, SuperStructureConstants.ARM_kD); + this.turningPIDController = new PIDController(SuperStructureConstants.ARM_kP, SuperStructureConstants.ARM_kI, SuperStructureConstants.ARM_kD); SmartDashboard.putNumber("Arm_kp", SuperStructureConstants.ARM_kP); SmartDashboard.putNumber("Arm_Kg", SuperStructureConstants.ARM_kg); setArmPidPosMode(); @@ -70,12 +50,12 @@ public void setBrakeMode() { private void setArmPidPosMode() { - this.m_turningPIDController.setTolerance(SuperStructureConstants.ARM_TOLERANCE); + this.turningPIDController.setTolerance(SuperStructureConstants.ARM_TOLERANCE); //this.m_turningPIDController.enableContinuousInput() - this.m_turningPIDController.reset(); - this.m_turningPIDController.setP(SuperStructureConstants.ARM_kP); - this.m_turningPIDController.setI(SuperStructureConstants.ARM_kI); - this.m_turningPIDController.setD(SuperStructureConstants.ARM_kD); + this.turningPIDController.reset(); + this.turningPIDController.setP(SuperStructureConstants.ARM_kP); + this.turningPIDController.setI(SuperStructureConstants.ARM_kI); + this.turningPIDController.setD(SuperStructureConstants.ARM_kD); //this.m_turningPIDController.enableContinuousInput(0, 360); } @@ -98,22 +78,21 @@ public static Arm getInstance() { private void setPIDPosition(double desiredAngle) { //need to double check these values - this.armEncoderAbsPosition = inputs.Position; - double physicsAngle = (desiredAngle - SuperStructureConstants.ARM_CARRY_POS); + //double physicsAngle = (desiredAngle - SuperStructureConstants.ARM_CARRY_POS); //kg is the scalar representing the percent power needed to hold the arm at 90 degrees away from the robot - double kg = SmartDashboard.getNumber("Arm_Kg", SuperStructureConstants.ARM_kg); + //double kg = SmartDashboard.getNumber("Arm_Kg", SuperStructureConstants.ARM_kg); // kp set as the fraction of control effort / error to cause control effort // for example .4 output is generated by a 40 degree error double kp = SmartDashboard.getNumber("Arm_kp", SuperStructureConstants.ARM_kP); - m_turningPIDController.setP(kp); + turningPIDController.setP(kp); double feedForward = 0.0;//kg * physicsAngle/SuperStructureConstants.ARM_HIGH_POS; - if (this.armEncoderAbsPosition < SuperStructureConstants.ARM_MID_POS + 10){ + if (inputs.Position < SuperStructureConstants.ARM_MID_POS + 10){ feedForward =0.0; } else if (desiredAngle < SuperStructureConstants.ARM_ZERO_POS+5) { feedForward = -.2; } - double turnOutput = m_turningPIDController.calculate(this.armEncoderAbsPosition, desiredAngle); + double turnOutput = turningPIDController.calculate(inputs.Position, desiredAngle); turnOutput = MathUtil.clamp(turnOutput,-1,1); io.set(turnOutput + feedForward); SmartDashboard.putNumber("Arm_Output", turnOutput + feedForward); @@ -142,7 +121,7 @@ public double getArmPosition() { return inputs.Position; } public boolean isArmAtPosition() { - return Math.abs(this.m_turningPIDController.getPositionError()) < 7; + return Math.abs(this.turningPIDController.getPositionError()) < 7; } /** * to be used for trajectory following without disrupting other commands @@ -164,7 +143,7 @@ public Command armSetPositionBlocking(double angleInDegrees) { this.currentState = States.CLOSED_LOOP; this.armSetpointAngleRaw = angleInDegrees;}, ()-> {}, - (b) -> {}, + b -> {}, this::isArmAtPosition, this); } @@ -177,10 +156,10 @@ public Command armFineTune(DoubleSupplier angleDeltaCommand) { return this.run(() -> fineTune(angleDeltaCommand.getAsDouble())); } public Command armAnalogUpCommand() { - return this.runEnd(() -> armAnalogUp(), () -> idle()); + return this.runEnd(this::armAnalogUp, this::idle); } public Command armAnalogDownCommand() { - return this.runEnd(() -> armAnalogDown(), () -> idle()); + return this.runEnd(this::armAnalogDown, this::idle); } @@ -196,7 +175,7 @@ public void periodic() { //SmartDashboard.putNumber("Arm_Position_Relative", armEncoder.getAbsolutePosition() - SuperStructureConstants.ARM_ZERO_POS); if(this.currentState == States.CLOSED_LOOP) { this.armSetpointAngleRaw = MathUtil.clamp(this.armSetpointAngleRaw, SuperStructureConstants.ARM_ZERO_POS, SuperStructureConstants.ARM_HIGH_POS); - Logger.getInstance().recordOutput("Arm/position_error", this.m_turningPIDController.getPositionError()); + Logger.getInstance().recordOutput("Arm/position_error", this.turningPIDController.getPositionError()); setPIDPosition(armSetpointAngleRaw); } } diff --git a/src/main/java/team3176/robot/subsystems/superstructure/ArmIO.java b/src/main/java/team3176/robot/subsystems/superstructure/ArmIO.java index e1b4d61..b6015be 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/ArmIO.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/ArmIO.java @@ -8,12 +8,6 @@ package team3176.robot.subsystems.superstructure; import org.littletonrobotics.junction.AutoLog; -import org.littletonrobotics.junction.LogTable; -import org.littletonrobotics.junction.inputs.LoggableInputs; - -import com.fasterxml.jackson.databind.ser.std.StdKeySerializers.Default; - -import edu.wpi.first.math.geometry.Rotation2d; /** Template hardware interface for a closed loop subsystem. */ public interface ArmIO{ diff --git a/src/main/java/team3176/robot/subsystems/superstructure/ArmIOSim.java b/src/main/java/team3176/robot/subsystems/superstructure/ArmIOSim.java index 59e6d7e..f83b620 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/ArmIOSim.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/ArmIOSim.java @@ -7,23 +7,13 @@ package team3176.robot.subsystems.superstructure; -import org.littletonrobotics.junction.AutoLog; -import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.inputs.LoggableInputs; -import com.ctre.phoenix.sensors.AbsoluteSensorRange; -import com.ctre.phoenix.sensors.CANCoder; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import team3176.robot.constants.SuperStructureConstants; import team3176.robot.Constants; @@ -37,8 +27,9 @@ public ArmIOSim() { armSim = new SingleJointedArmSim(DCMotor.getNEO(1), 75, 0.5, 0.7, -1.0*Math.PI, 3.14, true); } /** Updates the set of loggable inputs. */ + @Override public void updateInputs(ArmIOInputs inputs) { - armSim.update(Constants.loopPeriodSecs); + armSim.update(Constants.LOOP_PERIODIC_SECS); inputs.Position = Units.radiansToDegrees(armSim.getAngleRads()) + 90 + SuperStructureConstants.ARM_SIM_OFFSET; inputs.VelocityRadPerSec = armSim.getVelocityRadPerSec(); inputs.AppliedVolts = appliedVolts; @@ -46,6 +37,7 @@ public void updateInputs(ArmIOInputs inputs) { inputs.TempCelcius = new double[] {0.0}; Logger.getInstance().recordOutput("Arm/SimPos",armSim.getAngleRads()); } + @Override public void set(double percentOuput) { if(DriverStation.isEnabled()) { appliedVolts = percentOuput * 12; @@ -55,9 +47,5 @@ public void set(double percentOuput) { appliedVolts = MathUtil.clamp(appliedVolts,-12,12); armSim.setInputVoltage(appliedVolts); } - public void setCoastMode(boolean isCoastMode) { - - } - public void reset() {} } diff --git a/src/main/java/team3176/robot/subsystems/superstructure/ArmIOSpark.java b/src/main/java/team3176/robot/subsystems/superstructure/ArmIOSpark.java index 1b6b8bf..d440412 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/ArmIOSpark.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/ArmIOSpark.java @@ -7,17 +7,12 @@ package team3176.robot.subsystems.superstructure; -import org.littletonrobotics.junction.AutoLog; -import org.littletonrobotics.junction.LogTable; -import org.littletonrobotics.junction.inputs.LoggableInputs; - import com.ctre.phoenix.sensors.AbsoluteSensorRange; import com.ctre.phoenix.sensors.CANCoder; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import team3176.robot.constants.SuperStructureConstants; import team3176.robot.constants.Hardwaremap; @@ -36,6 +31,7 @@ public ArmIOSpark() { armEncoder.configSensorDirection(true,100); } /** Updates the set of loggable inputs. */ + @Override public void updateInputs(ArmIOInputs inputs) { inputs.Position = armEncoder.getAbsolutePosition(); inputs.VelocityRadPerSec = Units.degreesToRadians(armEncoder.getVelocity()); @@ -43,9 +39,11 @@ public void updateInputs(ArmIOInputs inputs) { inputs.CurrentAmps = new double[] {armController.getOutputCurrent()}; inputs.TempCelcius = new double[] {armController.getMotorTemperature()}; } + @Override public void set(double percentOuput) { armController.set(percentOuput); } + @Override public void setCoastMode(boolean isCoastMode) { if(isCoastMode) { armController.setIdleMode(IdleMode.kCoast); @@ -53,6 +51,9 @@ public void setCoastMode(boolean isCoastMode) { armController.setIdleMode(IdleMode.kBrake); } } - public void reset() {} + @Override + public void reset() { + //to be implemented + } } diff --git a/src/main/java/team3176/robot/subsystems/superstructure/Claw.java b/src/main/java/team3176/robot/subsystems/superstructure/Claw.java index e5e0158..3ab63e4 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Claw.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Claw.java @@ -12,13 +12,11 @@ import team3176.robot.constants.Hardwaremap; import team3176.robot.constants.SuperStructureConstants; import team3176.robot.subsystems.superstructure.Superstructure.GamePiece; - -import team3176.robot.subsystems.superstructure.ClawIO; import team3176.robot.subsystems.superstructure.ClawIO.ClawIOInputs; import org.littletonrobotics.junction.Logger; public class Claw extends SubsystemBase { - private CANSparkMax claw; + private CANSparkMax clawSpark; private DigitalInput linebreakOne; private DigitalInput linebreakTwo; private DigitalInput linebreakThree; @@ -28,17 +26,17 @@ public class Claw extends SubsystemBase { public GamePiece currentGamePiece = GamePiece.CONE; private Claw(ClawIO io) { this.io = io; - claw = new CANSparkMax(Hardwaremap.claw_CID, MotorType.kBrushless); + clawSpark = new CANSparkMax(Hardwaremap.claw_CID, MotorType.kBrushless); linebreakOne = new DigitalInput(0); linebreakTwo = new DigitalInput(2); linebreakThree = new DigitalInput(1); } public void setClawMotor(double percent, int amps) { - claw.set(percent); - claw.setSmartCurrentLimit(amps); + clawSpark.set(percent); + clawSpark.setSmartCurrentLimit(amps); SmartDashboard.putNumber("intake power (%)", percent); - SmartDashboard.putNumber("intake motor current (amps)", claw.getOutputCurrent()); - SmartDashboard.putNumber("intake motor temperature (C)", claw.getMotorTemperature()); + SmartDashboard.putNumber("intake motor current (amps)", clawSpark.getOutputCurrent()); + SmartDashboard.putNumber("intake motor temperature (C)", clawSpark.getMotorTemperature()); } //states now implemented as functions diff --git a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java index 2b6b16b..121b3de 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java @@ -4,12 +4,9 @@ package team3176.robot.subsystems.superstructure; -import com.ctre.phoenix.motorcontrol.can.TalonFX; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PneumaticsModuleType; @@ -20,24 +17,17 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.Command; -import team3176.robot.subsystems.superstructure.IntakeConeIO; import team3176.robot.subsystems.superstructure.IntakeConeIO.IntakeConeIOInputs; import org.littletonrobotics.junction.Logger; -import team3176.robot.subsystems.superstructure.Claw; - -import team3176.robot.constants.Hardwaremap; - public class IntakeCone extends SubsystemBase { /** Creates a new IntakeCone. */ private CANSparkMax rollermotor = new CANSparkMax(8, MotorType.kBrushless); private DoubleSolenoid pistonOne; private DigitalInput linebreak; - private boolean isExtended; - private boolean isInIntake; private static IntakeCone instance; - private Claw m_Claw; + private Claw claw; private final IntakeConeIO io; private final IntakeConeIOInputs inputs = new IntakeConeIOInputs(); public IntakeCone(IntakeConeIO io) @@ -47,7 +37,7 @@ public IntakeCone(IntakeConeIO io) //pistonTwo = new DoubleSolenoid(PneumaticsModuleType.REVPH, 3, 2); linebreak = new DigitalInput(3); - m_Claw = Claw.getInstance(); + claw = Claw.getInstance(); } @@ -77,14 +67,11 @@ public void setBrakeMode() { public void Extend() { pistonOne.set(Value.kForward); - //pistonTwo.set(Value.kForward); - this.isExtended = true; + } public void Retract() { pistonOne.set(Value.kReverse); - //pistonTwo.set(Value.kReverse); - this.isExtended = false; } public boolean getLinebreak() @@ -116,7 +103,7 @@ public void periodic() { public Command coneToClaw() { return this.run(() -> {spit();}) - .until(() -> this.m_Claw.getLinebreakTwo() == false) + .until(() -> this.claw.getLinebreakTwo() == false) .andThen(new WaitCommand(0.5)) .andThen(this.runOnce(()->idle())).withTimeout(2.0).finallyDo((b)->idle()); } diff --git a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java index 35de729..f4ae54c 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java @@ -17,7 +17,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.Command; -import team3176.robot.subsystems.superstructure.IntakeCubeIO; import team3176.robot.subsystems.superstructure.IntakeCubeIO.IntakeCubeIOInputs; import org.littletonrobotics.junction.Logger; @@ -28,11 +27,8 @@ public class IntakeCube extends SubsystemBase { private TalonFX rollermotor = new TalonFX(Hardwaremap.intake_CID); private TalonSRX conveyor = new TalonSRX(61); //TODO: Add to HArdwareMap private DoubleSolenoid pistonOne; - private DoubleSolenoid pistonTwo; private DigitalInput linebreak; - private boolean isExtended; - private boolean isInIntake; private static IntakeCube instance; private final IntakeCubeIO io; private final IntakeCubeIOInputs inputs = new IntakeCubeIOInputs(); @@ -67,13 +63,11 @@ public void setBrakeMode() { public void Extend() { pistonOne.set(Value.kForward); //pistonTwo.set(Value.kForward); - this.isExtended = true; } public void Retract() { pistonOne.set(Value.kReverse); //pistonTwo.set(Value.kReverse); - this.isExtended = false; } public boolean getLinebreak() diff --git a/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java b/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java index bc03845..064b3bd 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java @@ -1,35 +1,27 @@ package team3176.robot.subsystems.superstructure; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; -import team3176.robot.commands.superstructure.arm.armAnalogDown; import team3176.robot.commands.superstructure.claw.ClawInhaleCone; import team3176.robot.commands.superstructure.claw.ClawInhaleCube; import team3176.robot.commands.superstructure.intakecone.IntakeConeExtendSpin; import team3176.robot.commands.superstructure.intakecone.IntakeConeRetractSpinot; import team3176.robot.commands.superstructure.intakecube.*; -import team3176.robot.subsystems.superstructure.IntakeCone; -import team3176.robot.constants.Hardwaremap; import team3176.robot.constants.SuperStructureConstants; public class Superstructure extends SubsystemBase { private static Superstructure instance; - private Arm m_Arm; - private Claw m_Claw; - private IntakeCube m_IntakeCube; - private IntakeCone m_IntakeCone; + private Arm arm; + private Claw claw; + private IntakeCone intakeCone; public Superstructure() { - m_Arm = Arm.getInstance(); - m_Claw = Claw.getInstance(); - m_IntakeCube = IntakeCube.getInstance(); - m_IntakeCone = IntakeCone.getInstance(); + arm = Arm.getInstance(); + claw = Claw.getInstance(); + intakeCone = IntakeCone.getInstance(); } public static Superstructure getInstance() { if (instance == null){instance = new Superstructure();} @@ -38,7 +30,7 @@ public static Superstructure getInstance() { - public static enum GamePiece {CUBE, CONE, NONE}; + public enum GamePiece {CUBE, CONE, NONE} public Command groundCube() { @@ -47,18 +39,18 @@ public Command groundCube() { public Command groundCone() { - return new ParallelCommandGroup(m_Arm.armSetPositionOnce(SuperStructureConstants.ARM_CATCH_POS), + return new ParallelCommandGroup(arm.armSetPositionOnce(SuperStructureConstants.ARM_CATCH_POS), new IntakeConeExtendSpin(), new ClawInhaleCone()) - .until(() -> this.m_Claw.getLinebreakThree() == false) - .andThen(m_IntakeCone.coneToClaw()) + .until(() -> this.claw.getLinebreakThree() == false) + .andThen(intakeCone.coneToClaw()) .andThen(new IntakeConeRetractSpinot()) .andThen(this.prepareCarry()); } public Command clawIntakeCube() { - return new InstantCommand(() -> m_Claw.intakeGamePiece(GamePiece.CUBE)); + return new InstantCommand(() -> claw.intakeGamePiece(GamePiece.CUBE)); } /* @@ -72,65 +64,65 @@ public Command poopCube() { } */ public Command scoreGamePieceAuto() { - return m_Claw.determineGamePiece() - .andThen(m_Arm.armSetPositionBlocking(SuperStructureConstants.ARM_HIGH_POS).withTimeout(1.5) - .andThen(m_Claw.scoreGamePiece()) + return claw.determineGamePiece() + .andThen(arm.armSetPositionBlocking(SuperStructureConstants.ARM_HIGH_POS).withTimeout(1.5) + .andThen(claw.scoreGamePiece()) .andThen(this.prepareCarry())); } public Command scoreFirstGamePieceAuto() { - return m_Claw.determineGamePiece() - .andThen(m_IntakeCone.extendAndFreeSpin().withTimeout(1.0) - .alongWith(m_Arm.armSetPositionBlocking(SuperStructureConstants.ARM_HIGH_POS).withTimeout(3.0) + return claw.determineGamePiece() + .andThen(intakeCone.extendAndFreeSpin().withTimeout(1.0) + .alongWith(arm.armSetPositionBlocking(SuperStructureConstants.ARM_HIGH_POS).withTimeout(3.0) .andThen(new WaitCommand(0.5)) - .andThen(m_Claw.scoreGamePiece().withTimeout(1.0)) + .andThen(claw.scoreGamePiece().withTimeout(1.0)) .andThen(this.prepareCarry()))); } public Command scoreGamePieceHigh() { - return m_Claw.determineGamePiece() - .andThen(m_Arm.armSetPositionBlocking(SuperStructureConstants.ARM_HIGH_POS).withTimeout(3.0)) + return claw.determineGamePiece() + .andThen(arm.armSetPositionBlocking(SuperStructureConstants.ARM_HIGH_POS).withTimeout(3.0)) .andThen(new WaitCommand(0.5)) - .andThen(m_Claw.scoreGamePiece().withTimeout(1.0)) + .andThen(claw.scoreGamePiece().withTimeout(1.0)) .andThen(this.prepareCarry()); } public Command scoreCubeLow() { - return m_Arm.armSetPosition(SuperStructureConstants.ARM_ZERO_POS) + return arm.armSetPosition(SuperStructureConstants.ARM_ZERO_POS) .andThen(new WaitCommand(0.5)) - .andThen(m_Claw.scoreGamePiece()) + .andThen(claw.scoreGamePiece()) .andThen(this.prepareCarry()); } public Command scoreGamePieceLowAuto() { - return m_Claw.determineGamePiece() - .andThen(m_Arm.armSetPositionBlocking(SuperStructureConstants.ARM_CATCH_POS).withTimeout(3.0)) + return claw.determineGamePiece() + .andThen(arm.armSetPositionBlocking(SuperStructureConstants.ARM_CATCH_POS).withTimeout(3.0)) .andThen(new WaitCommand(0.5)) - .andThen(m_Claw.scoreGamePiece().withTimeout(1.0)) + .andThen(claw.scoreGamePiece().withTimeout(1.0)) .andThen(this.prepareCarry()); } public Command intakeCubeHumanPlayer() { - return new ParallelCommandGroup(new ClawInhaleCube(), m_Arm.armSetPositionOnce(SuperStructureConstants.ARM_HIGH_POS)) - .andThen(m_Arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)); + return new ParallelCommandGroup(new ClawInhaleCube(), arm.armSetPositionOnce(SuperStructureConstants.ARM_HIGH_POS)) + .andThen(arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)); } public Command intakeConeHumanPlayer() { - return new ParallelCommandGroup(new ClawInhaleCone(), m_Arm.armSetPositionOnce(SuperStructureConstants.ARM_HIGH_POS)) - .andThen(m_Arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)); + return new ParallelCommandGroup(new ClawInhaleCone(), arm.armSetPositionOnce(SuperStructureConstants.ARM_HIGH_POS)) + .andThen(arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)); } public Command preparePoop() { - return m_Arm.armSetPositionOnce(SuperStructureConstants.ARM_ZERO_POS); + return arm.armSetPositionOnce(SuperStructureConstants.ARM_ZERO_POS); } public Command prepareCarry() { - return m_Arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS); + return arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS); } public Command prepareCatch() { - return m_Arm.armSetPositionOnce(SuperStructureConstants.ARM_CATCH_POS); + return arm.armSetPositionOnce(SuperStructureConstants.ARM_CATCH_POS); } public Command prepareScoreMid() { - return m_Arm.armSetPositionOnce(SuperStructureConstants.ARM_MID_POS); + return arm.armSetPositionOnce(SuperStructureConstants.ARM_MID_POS); } public Command prepareScoreHigh() { - return m_Arm.armSetPositionOnce(SuperStructureConstants.ARM_HIGH_POS); + return arm.armSetPositionOnce(SuperStructureConstants.ARM_HIGH_POS); } } diff --git a/src/main/java/team3176/robot/subsystems/vision/VisionDual.java b/src/main/java/team3176/robot/subsystems/vision/VisionDual.java index 7596bd1..0232cd0 100644 --- a/src/main/java/team3176/robot/subsystems/vision/VisionDual.java +++ b/src/main/java/team3176/robot/subsystems/vision/VisionDual.java @@ -7,8 +7,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import team3176.robot.subsystems.vision.VisionDualIO.VisionDualInputs; - public class VisionDual extends SubsystemBase{ private VisionDualIO io; private VisionDualInputsAutoLogged inputs; @@ -37,7 +35,9 @@ public boolean isValid() { return inputs.lValid || inputs.rValid; } + @Override public void periodic() { + io.updateInputs(inputs); Logger.getInstance().processInputs("Vision", inputs); boolean isRed = DriverStation.getAlliance() == Alliance.Red; Pose3d rPose = isRed ? inputs.rfovRed : inputs.rfovBlue; From 5dfd6f26fdff121cbdd27444d1cec0ceeaa6360c Mon Sep 17 00:00:00 2001 From: Jonathan Date: Mon, 12 Jun 2023 14:45:20 -0600 Subject: [PATCH 2/7] drivetrain sonar lint --- .../subsystems/drivetrain/Drivetrain.java | 112 ++++++++---------- .../robot/subsystems/drivetrain/GyroIO.java | 4 - .../subsystems/drivetrain/SwervePod.java | 79 ++++++------ .../drivetrain/SwervePodIOFalconSpark.java | 24 +++- .../subsystems/drivetrain/SwervePodIOSim.java | 3 - .../subsystems/vision/VisionDualIOLime.java | 28 ++--- 6 files changed, 121 insertions(+), 129 deletions(-) diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index 7dfc926..7561211 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -3,7 +3,6 @@ // the WPILib BSD license file in the root directory of this project. package team3176.robot.subsystems.drivetrain; -import com.pathplanner.lib.PathPlannerTrajectory; import edu.wpi.first.math.geometry.Pose2d; @@ -17,8 +16,6 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.DoubleTopic; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; @@ -34,6 +31,7 @@ import team3176.robot.Constants; import team3176.robot.Constants.Mode; import team3176.robot.constants.DrivetrainConstants; +import team3176.robot.constants.DrivetrainHardwareMap; import team3176.robot.constants.SwervePodHardwareID; import team3176.robot.subsystems.vision.VisionDual; @@ -43,13 +41,8 @@ public class Drivetrain extends SubsystemBase { private static Drivetrain instance; - public SwerveDriveOdometry odom; - public SwerveDrivePoseEstimator poseEstimator; - - public NetworkTableInstance inst; - public NetworkTable table; - public DoubleTopic dblTopic; - public DoublePublisher dblPub; + private SwerveDriveOdometry odom; + private SwerveDrivePoseEstimator poseEstimator; // private Controller controller = Controller.getInstance(); // private Vision m_Vision = Vision.getInstance(); @@ -57,7 +50,7 @@ public enum coordType { FIELD_CENTRIC, ROBOT_CENTRIC } - public coordType currentCoordType = coordType.FIELD_CENTRIC; + private coordType currentCoordType = coordType.FIELD_CENTRIC; //private PowerDistribution PDH = new PowerDistribution(); // PowerDistribution(PowerManagementConstants.PDP_CAN_ID, ModuleType.kCTRE); @@ -67,7 +60,7 @@ public enum coordType { - Rotation2d FieldAngleOffset = Rotation2d.fromDegrees(0.0); + Rotation2d fieldAngleOffset = Rotation2d.fromDegrees(0.0); private double forwardCommand; @@ -93,12 +86,11 @@ public enum driveMode { private SwervePod podFL; private SwervePod podBL; private SwervePod podBR; - public PathPlannerTrajectory teleopTraj; NetworkTable vision; - NetworkTableEntry vision_pose; - Pose2d last_pose = new Pose2d(); + NetworkTableEntry visionPose; + Pose2d lastPose = new Pose2d(); double lastVisionTimeStamp = 0.0; double lastVisionX = 0.0; Rotation2d wheelOnlyHeading = new Rotation2d(); @@ -112,30 +104,24 @@ public enum driveMode { private Drivetrain(GyroIO io) { this.io = io; inputs = new GyroIOInputsAutoLogged(); - inst = NetworkTableInstance.getDefault(); - table = inst.getTable("datatable"); - - dblTopic = table.getDoubleTopic("Angle"); - - dblPub = dblTopic.publish(); field = new Field2d(); // check for duplicates - assert (!SwervePodHardwareID.check_duplicates_all(DrivetrainConstants.FR, DrivetrainConstants.FL, - DrivetrainConstants.BR, DrivetrainConstants.BL)); + assert (!SwervePodHardwareID.check_duplicates_all(DrivetrainHardwareMap.FR, DrivetrainHardwareMap.FL, + DrivetrainHardwareMap.BR, DrivetrainHardwareMap.BL)); // Instantiate pods if(Constants.getMode() != Mode.REPLAY) { switch(Constants.getRobot()){ case ROBOT_2023C: System.out.println("[init] normal swervePods"); - DrivetrainConstants.FR.OFFSET += 180; - DrivetrainConstants.FL.OFFSET += 90; - DrivetrainConstants.BL.OFFSET += 0; - DrivetrainConstants.BR.OFFSET += -90; - podFR = new SwervePod(0, new SwervePodIOFalconSpark(DrivetrainConstants.FR,DrivetrainConstants.STEER_FR_CID)); - podFL = new SwervePod(1, new SwervePodIOFalconSpark(DrivetrainConstants.FL,DrivetrainConstants.STEER_FL_CID)); - podBL = new SwervePod(2, new SwervePodIOFalconSpark(DrivetrainConstants.BL,DrivetrainConstants.STEER_BL_CID)); - podBR = new SwervePod(3, new SwervePodIOFalconSpark(DrivetrainConstants.BR,DrivetrainConstants.STEER_BR_CID)); + DrivetrainHardwareMap.FR.OFFSET += 180; + DrivetrainHardwareMap.FL.OFFSET += 90; + DrivetrainHardwareMap.BL.OFFSET += 0; + DrivetrainHardwareMap.BR.OFFSET += -90; + podFR = new SwervePod(0, new SwervePodIOFalconSpark(DrivetrainHardwareMap.FR,DrivetrainHardwareMap.STEER_FR_CID)); + podFL = new SwervePod(1, new SwervePodIOFalconSpark(DrivetrainHardwareMap.FL,DrivetrainHardwareMap.STEER_FL_CID)); + podBL = new SwervePod(2, new SwervePodIOFalconSpark(DrivetrainHardwareMap.BL,DrivetrainHardwareMap.STEER_BL_CID)); + podBR = new SwervePod(3, new SwervePodIOFalconSpark(DrivetrainHardwareMap.BR,DrivetrainHardwareMap.STEER_BR_CID)); break; case ROBOT_2023P: break; @@ -158,7 +144,7 @@ private Drivetrain(GyroIO io) { } // Instantiate array list then add instantiated pods to list - pods = new ArrayList(); + pods = new ArrayList<>(); pods.add(podFR); pods.add(podFL); pods.add(podBL); @@ -243,50 +229,50 @@ public void drive(double forwardCommand, double strafeCommand, double spinComman */ private void calculateNSetPodPositions() { if (currentDriveMode != driveMode.DEFENSE) { - ChassisSpeeds curr_chassisSpeeds = new ChassisSpeeds(forwardCommand, strafeCommand, spinCommand); + ChassisSpeeds currChassisSpeeds = new ChassisSpeeds(forwardCommand, strafeCommand, spinCommand); if (this.currentCoordType == coordType.FIELD_CENTRIC) { Rotation2d fieldOffset = this.getPose().getRotation(); if (DriverStation.getAlliance() == Alliance.Red) { fieldOffset.plus(Rotation2d.fromDegrees(180)); } - curr_chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(curr_chassisSpeeds, fieldOffset); + currChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(currChassisSpeeds, fieldOffset); } if (isSpinLocked) { - curr_chassisSpeeds.omegaRadiansPerSecond = spinLockPID.calculate(getPoseYawWrapped().getDegrees(), spinLockAngle.getDegrees()); + currChassisSpeeds.omegaRadiansPerSecond = spinLockPID.calculate(getPoseYawWrapped().getDegrees(), spinLockAngle.getDegrees()); SmartDashboard.putNumber("SpinLockYaw",getPoseYawWrapped().getDegrees()); } - SwerveModuleState[] pod_states = DrivetrainConstants.DRIVE_KINEMATICS.toSwerveModuleStates(curr_chassisSpeeds); - Logger.getInstance().recordOutput("Drive/pod0", pod_states[0].angle.getDegrees()); - SwerveDriveKinematics.desaturateWheelSpeeds(pod_states, DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND); + SwerveModuleState[] podStates = DrivetrainConstants.DRIVE_KINEMATICS.toSwerveModuleStates(currChassisSpeeds); + Logger.getInstance().recordOutput("Drive/pod0", podStates[0].angle.getDegrees()); + SwerveDriveKinematics.desaturateWheelSpeeds(podStates, DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND); SwerveModuleState[] optimizedStates = new SwerveModuleState[4]; SwerveModuleState[] realStates = new SwerveModuleState[4]; for (int idx = 0; idx < (pods.size()); idx++) { - optimizedStates[idx]=pods.get(idx).set_module(pod_states[idx]); + optimizedStates[idx]=pods.get(idx).setModule(podStates[idx]); realStates[idx] = new SwerveModuleState(pods.get(idx).getVelocity(),Rotation2d.fromDegrees(pods.get(idx).getAzimuth())); } - Logger.getInstance().recordOutput("SwerveStates/Setpoints", pod_states); + Logger.getInstance().recordOutput("SwerveStates/Setpoints", podStates); Logger.getInstance().recordOutput("SwerveStates/real", realStates); Logger.getInstance().recordOutput("SwerveStates/SetpointsOptimized", optimizedStates); Logger.getInstance().recordOutput("Drive/SpinCommand", spinCommand); SmartDashboard.putNumber("spinCommand", spinCommand); - SmartDashboard.putNumber("pod0 m/s", pod_states[0].speedMetersPerSecond); + SmartDashboard.putNumber("pod0 m/s", podStates[0].speedMetersPerSecond); - } else if (currentDriveMode == driveMode.DEFENSE) { // Enter defensive position + } else { // Enter defensive position double smallNum = Math.pow(10, -5); - pods.get(0).set_module(smallNum, Rotation2d.fromRadians(1.0 * Math.PI / 8.0)); - pods.get(1).set_module(smallNum, Rotation2d.fromRadians(-1.0 * Math.PI / 8.0)); - pods.get(2).set_module(smallNum, Rotation2d.fromRadians(-3.0 * Math.PI / 8.0)); - pods.get(3).set_module(smallNum, Rotation2d.fromRadians(3.0 * Math.PI / 8.0)); + pods.get(0).setModule(smallNum, Rotation2d.fromRadians(1.0 * Math.PI / 8.0)); + pods.get(1).setModule(smallNum, Rotation2d.fromRadians(-1.0 * Math.PI / 8.0)); + pods.get(2).setModule(smallNum, Rotation2d.fromRadians(-3.0 * Math.PI / 8.0)); + pods.get(3).setModule(smallNum, Rotation2d.fromRadians(3.0 * Math.PI / 8.0)); } } public void setDriveMode(driveMode wantedDriveMode) { - currentDriveMode = wantedDriveMode; + this.currentDriveMode = wantedDriveMode; } public driveMode getCurrentDriveMode() { - return currentDriveMode; + return this.currentDriveMode; } public Pose2d getPose() { @@ -313,7 +299,7 @@ public void resetPoseToVision() { public void setModuleStates(SwerveModuleState[] states) { for (int idx = 0; idx < (pods.size()); idx++) { - pods.get(idx).set_module(states[idx]); + pods.get(idx).setModule(states[idx]); } } @@ -396,11 +382,11 @@ public void resetFieldOrientation() { // do not need to invert because the navx rotation2D call returns a NWU // coordsys! //this.FieldAngleOffset = m_NavX.getRotation2d(); - Rotation2d RedorBlue_Zero = new Rotation2d(); + Rotation2d redOrBlueZero = new Rotation2d(); if (DriverStation.getAlliance() == Alliance.Red) { - RedorBlue_Zero.plus(Rotation2d.fromDegrees(180)); + redOrBlueZero.plus(Rotation2d.fromDegrees(180)); } - resetPose(new Pose2d(getPose().getTranslation(),RedorBlue_Zero)); + resetPose(new Pose2d(getPose().getTranslation(),redOrBlueZero)); } public double getPodVelocity(int podID) { @@ -478,7 +464,7 @@ public void periodic() { // SmartDashboard.putNumber("camX",cam_pose.getX()); // } - last_pose = odom.getPoseMeters(); + lastPose = odom.getPoseMeters(); SwerveModulePosition[] deltas = new SwerveModulePosition[4]; for(int i=0;i< pods.size(); i++) { deltas[i] = pods.get(i).getDelta(); @@ -492,8 +478,8 @@ public void periodic() { SmartDashboard.putNumber("NavYaw",getPoseYawWrapped().getDegrees()); //Liam and Andrews work! - double[] vision_pose = NetworkTableInstance.getDefault().getTable("limelight-rfov").getEntry("botpose_wpiblue").getDoubleArray(new double[6]); - Pose3d visionPose3dNT = new Pose3d(vision_pose[0], vision_pose[1], vision_pose[2], new Rotation3d( Units.degreesToRadians(vision_pose[3]), Units.degreesToRadians(vision_pose[4]), Units.degreesToRadians(vision_pose[5]))); + double[] visionPoseArray = NetworkTableInstance.getDefault().getTable("limelight-rfov").getEntry("botpose_wpiblue").getDoubleArray(new double[6]); + Pose3d visionPose3dNT = new Pose3d(visionPoseArray[0], visionPoseArray[1], visionPoseArray[2], new Rotation3d( Units.degreesToRadians(visionPoseArray[3]), Units.degreesToRadians(visionPoseArray[4]), Units.degreesToRadians(visionPoseArray[5]))); Logger.getInstance().recordOutput("Drive/vision_pose", visionPose3dNT); //new vision proposal @@ -556,23 +542,23 @@ public void simulationPeriodic() { } public void publishSwervePodPIDErrors(){ - double FRAzError = podFR.getAzimuthSetpoint() - podFR.getAzimuth(); - double FRThrustError = podFR.getThrustSetpoint() - podFR.getThrustEncoderVelocity(); + final double FRAzError = podFR.getAzimuthSetpoint() - podFR.getAzimuth(); + final double FRThrustError = podFR.getThrustSetpoint() - podFR.getThrustEncoderVelocity(); SmartDashboard.putNumber("FRAzError", FRAzError); SmartDashboard.putNumber("FRThrustError", FRThrustError); - double FLAzError = podFL.getAzimuthSetpoint() - podFL.getAzimuth(); - double FLThrustError = podFL.getThrustSetpoint() - podFL.getThrustEncoderVelocity(); + final double FLAzError = podFL.getAzimuthSetpoint() - podFL.getAzimuth(); + final double FLThrustError = podFL.getThrustSetpoint() - podFL.getThrustEncoderVelocity(); SmartDashboard.putNumber("FLAzError", FLAzError); SmartDashboard.putNumber("FLThrustError", FLThrustError); - double BRAzError = podBR.getAzimuthSetpoint() - podBR.getAzimuth(); - double BRThrustError = podBR.getThrustSetpoint() - podBR.getThrustEncoderVelocity(); + final double BRAzError = podBR.getAzimuthSetpoint() - podBR.getAzimuth(); + final double BRThrustError = podBR.getThrustSetpoint() - podBR.getThrustEncoderVelocity(); SmartDashboard.putNumber("BRAzError", BRAzError); SmartDashboard.putNumber("BRThrustError", BRThrustError); - double BLAzError = podBL.getAzimuthSetpoint() - podBL.getAzimuth(); - double BLThrustError = podBL.getThrustSetpoint() - podBL.getThrustEncoderVelocity(); + final double BLAzError = podBL.getAzimuthSetpoint() - podBL.getAzimuth(); + final double BLThrustError = podBL.getThrustSetpoint() - podBL.getThrustEncoderVelocity(); SmartDashboard.putNumber("BLAzError", BLAzError); SmartDashboard.putNumber("BLThrustError", BLThrustError); diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/GyroIO.java b/src/main/java/team3176/robot/subsystems/drivetrain/GyroIO.java index 05515f8..1945462 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/GyroIO.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/GyroIO.java @@ -5,11 +5,7 @@ package team3176.robot.subsystems.drivetrain; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - import org.littletonrobotics.junction.AutoLog; -import org.littletonrobotics.junction.LogTable; -import org.littletonrobotics.junction.inputs.LoggableInputs; /** Template hardware interface for a closed loop subsystem. */ public interface GyroIO{ diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java index 7d16ae1..3c9cd98 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import team3176.robot.constants.DrivetrainConstants; +import team3176.robot.constants.DrivetrainHardwareMap; import team3176.robot.util.LoggedTunableNumber; import team3176.robot.util.God.*; @@ -24,7 +25,7 @@ public class SwervePod { /** Current value in radians of the azimuthEncoder's position */ double azimuthEncoderRelPosition; double azimuthEncoderAbsPosition; - double desired_optimized_azimuth_position; + double desiredOptimizedAzimuthPosition; double velTicsPer100ms; boolean lastHasResetOccurred; @@ -42,21 +43,15 @@ public class SwervePod { private double lastEncoderPos; - public int kSlotIdx_Azimuth, kPIDLoopIdx_Azimuth, kTimeoutMs_Azimuth,kSlotIdx_Thrust, kPIDLoopIdx_Thrust, kTimeoutMs_Thrust; - - public double podThrust, podAzimuth, podAbsAzimuth; - //private double kP_Azimuth; - private LoggedTunableNumber kP_azimuth = new LoggedTunableNumber("kP_azimuth"); - private LoggedTunableNumber kI_Azimuth = new LoggedTunableNumber("kI_azimuth"); - private double kD_Azimuth; + private LoggedTunableNumber kPAzimuth = new LoggedTunableNumber("kP_azimuth"); + private LoggedTunableNumber kIAzimuth = new LoggedTunableNumber("kI_azimuth"); + private double kDAzimuth; private double lastDistance =0.0; private double delta = 0.0; private LoggedTunableNumber velMax = new LoggedTunableNumber("az_vel"); private LoggedTunableNumber velAcc = new LoggedTunableNumber("az_acc"); - private double turnOutput; - private final PIDController turningPIDController; //private final ProfiledPIDController m_turningProfiledPIDController; //private ProfiledPIDController m_turningPIDController; @@ -67,71 +62,71 @@ public class SwervePod { public SwervePod(int id, SwervePodIO io) { this.id = id; this.io = io; - this.desired_optimized_azimuth_position = 0.0; + this.desiredOptimizedAzimuthPosition = 0.0; //this.kP_Azimuth = 0.006; - kP_azimuth.initDefault(.007); - this.kI_Azimuth.initDefault(0.0); - this.kD_Azimuth = 0.0; + kPAzimuth.initDefault(.007); + this.kIAzimuth.initDefault(0.0); + this.kDAzimuth = 0.0; velMax.initDefault(900); velAcc.initDefault(900); - turningPIDController = new PIDController(kP_azimuth.get(), kI_Azimuth.get(), kD_Azimuth);//,new Constraints(velMax.get(), velAcc.get())); + turningPIDController = new PIDController(kPAzimuth.get(), kIAzimuth.get(), kDAzimuth);//,new Constraints(velMax.get(), velAcc.get())); turningPIDController.setTolerance(4); turningPIDController.enableContinuousInput(-180, 180); turningPIDController.setIntegratorRange(-0.1,0.1); - turningPIDController.setP(this.kP_azimuth.get()); - turningPIDController.setI(this.kI_Azimuth.get()); - turningPIDController.setD(this.kD_Azimuth); + turningPIDController.setP(this.kPAzimuth.get()); + turningPIDController.setI(this.kIAzimuth.get()); + turningPIDController.setD(this.kDAzimuth); } - public void set_module(double speedMetersPerSecond, Rotation2d angle) { - set_module(new SwerveModuleState(speedMetersPerSecond,angle)); + public void setModule(double speedMetersPerSecond, Rotation2d angle) { + setModule(new SwerveModuleState(speedMetersPerSecond,angle)); } /** * alternative method for setting swervepod in line with WPILIB standard library * @param desiredState */ - public SwerveModuleState set_module(SwerveModuleState desiredState) { + public SwerveModuleState setModule(SwerveModuleState desiredState) { io.updateInputs(inputs); Logger.getInstance().processInputs("Drive/Module" + Integer.toString(this.id), inputs); this.azimuthEncoderAbsPosition = inputs.turnAbsolutePositionDegrees; - SwerveModuleState desired_optimized = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(this.azimuthEncoderAbsPosition)); - this.desired_optimized_azimuth_position = desired_optimized.angle.getDegrees(); - + SwerveModuleState desiredOptimized = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(this.azimuthEncoderAbsPosition)); + this.desiredOptimizedAzimuthPosition = desiredOptimized.angle.getDegrees(); + double turnOutput; if (desiredState.speedMetersPerSecond > (-Math.pow(10,-10)) && desiredState.speedMetersPerSecond < (Math.pow(10,-10))) { - this.turnOutput = turningPIDController.calculate(this.azimuthEncoderAbsPosition, this.lastEncoderPos); + turnOutput = turningPIDController.calculate(this.azimuthEncoderAbsPosition, this.lastEncoderPos); } else { - this.turnOutput = turningPIDController.calculate(this.azimuthEncoderAbsPosition, desired_optimized.angle.getDegrees()); - this.lastEncoderPos = desired_optimized.angle.getDegrees(); + turnOutput = turningPIDController.calculate(this.azimuthEncoderAbsPosition, desiredOptimized.angle.getDegrees()); + this.lastEncoderPos = desiredOptimized.angle.getDegrees(); } // reduce output if the error is high double currentDistance = Units.feetToMeters((DrivetrainConstants.WHEEL_DIAMETER_INCHES/12.0 * Math.PI) * inputs.drivePositionRad / (2*Math.PI)); this.delta = currentDistance - this.lastDistance; this.lastDistance = currentDistance; - desired_optimized.speedMetersPerSecond *= Math.abs(Math.cos(desired_optimized.angle.minus(Rotation2d.fromDegrees(azimuthEncoderAbsPosition)).getRadians())); + desiredOptimized.speedMetersPerSecond *= Math.abs(Math.cos(desiredOptimized.angle.minus(Rotation2d.fromDegrees(azimuthEncoderAbsPosition)).getRadians())); //Logger.getInstance().recordOutput("Drive/Module" + Integer.toString(this.id) + "", id); - io.setTurn(MathUtil.clamp(this.turnOutput, -0.4, 0.4)); + io.setTurn(MathUtil.clamp(turnOutput, -0.4, 0.4)); Logger.getInstance().recordOutput("Drive/Module" + Integer.toString(this.id) + "/error",turningPIDController.getPositionError()); //Logger.getInstance().recordOutput("Drive/Module" + Integer.toString(this.id) + "/setpoint",turningPIDController.getSetpoint().position); - this.velTicsPer100ms = Units3176.mps2ums(desired_optimized.speedMetersPerSecond); - io.setDrive(desired_optimized.speedMetersPerSecond); + this.velTicsPer100ms = Units3176.mps2ums(desiredOptimized.speedMetersPerSecond); + io.setDrive(desiredOptimized.speedMetersPerSecond); - if(kP_azimuth.hasChanged(hashCode()) || kI_Azimuth.hasChanged(hashCode())) { - turningPIDController.setP(kP_azimuth.get()); - turningPIDController.setI(kI_Azimuth.get()); + if(kPAzimuth.hasChanged(hashCode()) || kIAzimuth.hasChanged(hashCode())) { + turningPIDController.setP(kPAzimuth.get()); + turningPIDController.setI(kIAzimuth.get()); } // if(velAcc.hasChanged(hashCode()) || velMax.hasChanged(hashCode())){ // turningPIDController.setConstraints(new Constraints(velMax.get(),velAcc.get())); // } - return desired_optimized; + return desiredOptimized; } /* * odometry calls @@ -146,8 +141,8 @@ public SwerveModulePosition getDelta() { public double getVelocity() { double wheelVelocityInFeetPerSecond = inputs.driveVelocityRadPerSec / (Math.PI *2) * DrivetrainConstants.WHEEL_DIAMETER_INCHES/12.0 * Math.PI; - double wheelVelocityInMetersPerSecond = Units3176.feetPerSecond2metersPerSecond(wheelVelocityInFeetPerSecond); - return wheelVelocityInMetersPerSecond; + return Units3176.feetPerSecond2metersPerSecond(wheelVelocityInFeetPerSecond); + } /** @@ -167,7 +162,7 @@ public void setThrustBrake() { } public double getAzimuthSetpoint() { - return this.desired_optimized_azimuth_position; + return this.desiredOptimizedAzimuthPosition; } public double getThrustSetpoint() { return this.velTicsPer100ms; @@ -178,24 +173,24 @@ public double getThrustEncoderVelocity() { public void setupShuffleboard() { Shuffleboard.getTab(this.idString) - .add(idString+"/podAzimuth_setpoint_angle",DrivetrainConstants.AZIMUTH_ABS_ENCODER_OFFSET_POSITION[id]) + .add(idString+"/podAzimuth_setpoint_angle",DrivetrainHardwareMap.AZIMUTH_ABS_ENCODER_OFFSET_POSITION[id]) .withWidget(BuiltInWidgets.kNumberSlider) .withProperties(Map.of("min", -3.16, "max", 3.16)) .withSize(2,1) .withPosition(2,1) .getEntry(); Shuffleboard.getTab(this.idString) - .add(idString+"/kP_Azimuth", this.kP_azimuth.get()) + .add(idString+"/kP_Azimuth", this.kPAzimuth.get()) .withSize(1,1) .withPosition(4,1) .getEntry(); Shuffleboard.getTab(this.idString) - .add(idString+"/kI_Azimuth", this.kI_Azimuth) + .add(idString+"/kI_Azimuth", this.kIAzimuth) .withSize(1,1) .withPosition(5,1) .getEntry(); Shuffleboard.getTab(this.idString) - .add(idString+"/kD_Azimuth", this.kD_Azimuth) + .add(idString+"/kD_Azimuth", this.kDAzimuth) .withSize(1,1) .withPosition(6,1) .getEntry(); diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOFalconSpark.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOFalconSpark.java index a641159..33d6573 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOFalconSpark.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOFalconSpark.java @@ -1,10 +1,12 @@ package team3176.robot.subsystems.drivetrain; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; import com.ctre.phoenix.sensors.AbsoluteSensorRange; import com.ctre.phoenix.sensors.CANCoder; import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import team3176.robot.constants.DrivetrainConstants; @@ -15,7 +17,7 @@ public class SwervePodIOFalconSpark implements SwervePodIO{ private CANSparkMax turnSparkMax; private TalonFX thrustFalcon; private CANCoder azimuthEncoder; - public static double conversion_feet_to_tics = 12.0 * (1.0/ (DrivetrainConstants.WHEEL_DIAMETER_INCHES * Math.PI)) * (1.0 /DrivetrainConstants.THRUST_GEAR_RATIO) * DrivetrainConstants.THRUST_ENCODER_UNITS_PER_REVOLUTION; + public static final double FEET2TICS = 12.0 * (1.0/ (DrivetrainConstants.WHEEL_DIAMETER_INCHES * Math.PI)) * (1.0 /DrivetrainConstants.THRUST_GEAR_RATIO) * DrivetrainConstants.THRUST_ENCODER_UNITS_PER_REVOLUTION; public SwervePodIOFalconSpark(SwervePodHardwareID id,int sparkMaxID) { turnSparkMax = new CANSparkMax(sparkMaxID, MotorType.kBrushless); thrustFalcon = new TalonFX(id.THRUST_CID); @@ -41,6 +43,7 @@ public SwervePodIOFalconSpark(SwervePodHardwareID id,int sparkMaxID) { azimuthEncoder.configSensorDirection(true,100); } + @Override public void updateInputs(SwervePodIOInputs inputs) { inputs.drivePositionRad = thrustFalcon.getSelectedSensorPosition() * (DrivetrainConstants.THRUST_GEAR_RATIO) * 1.0/DrivetrainConstants.THRUST_ENCODER_UNITS_PER_REVOLUTION* 2 * Math.PI; inputs.driveVelocityRadPerSec = thrustFalcon.getSelectedSensorVelocity() * (DrivetrainConstants.THRUST_GEAR_RATIO) * 1.0/DrivetrainConstants.THRUST_ENCODER_UNITS_PER_REVOLUTION * 10 * 2 * Math.PI; @@ -64,13 +67,28 @@ public void setDrive(double velMetersPerSecond) { } /** Run the turn motor at the specified voltage. */ + @Override public void setTurn(double volts) { turnSparkMax.set(volts); } /** Enable or disable brake mode on the drive motor. */ - public void setDriveBrakeMode(boolean enable) {} + @Override + public void setDriveBrakeMode(boolean enable) { + if(enable){ + thrustFalcon.setNeutralMode(NeutralMode.Brake); + } else { + thrustFalcon.setNeutralMode(NeutralMode.Coast); + } + } /** Enable or disable brake mode on the turn motor. */ - public void setTurnBrakeMode(boolean enable) {} + @Override + public void setTurnBrakeMode(boolean enable) { + if(enable) { + turnSparkMax.setIdleMode(IdleMode.kBrake); + } else { + turnSparkMax.setIdleMode(IdleMode.kCoast); + } + } } diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java index a6ee21b..cc5dcab 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java @@ -1,7 +1,6 @@ package team3176.robot.subsystems.drivetrain; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; @@ -13,7 +12,6 @@ public class SwervePodIOSim implements SwervePodIO{ private FlywheelSim driveSim = new FlywheelSim(DCMotor.getFalcon500(1), 4.714, 0.025); private FlywheelSim turnSim = new FlywheelSim(DCMotor.getNeo550(1), 70.0, 0.0005); //private PIDController drivePID = new PIDController(.03, 0, 0.0,.045); - private double turnRelativePositionRad = 0.0; private double turnAbsolutePositionRad = Math.random() * 2.0 * Math.PI; private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; @@ -23,7 +21,6 @@ public void updateInputs(SwervePodIOInputs inputs) { driveSim.update(Constants.LOOP_PERIODIC_SECS); turnSim.update(Constants.LOOP_PERIODIC_SECS); double angleDiffRad = Units.radiansToDegrees(turnSim.getAngularVelocityRadPerSec() * Constants.LOOP_PERIODIC_SECS); - turnRelativePositionRad += angleDiffRad; turnAbsolutePositionRad += angleDiffRad; while (turnAbsolutePositionRad < -180) { turnAbsolutePositionRad += 360; diff --git a/src/main/java/team3176/robot/subsystems/vision/VisionDualIOLime.java b/src/main/java/team3176/robot/subsystems/vision/VisionDualIOLime.java index 89b392e..0c7c6d0 100644 --- a/src/main/java/team3176/robot/subsystems/vision/VisionDualIOLime.java +++ b/src/main/java/team3176/robot/subsystems/vision/VisionDualIOLime.java @@ -1,24 +1,24 @@ package team3176.robot.subsystems.vision; -import org.littletonrobotics.junction.AutoLog; -import edu.wpi.first.math.geometry.Pose3d; + import team3176.robot.subsystems.drivetrain.LimelightHelpers; public class VisionDualIOLime implements VisionDualIO { - public static final String rfov = "rfov-limelight"; - public static final String lfov = "lfov-limelight"; + public static final String RFOV = "rfov-limelight"; + public static final String LFOV = "lfov-limelight"; /** Updates the set of loggable inputs. */ + @Override public void updateInputs(VisionDualInputs inputs) { - inputs.rfovBlue = LimelightHelpers.getBotPose3d_wpiBlue(rfov); - inputs.rfovRed = LimelightHelpers.getBotPose3d_wpiRed(rfov); - inputs.lfovBlue = LimelightHelpers.getBotPose3d_wpiBlue(lfov); - inputs.lfovRed = LimelightHelpers.getBotPose3d_wpiRed(lfov); - inputs.lLatency = LimelightHelpers.getLatency_Capture(lfov) + LimelightHelpers.getLatency_Pipeline(lfov); - inputs.rLatency = LimelightHelpers.getLatency_Capture(rfov) + LimelightHelpers.getLatency_Pipeline(rfov); - inputs.rNumTags = LimelightHelpers.getLatestResults(rfov).targetingResults.targets_Fiducials.length; - inputs.lNumTags = LimelightHelpers.getLatestResults(lfov).targetingResults.targets_Fiducials.length; - inputs.rValid = LimelightHelpers.getTV(rfov); - inputs.lValid = LimelightHelpers.getTV(lfov); + inputs.rfovBlue = LimelightHelpers.getBotPose3d_wpiBlue(RFOV); + inputs.rfovRed = LimelightHelpers.getBotPose3d_wpiRed(RFOV); + inputs.lfovBlue = LimelightHelpers.getBotPose3d_wpiBlue(LFOV); + inputs.lfovRed = LimelightHelpers.getBotPose3d_wpiRed(LFOV); + inputs.lLatency = LimelightHelpers.getLatency_Capture(LFOV) + LimelightHelpers.getLatency_Pipeline(LFOV); + inputs.rLatency = LimelightHelpers.getLatency_Capture(RFOV) + LimelightHelpers.getLatency_Pipeline(RFOV); + inputs.rNumTags = LimelightHelpers.getLatestResults(RFOV).targetingResults.targets_Fiducials.length; + inputs.lNumTags = LimelightHelpers.getLatestResults(LFOV).targetingResults.targets_Fiducials.length; + inputs.rValid = LimelightHelpers.getTV(RFOV); + inputs.lValid = LimelightHelpers.getTV(LFOV); } } From a83b5aecf43e56579b32e2eeb448a5b45b8143dc Mon Sep 17 00:00:00 2001 From: Jonathan Date: Mon, 12 Jun 2023 15:12:02 -0600 Subject: [PATCH 3/7] button is being deprecated by wpilib --- .../java/team3176/robot/RobotContainer.java | 191 ++++++------- .../subsystems/controller/Controller.java | 260 +----------------- 2 files changed, 104 insertions(+), 347 deletions(-) diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index b50ec68..c21dfd6 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -8,12 +8,14 @@ import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.Joystick; 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.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import team3176.robot.commands.*; import team3176.robot.commands.drivetrain.*; import team3176.robot.commands.superstructure.claw.ClawIdle; @@ -41,169 +43,170 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... - private final Arm m_Arm; - private final Controller m_Controller; - private final Claw m_Claw; - private final IntakeCube m_IntakeCube; - private final IntakeCone m_IntakeCone; - private PowerDistribution m_PDH; + private final Arm arm; + private final Controller controller; + private final Claw claw; + private final IntakeCube intakeCube; + private final IntakeCone intakeCone; + private PowerDistribution pdh; // is this why we don't have a compressor? private final Compressor m_Compressor - private final Drivetrain m_Drivetrain; - private final VisionDual m_Vision; - private final Superstructure m_Superstructure; - private SendableChooser m_autonChooser; - + private final Drivetrain drivetrain; + private final VisionDual vision; + private final Superstructure superstructure; + private SendableChooser autonChooser; + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the trigger bindings - m_Arm = Arm.getInstance(); - m_Controller = Controller.getInstance(); - m_Claw = Claw.getInstance(); - m_Drivetrain = Drivetrain.getInstance(); - m_IntakeCube = IntakeCube.getInstance(); - m_IntakeCone = IntakeCone.getInstance(); - m_PDH = new PowerDistribution(Hardwaremap.PDH_CID, ModuleType.kRev); - - m_Vision = VisionDual.getInstance(); - m_Superstructure = Superstructure.getInstance(); - m_Drivetrain.setDefaultCommand(new SwerveDrive( - m_Controller::getForward, - m_Controller::getStrafe, - m_Controller::getSpin)); - m_Arm.setDefaultCommand(m_Arm.armFineTune( () -> m_Controller.operator.getLeftY())); - m_autonChooser = new SendableChooser<>(); + arm = Arm.getInstance(); + controller = Controller.getInstance(); + claw = Claw.getInstance(); + drivetrain = Drivetrain.getInstance(); + intakeCube = IntakeCube.getInstance(); + intakeCone = IntakeCone.getInstance(); + pdh = new PowerDistribution(Hardwaremap.PDH_CID, ModuleType.kRev); + + vision = VisionDual.getInstance(); + superstructure = Superstructure.getInstance(); + drivetrain.setDefaultCommand(new SwerveDrive( + controller::getForward, + controller::getStrafe, + controller::getSpin)); + arm.setDefaultCommand(arm.armFineTune( () -> controller.operator.getLeftY())); + autonChooser = new SendableChooser<>(); File paths = new File(Filesystem.getDeployDirectory(), "pathplanner"); for (File f : paths.listFiles()) { if (!f.isDirectory()) { String s = f.getName().split("\\.", 0)[0]; - m_autonChooser.addOption(s, s); + autonChooser.addOption(s, s); } } - SmartDashboard.putData("Auton Choice", m_autonChooser); + SmartDashboard.putData("Auton Choice", autonChooser); configureBindings(); } private void configureBindings() { // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - m_Controller.getTransStick_Button1().whileTrue(m_Claw.scoreGamePiece()); + + controller.transStick.button(1).whileTrue(claw.scoreGamePiece()); //m_Controller.getTransStick_Button1().onFalse(new InstantCommand(() -> m_Drivetrain.setTurbo(false), m_Drivetrain)); - m_Controller.getTransStick_Button2().whileTrue(new IntakeGroundCube()); - m_Controller.getTransStick_Button2().onFalse(new IntakeRetractSpinot().andThen(m_Superstructure.prepareCarry())); - m_Controller.getTransStick_Button2().onFalse(m_Superstructure.prepareCarry()); - m_Controller.getTransStick_Button3().whileTrue(new SetColorWantState(3)); - m_Controller.getTransStick_Button3().whileTrue(m_Superstructure.groundCube()); - m_Controller.getTransStick_Button3().onFalse(new IntakeRetractSpinot()); - m_Controller.getTransStick_Button3().onFalse(m_Superstructure.prepareCarry()); - - m_Controller.getTransStick_Button4().whileTrue(m_Superstructure.prepareScoreHigh()); - m_Controller.getTransStick_Button4().onFalse((m_Superstructure.prepareCarry())); - m_Controller.getTransStick_Button5().onTrue(new InstantCommand(() -> m_Drivetrain.resetPoseToVision(),m_Drivetrain)); - m_Controller.getTransStick_Button10().whileTrue(new InstantCommand(() -> m_Drivetrain.setBrakeMode()).andThen(new SwerveDefense())); + controller.transStick.button(2).whileTrue(new IntakeGroundCube()); + controller.transStick.button(2).onFalse(new IntakeRetractSpinot().andThen(superstructure.prepareCarry())); + controller.transStick.button(2).onFalse(superstructure.prepareCarry()); + controller.transStick.button(3).whileTrue(new SetColorWantState(3)); + controller.transStick.button(3).whileTrue(superstructure.groundCube()); + controller.transStick.button(3).onFalse(new IntakeRetractSpinot()); + controller.transStick.button(3).onFalse(superstructure.prepareCarry()); + + controller.transStick.button(4).whileTrue(superstructure.prepareScoreHigh()); + controller.transStick.button(4).onFalse((superstructure.prepareCarry())); + controller.transStick.button(5).onTrue(new InstantCommand(drivetrain::resetPoseToVision,drivetrain)); + controller.transStick.button(10).whileTrue(new InstantCommand(drivetrain::setBrakeMode).andThen(new SwerveDefense())); //m_Controller.getTransStick_Button10() // .onFalse(new InstantCommand(() -> m_Drivetrain.setDriveMode(driveMode.DRIVE), m_Drivetrain)); // m_Controller.getRotStick_Button2().whileTrue(new FlipField); - m_Controller.getRotStick_Button1().whileTrue(new Turbo( - () -> m_Controller.getForward(), - () -> m_Controller.getStrafe(), - () -> m_Controller.getSpin()) - ); + controller.rotStick.button(1).whileTrue(new Turbo( + controller::getForward, + controller::getStrafe, + controller::getSpin + )); - m_Controller.getRotStick_Button2().whileTrue(new SpinLockDrive( - () -> m_Controller.getForward(), - () -> m_Controller.getStrafe()) + controller.rotStick.button(2).whileTrue(new SpinLockDrive( + controller::getForward, + controller::getStrafe) ); - m_Controller.getRotStick_Button3().whileTrue(new InstantCommand(() -> m_Drivetrain.setBrakeMode()).andThen(new SwerveDefense())); + controller.rotStick.button(3).whileTrue(new InstantCommand(drivetrain::setBrakeMode).andThen(new SwerveDefense())); - m_Controller.getRotStick_Button4().whileTrue(m_Superstructure.intakeCubeHumanPlayer()); - m_Controller.getRotStick_Button4().onFalse(m_Superstructure.prepareCarry()); - m_Controller.getTransStick_Button8() - .whileTrue(new InstantCommand(() -> m_Drivetrain.resetFieldOrientation(), m_Drivetrain)); + controller.rotStick.button(4).whileTrue(superstructure.intakeCubeHumanPlayer()); + controller.rotStick.button(4).onFalse(superstructure.prepareCarry()); + controller.rotStick.button(8) + .whileTrue(new InstantCommand(drivetrain::resetFieldOrientation, drivetrain)); double conveyorBumpTime = .1; //In units of seconds - m_Controller.operator.povUp().whileTrue(m_Superstructure.prepareScoreHigh()); - m_Controller.operator.povUp().onTrue(m_IntakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); - m_Controller.operator.povRight().whileTrue(m_Superstructure.prepareCarry()); - m_Controller.operator.povRight().onTrue(m_IntakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); - m_Controller.operator.povDown().whileTrue(m_Superstructure.prepareCatch()); - m_Controller.operator.povDown().onTrue(m_IntakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); - m_Controller.operator.povLeft().whileTrue(m_Superstructure.prepareScoreMid()); - m_Controller.operator.povLeft().onTrue(m_IntakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); + controller.operator.povUp().whileTrue(superstructure.prepareScoreHigh()); + controller.operator.povUp().onTrue(intakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); + controller.operator.povRight().whileTrue(superstructure.prepareCarry()); + controller.operator.povRight().onTrue(intakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); + controller.operator.povDown().whileTrue(superstructure.prepareCatch()); + controller.operator.povDown().onTrue(intakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); + controller.operator.povLeft().whileTrue(superstructure.prepareScoreMid()); + controller.operator.povLeft().onTrue(intakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); // m_Controller.operator.start().onTrue(new ToggleVisionLEDs()); // m_Controller.operator.back().onTrue(new SwitchToNextVisionPipeline()); - m_Controller.operator.b().onTrue(new SetColorWantState(1)); - m_Controller.operator.b().whileTrue(m_Superstructure.intakeConeHumanPlayer()); - m_Controller.operator.b().onFalse(m_Superstructure.prepareCarry()); + controller.operator.b().onTrue(new SetColorWantState(1)); + controller.operator.b().whileTrue(superstructure.intakeConeHumanPlayer()); + controller.operator.b().onFalse(superstructure.prepareCarry()); - m_Controller.operator.x().onTrue(new SetColorWantState(2)); - m_Controller.operator.x().whileTrue(m_Superstructure.intakeCubeHumanPlayer()); - m_Controller.operator.x().onFalse(m_Superstructure.prepareCarry()); + controller.operator.x().onTrue(new SetColorWantState(2)); + controller.operator.x().whileTrue(superstructure.intakeCubeHumanPlayer()); + controller.operator.x().onFalse(superstructure.prepareCarry()); - m_Controller.operator.a().onTrue(new SetColorWantState(3)); - m_Controller.operator.a().whileTrue(m_Superstructure.groundCube()); - m_Controller.operator.a().onFalse(new IntakeRetractSpinot()); - m_Controller.operator.a().onFalse(m_Superstructure.prepareCarry()); + controller.operator.a().onTrue(new SetColorWantState(3)); + controller.operator.a().whileTrue(superstructure.groundCube()); + controller.operator.a().onFalse(new IntakeRetractSpinot()); + controller.operator.a().onFalse(superstructure.prepareCarry()); - m_Controller.operator.y().onTrue(new SetColorWantState(0)); - m_Controller.operator.y().whileTrue(m_Claw.scoreGamePiece()); - m_Controller.operator.y().onFalse(new ClawIdle()); + controller.operator.y().onTrue(new SetColorWantState(0)); + controller.operator.y().whileTrue(claw.scoreGamePiece()); + controller.operator.y().onFalse(new ClawIdle()); - m_Controller.operator.rightBumper().and(m_Controller.operator.leftBumper().negate()).onTrue(new SetColorWantState(3)); - m_Controller.operator.rightBumper().and(m_Controller.operator.leftBumper().negate()).whileTrue(new IntakeGroundCube()); - m_Controller.operator.rightBumper().and(m_Controller.operator.leftBumper().negate()).onFalse(new IntakeRetractSpinot()); + controller.operator.rightBumper().and(controller.operator.leftBumper().negate()).onTrue(new SetColorWantState(3)); + controller.operator.rightBumper().and(controller.operator.leftBumper().negate()).whileTrue(new IntakeGroundCube()); + controller.operator.rightBumper().and(controller.operator.leftBumper().negate()).onFalse(new IntakeRetractSpinot()); //m_Controller.operator.rightBumper().and(m_Controller.operator.leftBumper().negate()).onFalse(m_Superstructure.prepareCarry()); - m_Controller.operator.leftBumper().and(m_Controller.operator.rightBumper()).whileTrue((new PoopCube())); + controller.operator.leftBumper().and(controller.operator.rightBumper()).whileTrue((new PoopCube())); // m_Controller.operator.leftBumper().and(m_Controller.operator.rightBumper()).onFalse(new InstantCommand( () -> m_IntakeCone.idle())); - m_Controller.operator.leftTrigger().onTrue(m_Arm.armSetPositionOnce(140).andThen(m_Arm.armFineTune( () -> m_Controller.operator.getLeftY()))); + controller.operator.leftTrigger().onTrue(arm.armSetPositionOnce(140).andThen(arm.armFineTune( () -> controller.operator.getLeftY()))); //m_Controller.operator.leftBumper().onTrue(m_Arm.armSetPositionOnce(200).andThen(m_Arm.armFineTune( () -> m_Controller.operator.getLeftY()))); //m_Controller.operator.leftBumper().onTrue(new ArmFollowTrajectory(SuperStructureConstants.ARM_MID_POS)); //m_Controller.operator.start().whileTrue(new OldPoopCube()); - m_Controller.operator.start().whileTrue(new InstantCommand( () -> m_IntakeCone.spit())); - m_Controller.operator.start().onFalse(new InstantCommand( () -> m_IntakeCone.idle())); + controller.operator.start().whileTrue(new InstantCommand(intakeCone::spit)); + controller.operator.start().onFalse(new InstantCommand(intakeCone::idle)); //m_Controller.operator.start().onFalse(new IntakeRetractSpinot().andThen(m_Superstructure.prepareCarry())); - m_Controller.operator.back().whileTrue(m_Superstructure.preparePoop()); + controller.operator.back().whileTrue(superstructure.preparePoop()); //m_Controller.operator.leftTrigger().whileTrue(new PoopCube()); - m_Controller.operator.rightTrigger().whileTrue(m_Superstructure.preparePoop()); + controller.operator.rightTrigger().whileTrue(superstructure.preparePoop()); } public void setArmCoast() { - m_Arm.setCoastMode(); + arm.setCoastMode(); } public void setArmBrake() { - m_Arm.setBrakeMode(); + arm.setBrakeMode(); } public void setThrustCoast() { - m_Drivetrain.setCoastMode(); + drivetrain.setCoastMode(); } public void setThrustBrake() { - m_Drivetrain.setBrakeMode(); + drivetrain.setBrakeMode(); } public void clearCanFaults(){ - m_PDH.clearStickyFaults(); + pdh.clearStickyFaults(); } public void printCanFaults(){ - m_PDH.getStickyFaults(); + pdh.getStickyFaults(); } /** @@ -213,10 +216,10 @@ public void printCanFaults(){ */ public Command getAutonomousCommand() { // An example command will be run in autonomous - String chosen = m_autonChooser.getSelected(); + String chosen = autonChooser.getSelected(); //String chosen = "wall_cone_exit_balance"; - PathPlannerAuto PPSwerveauto = new PathPlannerAuto(chosen); - return PPSwerveauto.getauto(); + PathPlannerAuto ppSwerveAuto = new PathPlannerAuto(chosen); + return ppSwerveAuto.getauto(); } } diff --git a/src/main/java/team3176/robot/subsystems/controller/Controller.java b/src/main/java/team3176/robot/subsystems/controller/Controller.java index 49b8faa..e51bd99 100644 --- a/src/main/java/team3176/robot/subsystems/controller/Controller.java +++ b/src/main/java/team3176/robot/subsystems/controller/Controller.java @@ -4,16 +4,10 @@ package team3176.robot.subsystems.controller; -import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.POVButton; -import edu.wpi.first.wpilibj2.command.button.Trigger; import team3176.robot.constants.ControllerConstants; // import team3176.robot.util.XboxController.XboxAxisAsButton; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.XboxController.Axis; -import edu.wpi.first.wpilibj.XboxController.Button; // import team3176.robot.util.XboxController.*; public class Controller { @@ -26,187 +20,20 @@ public static Controller getInstance() { return instance;} /* The Three Physical Controllers that we have */ - - private final Joystick transStick; - private final Joystick rotStick; - public CommandXboxController operator; + + public final CommandJoystick transStick; + public final CommandJoystick rotStick; + public final CommandXboxController operator; /* First Part of Creating the Buttons on the Joysticks */ - private final JoystickButton transStick_Button1; - private final JoystickButton transStick_Button2; - private final JoystickButton transStick_Button3; - private final JoystickButton transStick_Button4; - private final JoystickButton transStick_Button5; - private final JoystickButton transStick_Button6; - private final JoystickButton transStick_Button7; - private final JoystickButton transStick_Button8; - private final JoystickButton transStick_Button9; - private final JoystickButton transStick_Button10; - private final JoystickButton transStick_Button11; - private final JoystickButton transStick_Button12; - private final JoystickButton transStick_Button13; - private final JoystickButton transStick_Button14; - private final JoystickButton transStick_Button15; - private final JoystickButton transStick_Button16; - private final POVButton transStick_HAT_0; - private final POVButton transStick_HAT_45; - private final POVButton transStick_HAT_90; - private final POVButton transStick_HAT_135; - private final POVButton transStick_HAT_180; - private final POVButton transStick_HAT_225; - private final POVButton transStick_HAT_270; - private final POVButton transStick_HAT_315; - - - private final JoystickButton rotStick_Button1; - private final JoystickButton rotStick_Button2; - private final JoystickButton rotStick_Button3; - private final JoystickButton rotStick_Button4; - private final JoystickButton rotStick_Button5; - private final JoystickButton rotStick_Button6; - private final JoystickButton rotStick_Button7; - private final JoystickButton rotStick_Button8; - private final JoystickButton rotStick_Button9; - private final JoystickButton rotStick_Button10; - private final JoystickButton rotStick_Button11; - private final JoystickButton rotStick_Button12; - private final JoystickButton rotStick_Button13; - private final JoystickButton rotStick_Button14; - private final JoystickButton rotStick_Button15; - private final JoystickButton rotStick_Button16; - private final POVButton rotStick_HAT_0; - private final POVButton rotStick_HAT_45; - private final POVButton rotStick_HAT_90; - private final POVButton rotStick_HAT_135; - private final POVButton rotStick_HAT_180; - private final POVButton rotStick_HAT_225; - private final POVButton rotStick_HAT_270; - private final POVButton rotStick_HAT_315; - - //TODO: Add slider - - // private final Trigger op_A; - // private final Trigger op_A_Shift; - // private final Trigger op_A_Double_Shift; - // private final Trigger op_B; - // private final Trigger op_B_Shift; - // private final Trigger op_B_Double_Shift; - // private final Trigger op_X; - // private final Trigger op_X_Shift; - // private final Trigger op_X_Double_Shift; - // private final Trigger op_Y; - // private final Trigger op_Y_Shift; - // private final Trigger op_Y_Double_Shift; - // private final Trigger op_Start; - // private final Trigger op_Start_Shift; - // private final Trigger op_Start_Double_Shift; - // private final Trigger op_Back; - // private final Trigger op_Back_Shift; - // private final Trigger op_Back_Double_Shift; - // private final Trigger op_LTrigger; - // private final Trigger op_RTrigger; - // private final POVButton op_DPAD_Up; - // private final POVButton op_DPAD_Left; - // private final POVButton op_DPAD_Down; - // private final POVButton op_DPAD_Right; - public Controller() { /* Finish Creating the Objects */ - transStick = new Joystick(ControllerConstants.TRANS_ID); - rotStick = new Joystick(ControllerConstants.ROT_ID); + transStick = new CommandJoystick(ControllerConstants.TRANS_ID); + rotStick = new CommandJoystick(ControllerConstants.ROT_ID); operator = new CommandXboxController(ControllerConstants.OP_ID); - transStick_Button1 = new JoystickButton(transStick, 1); - transStick_Button2 = new JoystickButton(transStick, 2); - transStick_Button3 = new JoystickButton(transStick, 3); - transStick_Button4 = new JoystickButton(transStick, 4); - transStick_Button5 = new JoystickButton(transStick, 5); - transStick_Button6 = new JoystickButton(transStick, 6); - transStick_Button7 = new JoystickButton(transStick, 7); - transStick_Button8 = new JoystickButton(transStick, 8); - transStick_Button9 = new JoystickButton(transStick, 9); - transStick_Button10 = new JoystickButton(transStick, 10); - transStick_Button11 = new JoystickButton(transStick, 11); - transStick_Button12 = new JoystickButton(transStick, 12); - transStick_Button13 = new JoystickButton(transStick, 13); - transStick_Button14 = new JoystickButton(transStick, 14); - transStick_Button15 = new JoystickButton(transStick, 15); - transStick_Button16 = new JoystickButton(transStick, 16); - - /** - * The HAT on the transStick - * The values are 0 as UP going in a 360 circle CCW - */ - transStick_HAT_0 = new POVButton(transStick, 0); - transStick_HAT_45 = new POVButton(transStick, 45); - transStick_HAT_90 = new POVButton(transStick, 90); - transStick_HAT_135 = new POVButton(transStick, 135); - transStick_HAT_180 = new POVButton(transStick, 180); - transStick_HAT_225 = new POVButton(transStick, 225); - transStick_HAT_270 = new POVButton(transStick, 270); - transStick_HAT_315 = new POVButton(transStick, 315); - - - - rotStick_Button1 = new JoystickButton(rotStick, 1); - rotStick_Button2 = new JoystickButton(rotStick, 2); - rotStick_Button3 = new JoystickButton(rotStick, 3); - rotStick_Button4 = new JoystickButton(rotStick, 4); - rotStick_Button5 = new JoystickButton(rotStick, 5); - rotStick_Button6 = new JoystickButton(rotStick, 6); - rotStick_Button7 = new JoystickButton(rotStick, 7); - rotStick_Button8 = new JoystickButton(rotStick, 8); - rotStick_Button9 = new JoystickButton(rotStick, 9); - rotStick_Button10 = new JoystickButton(rotStick, 10); - rotStick_Button11 = new JoystickButton(rotStick, 11); - rotStick_Button12 = new JoystickButton(rotStick, 12); - rotStick_Button13 = new JoystickButton(rotStick, 13); - rotStick_Button14 = new JoystickButton(rotStick, 14); - rotStick_Button15 = new JoystickButton(rotStick, 15); - rotStick_Button16 = new JoystickButton(rotStick, 16); - - /** - * The HAT on the rotStick - * The values are 0 as UP going in a 360 circle CCW - */ - rotStick_HAT_0 = new POVButton(rotStick, 0); - rotStick_HAT_45 = new POVButton(rotStick, 45); - rotStick_HAT_90 = new POVButton(rotStick, 90); - rotStick_HAT_135 = new POVButton(rotStick, 135); - rotStick_HAT_180 = new POVButton(rotStick, 180); - rotStick_HAT_225 = new POVButton(rotStick, 225); - rotStick_HAT_270 = new POVButton(rotStick, 270); - rotStick_HAT_315= new POVButton(rotStick, 315); - - /* - * The Xbox Controller Buttons - * XboxMain: The first level of control; NAME + NOT FIRST SHIFT + NOT SECOND SHIFT - * XboxShift: The second level of control; The first shift; NAME + First SHIFT + NOT SECOND SHIFT - * XboxDBLShift: The third level of control; The second shift; NAME + FIRST SHIFT + SECOND SHIFT - */ - - // op_A = new XboxMain(operator, Button.kA.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_A_Shift = new XboxShift(operator, Button.kA.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_A_Double_Shift = new XboxDBLShift(operator, Button.kA.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_B = new XboxMain(operator, Button.kB.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_B_Shift = new XboxShift(operator, Button.kB.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_B_Double_Shift = new XboxDBLShift(operator, Button.kB.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_X = new XboxMain(operator, Button.kX.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_X_Shift = new XboxShift(operator, Button.kX.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_X_Double_Shift = new XboxDBLShift(operator, Button.kX.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_Y = new XboxMain(operator, Button.kY.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_Y_Shift = new XboxShift(operator, Button.kY.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_Y_Double_Shift = new XboxDBLShift(operator, Button.kY.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_Start = new XboxMain(operator, Button.kStart.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_Start_Shift = new XboxShift(operator, Button.kStart.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_Start_Double_Shift = new XboxDBLShift(operator, Button.kStart.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_Back = new XboxMain(operator, Button.kBack.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_Back_Shift = new XboxShift(operator, Button.kBack.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_Back_Double_Shift = new XboxDBLShift(operator, Button.kBack.value, Button.kLeftBumper.value, Button.kRightBumper.value); - // op_LTrigger = new XboxAxisAsButton(operator, Axis.kLeftTrigger.value, ControllerConstants.TRIGGER_THRESHOLD); //TODO: CHANGE THRESHOLD - // op_RTrigger = new XboxAxisAsButton(operator, Axis.kRightTrigger.value, ControllerConstants.TRIGGER_THRESHOLD); @@ -361,24 +188,6 @@ public double getOrbitSpeed() { //TODO: FIND IF WE NEED return Math.pow(rotStick.getY(), 1); } - /** - * Scale is the power of 1 - * @return The position of the POV on TransStick (The mini-joystick on top) - */ - - public int getTransStickPOV() { - return transStick.getPOV(); - } - - /** - * Scale is the power of 1 - * @return The position of the POV on RotStick (The mini-joystick on top) - */ - - public int getRotStickPOV() { - return rotStick.getPOV(); - } - /** * Scale is the power of 1 * @return The value of the y axis of the left joystick of the Xbox Controller @@ -419,61 +228,6 @@ public double getOp_RightX() { return Math.pow(operator.getRightX(), 1); } - /* Returns the object of the named button */ - - public JoystickButton getTransStick_Button1() {return transStick_Button1;} - public JoystickButton getTransStick_Button2() {return transStick_Button2;} - public JoystickButton getTransStick_Button3() {return transStick_Button3;} - public JoystickButton getTransStick_Button4() {return transStick_Button4;} - public JoystickButton getTransStick_Button5() {return transStick_Button5;} - public JoystickButton getTransStick_Button6() {return transStick_Button6;} - public JoystickButton getTransStick_Button7() {return transStick_Button7;} - public JoystickButton getTransStick_Button8() {return transStick_Button8;} - public JoystickButton getTransStick_Button9() {return transStick_Button9;} - public JoystickButton getTransStick_Button10() {return transStick_Button10;} - public JoystickButton getTransStick_Button11() {return transStick_Button11;} - public JoystickButton getTransStick_Button12() {return transStick_Button12;} - public JoystickButton getTransStick_Button13() {return transStick_Button13;} - public JoystickButton getTransStick_Button14() {return transStick_Button14;} - public JoystickButton getTransStick_Button15() {return transStick_Button15;} - public JoystickButton getTransStick_Button16() {return transStick_Button16;} - - public POVButton getTransStick_HAT_0() {return transStick_HAT_0;} - public POVButton getTransStick_HAT_45() {return transStick_HAT_45;} - public POVButton getTransStick_HAT_90() {return transStick_HAT_90;} - public POVButton getTransStick_HAT_135() {return transStick_HAT_135;} - public POVButton getTransStick_HAT_180() {return transStick_HAT_180;} - public POVButton getTransStick_HAT_225() {return transStick_HAT_225;} - public POVButton getTransStick_HAT_270() {return transStick_HAT_270;} - public POVButton getTransStick_HAT_315() {return transStick_HAT_315;} - - - - public JoystickButton getRotStick_Button1() {return rotStick_Button1;} - public JoystickButton getRotStick_Button2() {return rotStick_Button2;} - public JoystickButton getRotStick_Button3() {return rotStick_Button3;} - public JoystickButton getRotStick_Button4() {return rotStick_Button4;} - public JoystickButton getRotStick_Button5() {return rotStick_Button5;} - public JoystickButton getRotStick_Button6() {return rotStick_Button6;} - public JoystickButton getRotStick_Button7() {return rotStick_Button7;} - public JoystickButton getRotStick_Button8() {return rotStick_Button8;} - public JoystickButton getRotStick_Button9() {return rotStick_Button9;} - public JoystickButton getRotStick_Button10() {return rotStick_Button10;} - public JoystickButton getRotStick_Button11() {return rotStick_Button11;} - public JoystickButton getRotStick_Button12() {return rotStick_Button12;} - public JoystickButton getRotStick_Button13() {return rotStick_Button13;} - public JoystickButton getRotStick_Button14() {return rotStick_Button14;} - public JoystickButton getRotStick_Button15() {return rotStick_Button15;} - public JoystickButton getRotStick_Button16() {return rotStick_Button16;} - - public POVButton getRotStick_HAT_0() {return rotStick_HAT_0;} - public POVButton getRotStick_HAT_45() {return rotStick_HAT_45;} - public POVButton getRotStick_HAT_90() {return rotStick_HAT_90;} - public POVButton getRotStick_HAT_135() {return rotStick_HAT_135;} - public POVButton getRotStick_HAT_180() {return rotStick_HAT_180;} - public POVButton getRotStick_HAT_225() {return rotStick_HAT_225;} - public POVButton getRotStick_HAT_270() {return rotStick_HAT_270;} - public POVButton getRotStick_HAT_315() {return rotStick_HAT_315;} // public Trigger getOp_A() {return op_A;} From 21006a331f574aa9e3e8a9de97c4c8c0b66db07f Mon Sep 17 00:00:00 2001 From: Jonathan Date: Mon, 12 Jun 2023 15:42:45 -0600 Subject: [PATCH 4/7] Command cleaning --- .../commands/drivetrain/SpinLockDrive.java | 17 ++++++----- .../constants/DrivetrainHardwareMap.java | 28 +++++++++---------- 2 files changed, 22 insertions(+), 23 deletions(-) diff --git a/src/main/java/team3176/robot/commands/drivetrain/SpinLockDrive.java b/src/main/java/team3176/robot/commands/drivetrain/SpinLockDrive.java index 82ea8fc..b9763cf 100644 --- a/src/main/java/team3176/robot/commands/drivetrain/SpinLockDrive.java +++ b/src/main/java/team3176/robot/commands/drivetrain/SpinLockDrive.java @@ -2,7 +2,6 @@ import java.util.function.DoubleSupplier; -import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj2.command.CommandBase; import team3176.robot.constants.DrivetrainConstants; import team3176.robot.subsystems.drivetrain.Drivetrain; @@ -11,7 +10,7 @@ import edu.wpi.first.math.MathUtil; public class SpinLockDrive extends CommandBase { - private Drivetrain m_Drivetrain = Drivetrain.getInstance(); + private Drivetrain drivetrain = Drivetrain.getInstance(); private PIDController wController = new PIDController(1.0, 0.0, 0.0); @@ -23,14 +22,14 @@ public SpinLockDrive( DoubleSupplier forwardCommand, DoubleSupplier strafeComman this.forwardCommand = forwardCommand; this.strafeCommand = strafeCommand; - addRequirements(m_Drivetrain); + addRequirements(drivetrain); } @Override public void initialize() { - m_Drivetrain.setDriveMode(driveMode.DRIVE); - m_Drivetrain.setSpinLock(true); - this.spinLockAngle = m_Drivetrain.getPoseYawWrapped().getDegrees(); + drivetrain.setDriveMode(driveMode.DRIVE); + drivetrain.setSpinLock(true); + this.spinLockAngle = drivetrain.getPoseYawWrapped().getDegrees(); //drivetrain.setCoastMode(); } @@ -38,9 +37,9 @@ public void initialize() { @Override public void execute() { - m_Drivetrain.drive(forwardCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND *0.7, + drivetrain.drive(forwardCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND *0.7, strafeCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND *0.7, - MathUtil.clamp(wController.calculate(m_Drivetrain.getPoseYawWrapped().getDegrees(), this.spinLockAngle), -2, 2)); + MathUtil.clamp(wController.calculate(drivetrain.getPoseYawWrapped().getDegrees(), this.spinLockAngle), -2, 2)); //spinCommand.getAsDouble()*100); } @@ -49,6 +48,6 @@ public void execute() { @Override public void end(boolean interrupted) { - m_Drivetrain.setSpinLock(false); + drivetrain.setSpinLock(false); } } \ No newline at end of file diff --git a/src/main/java/team3176/robot/constants/DrivetrainHardwareMap.java b/src/main/java/team3176/robot/constants/DrivetrainHardwareMap.java index 8174699..aec72dd 100644 --- a/src/main/java/team3176/robot/constants/DrivetrainHardwareMap.java +++ b/src/main/java/team3176/robot/constants/DrivetrainHardwareMap.java @@ -3,30 +3,30 @@ import team3176.robot.constants.SwervePodHardwareID; public class DrivetrainHardwareMap { //statics constants for swerve pods - public static SwervePodHardwareID pod001 = + public static final SwervePodHardwareID POD001 = new SwervePodHardwareID( 10, 12, -172.135); - public static SwervePodHardwareID pod002 = + public static final SwervePodHardwareID POD002 = new SwervePodHardwareID( 20, 22, -225.186); - public static SwervePodHardwareID pod003 = + public static final SwervePodHardwareID POD003 = new SwervePodHardwareID( 30, 32, -40); - public static SwervePodHardwareID pod004 = + public static final SwervePodHardwareID POD004 = new SwervePodHardwareID( 40, 42, 140.463); //120.5 - public static SwervePodHardwareID pod005 = + public static final SwervePodHardwareID POD005 = new SwervePodHardwareID( 13, 14, -30.525); - public static SwervePodHardwareID pod006 = + public static final SwervePodHardwareID POD006 = new SwervePodHardwareID( 23, 24, 120.556); - public static SwervePodHardwareID pod007 = + public static final SwervePodHardwareID POD007 = new SwervePodHardwareID( 33, 34, 125.508); - public static SwervePodHardwareID pod008 = + public static final SwervePodHardwareID POD008 = new SwervePodHardwareID( 43, 44, -173); - public static SwervePodHardwareID pod009 = + public static final SwervePodHardwareID POD009 = new SwervePodHardwareID( 15, 16, -358.330); - public static SwervePodHardwareID FR = pod009; - public static SwervePodHardwareID FL = pod008; - public static SwervePodHardwareID BL = pod006; - public static SwervePodHardwareID BR = pod003; + public static final SwervePodHardwareID FR = POD009; + public static final SwervePodHardwareID FL = POD008; + public static final SwervePodHardwareID BL = POD006; + public static final SwervePodHardwareID BR = POD003; // public static final int THRUST_FR_CID = FR.THRUST_CID; // public static final int THRUST_FL_CID = FL.THRUST_CID; @@ -36,7 +36,7 @@ public class DrivetrainHardwareMap { public static final int[] STEER_CANCODER_CID = //{12, 22, 32, 42}; - {(int) FR.CANCODER_CID, (int) FL.CANCODER_CID, (int) BL.CANCODER_CID, (int) BR.CANCODER_CID}; + {FR.CANCODER_CID, FL.CANCODER_CID, BL.CANCODER_CID, BR.CANCODER_CID}; //The swerve pod offset is measured when the swerve pod is in the front right position and the wheel gear is facing the right From d43e01ae39ba8da12aa3b68bc40cfb995779ba0a Mon Sep 17 00:00:00 2001 From: Jonathan Date: Mon, 12 Jun 2023 15:42:56 -0600 Subject: [PATCH 5/7] command cleaning 2 --- src/main/java/team3176/robot/Constants.java | 2 +- .../commands/drivetrain/AutoBalance.java | 25 +++-- .../robot/commands/drivetrain/FeederPID.java | 35 +++--- .../commands/drivetrain/FeederPID3D.java | 43 ++++---- .../commands/drivetrain/PathPlannerAuto.java | 18 +--- .../commands/drivetrain/SwerveDrive.java | 5 - .../{teleopPath.java => TeleopPath.java} | 32 ++---- .../robot/commands/drivetrain/Turbo.java | 4 - .../commands/drivetrain/TurtleSpeed.java | 3 - .../commands/superstructure/OldPoopCube.java | 86 --------------- .../{armAnalogUp.java => ArmAnalogDown.java} | 25 ++--- ...{armAnalogIdle.java => ArmAnalogIdle.java} | 27 +---- .../{armAnalogDown.java => ArmAnalogUp.java} | 28 +---- .../arm/ArmFollowTrajectory.java | 13 +-- ...itionArm.java => ManuallyPositionArm.java} | 28 ++--- .../superstructure/autoScoreConeHigh.java | 70 ------------ .../superstructure/claw/ClawIdle.java | 22 +--- .../robot/constants/DrivetrainConstants.java | 6 -- .../team3176/robot/util/God/Math3176.java | 9 -- .../java/team3176/robot/util/God/PID3176.java | 100 ------------------ .../robot/util/LoggedTunableNumber.java | 8 +- 21 files changed, 102 insertions(+), 487 deletions(-) rename src/main/java/team3176/robot/commands/drivetrain/{teleopPath.java => TeleopPath.java} (58%) delete mode 100644 src/main/java/team3176/robot/commands/superstructure/OldPoopCube.java rename src/main/java/team3176/robot/commands/superstructure/arm/{armAnalogUp.java => ArmAnalogDown.java} (63%) rename src/main/java/team3176/robot/commands/superstructure/arm/{armAnalogIdle.java => ArmAnalogIdle.java} (51%) rename src/main/java/team3176/robot/commands/superstructure/arm/{armAnalogDown.java => ArmAnalogUp.java} (52%) rename src/main/java/team3176/robot/commands/superstructure/arm/{manuallyPositionArm.java => ManuallyPositionArm.java} (67%) delete mode 100644 src/main/java/team3176/robot/commands/superstructure/autoScoreConeHigh.java delete mode 100644 src/main/java/team3176/robot/util/God/Math3176.java delete mode 100644 src/main/java/team3176/robot/util/God/PID3176.java diff --git a/src/main/java/team3176/robot/Constants.java b/src/main/java/team3176/robot/Constants.java index a1be3e2..f255841 100644 --- a/src/main/java/team3176/robot/Constants.java +++ b/src/main/java/team3176/robot/Constants.java @@ -13,7 +13,7 @@ public final class Constants { private static final RobotType robot = RobotType.ROBOT_SIMBOT; public static final double LOOP_PERIODIC_SECS = 0.02; - public static final boolean TUNING_MODE = false; + public static final boolean TUNING_MODE = true; public static boolean invalidRobotAlertSent = false; diff --git a/src/main/java/team3176/robot/commands/drivetrain/AutoBalance.java b/src/main/java/team3176/robot/commands/drivetrain/AutoBalance.java index 06f5ede..41b314d 100644 --- a/src/main/java/team3176/robot/commands/drivetrain/AutoBalance.java +++ b/src/main/java/team3176/robot/commands/drivetrain/AutoBalance.java @@ -8,17 +8,16 @@ import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; public class AutoBalance extends CommandBase { - private Drivetrain m_Drivetrain; - private boolean isDone = false; - private int num_balanced = 0; + private Drivetrain drivetrain; + //private boolean isDone = false; + //private int numBalanced = 0; public AutoBalance() { - m_Drivetrain = Drivetrain.getInstance(); - addRequirements(m_Drivetrain); + drivetrain = Drivetrain.getInstance(); + addRequirements(drivetrain); } @Override public void initialize() { - // TODO Auto-generated method stub - m_Drivetrain.setBrakeMode(); + drivetrain.setBrakeMode(); } @Override @@ -26,9 +25,9 @@ public void execute() { //double Kp = 0.1; //Bang Bang controller! - double forward = 0.0; + double forward; double deadbandDegrees = 8; - SmartDashboard.putNumber("pitch", m_Drivetrain.getChassisPitch()); + SmartDashboard.putNumber("pitch", drivetrain.getChassisPitch()); // if(m_Drivetrain.getChassisPitch() > 0 + deadbandDegrees) { // forward = 0.37 * Math.pow(.96,num_balanced); // } else if(m_Drivetrain.getChassisPitch() < 0 - deadbandDegrees) { @@ -37,19 +36,19 @@ public void execute() { // num_balanced ++; // } //P loop option - if(Math.abs(m_Drivetrain.getChassisPitch()) > 0 + deadbandDegrees) { - forward = 0.03 * m_Drivetrain.getChassisPitch(); + if(Math.abs(drivetrain.getChassisPitch()) > 0 + deadbandDegrees) { + forward = 0.03 * drivetrain.getChassisPitch(); forward = MathUtil.clamp(forward, -0.5, 0.5); } else { forward = 0.0; } - m_Drivetrain.drive(forward, 0, 0, Drivetrain.coordType.ROBOT_CENTRIC); + drivetrain.drive(forward, 0, 0, Drivetrain.coordType.ROBOT_CENTRIC); } @Override public void end(boolean interrupted) { - m_Drivetrain.setDriveMode(driveMode.DEFENSE); + drivetrain.setDriveMode(driveMode.DEFENSE); } @Override public boolean isFinished() { diff --git a/src/main/java/team3176/robot/commands/drivetrain/FeederPID.java b/src/main/java/team3176/robot/commands/drivetrain/FeederPID.java index 93757cc..c9c34ca 100644 --- a/src/main/java/team3176/robot/commands/drivetrain/FeederPID.java +++ b/src/main/java/team3176/robot/commands/drivetrain/FeederPID.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.util.InterpolatingTreeMap; @@ -13,11 +12,9 @@ import team3176.robot.subsystems.drivetrain.Drivetrain; -import team3176.robot.subsystems.drivetrain.Drivetrain.coordType; -import team3176.robot.subsystems.superstructure.Superstructure.GamePiece; public class FeederPID extends CommandBase{ - Drivetrain m_Drivetrain; + Drivetrain drivetrain; PIDController xController = new PIDController(2.0,0.0,0.0); PIDController yController = new PIDController(.5,0.0,0.0); PIDController wController = new PIDController(.5,0.0,0.0); @@ -33,8 +30,8 @@ public class FeederPID extends CommandBase{ public FeederPID(String side) { this.side = side; alliance = DriverStation.getAlliance(); - m_Drivetrain = Drivetrain.getInstance(); - addRequirements(m_Drivetrain); + drivetrain = Drivetrain.getInstance(); + addRequirements(drivetrain); //vision = NetworkTableInstance.getDefault().getTable("limelight"); limelight_lfov = NetworkTableInstance.getDefault().getTable("limelight-lfov"); limelight_rfov = NetworkTableInstance.getDefault().getTable("limelight-rfov"); @@ -49,7 +46,7 @@ public FeederPID(String side) { offsetTreeL.put(0.19,2.0); offsetTreeL.put(0.0,0.0); - if(side == "right") { + if(side.equals("right")) { offsetTree = offsetTreeR; } else { offsetTree = offsetTreeL; @@ -61,7 +58,7 @@ public void initialize(){ deadband = 1; txSetpoint = 0.0; - if (ta > 1.1 );{ + if (ta > 1.1 ){ if(side == "right") { txSetpoint = 0 ; //-20; } else { @@ -69,13 +66,13 @@ public void initialize(){ } } - m_Drivetrain.setSpinLock(true); - if (alliance == Alliance.Red) { + drivetrain.setSpinLock(true); + if(alliance == Alliance.Red) { wSetpoint = 0; - m_Drivetrain.setSpinLockAngle(wSetpoint); + drivetrain.setSpinLockAngle(wSetpoint); } else { wSetpoint = 0; - m_Drivetrain.setSpinLockAngle(wSetpoint); + drivetrain.setSpinLockAngle(wSetpoint); }; } @@ -115,17 +112,17 @@ public void execute() { SmartDashboard.putNumber("ty", ty); SmartDashboard.putNumber("ta", ta); SmartDashboard.putNumber("tv", tv); - SmartDashboard.putNumber("yawWrapped", m_Drivetrain.getPoseYawWrapped().getDegrees()); + SmartDashboard.putNumber("yawWrapped", drivetrain.getPoseYawWrapped().getDegrees()); //if (Math.abs(m_Drivetrain.getPoseYawWrapped().getDegrees()) > 0 && tv != 0.0) { // m_Drivetrain.drive (MathUtil.clamp(xController.calculate(ta, 1.5),-1.5,1.5), // (MathUtil.clamp(yController.calculate(tx,txSetpoint),-1.5,1.5)), // 0.0, coordType.ROBOT_CENTRIC); - if ((tx < (txSetpoint-deadband) || (tx > (txSetpoint+deadband)))) { - m_Drivetrain.drive(0, - (MathUtil.clamp(-1 * yController.calculate(tx,txSetpoint), -1.5, 1.5)), - (MathUtil.clamp(-1 * wController.calculate(m_Drivetrain.getPoseYawWrapped().getDegrees(), wSetpoint), -1.5, 1.5))); - } + if ((tx < (txSetpoint-deadband) || (tx > (txSetpoint+deadband)))) { + drivetrain.drive(0, + (MathUtil.clamp(-1 * yController.calculate(tx,txSetpoint), -1.5, 1.5)), + (MathUtil.clamp(-1 * wController.calculate(drivetrain.getPoseYawWrapped().getDegrees(), wSetpoint), -1.5, 1.5))); + } //} else m_Drivetrain.drive (Math.pow(10,-7),Math.pow(10,-7),Math.pow(10,-7)); } @@ -138,6 +135,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - m_Drivetrain.setSpinLock(false); + drivetrain.setSpinLock(false); } } diff --git a/src/main/java/team3176/robot/commands/drivetrain/FeederPID3D.java b/src/main/java/team3176/robot/commands/drivetrain/FeederPID3D.java index 7fa0ad8..eee98be 100644 --- a/src/main/java/team3176/robot/commands/drivetrain/FeederPID3D.java +++ b/src/main/java/team3176/robot/commands/drivetrain/FeederPID3D.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -15,13 +14,13 @@ import team3176.robot.subsystems.drivetrain.Drivetrain; public class FeederPID3D extends CommandBase{ - Drivetrain m_Drivetrain; + Drivetrain drivetrain; PIDController xController = new PIDController(2.0,0.0,0.0); PIDController yController = new PIDController(2.0,0.0,0.0); - Pose2d RedRight = new Pose2d(1.17, 7.44, Rotation2d.fromDegrees(180)); - Pose2d RedLeft = new Pose2d(1.17, 6.17, Rotation2d.fromDegrees(180)); - Pose2d BlueRight = new Pose2d(15.44, 6.17, Rotation2d.fromDegrees(0.0)); - Pose2d BlueLeft = new Pose2d(15.44, 7.44, Rotation2d.fromDegrees(0.0)); + Pose2d redRight = new Pose2d(1.17, 7.44, Rotation2d.fromDegrees(180)); + Pose2d redLeft = new Pose2d(1.17, 6.17, Rotation2d.fromDegrees(180)); + Pose2d blueRight = new Pose2d(15.44, 6.17, Rotation2d.fromDegrees(0.0)); + Pose2d blueLeft = new Pose2d(15.44, 7.44, Rotation2d.fromDegrees(0.0)); Pose2d targetPose; NetworkTable vision; Alliance alliance; @@ -29,40 +28,40 @@ public class FeederPID3D extends CommandBase{ public FeederPID3D(String side) { this.side = side; alliance = DriverStation.getAlliance(); - m_Drivetrain = Drivetrain.getInstance(); - addRequirements(m_Drivetrain); + drivetrain = Drivetrain.getInstance(); + addRequirements(drivetrain); vision = NetworkTableInstance.getDefault().getTable("limelight"); - if(side == "right") { + if(side.equals("right")) { if(DriverStation.getAlliance() == Alliance.Red) { - targetPose = RedRight; + targetPose = redRight; } else { - targetPose = BlueRight; + targetPose = blueRight; } } else { if(DriverStation.getAlliance() == Alliance.Red) { - targetPose = RedLeft; + targetPose = redLeft; } else { - targetPose = BlueLeft; + targetPose = blueLeft; } } } @Override public void initialize(){ - m_Drivetrain.setSpinLock(true); - m_Drivetrain.setSpinLockAngle(targetPose.getRotation().getDegrees()); + drivetrain.setSpinLock(true); + drivetrain.setSpinLockAngle(targetPose.getRotation().getDegrees()); } @Override public void execute() { - double[] default_pose = {0.0,0.0,0.0,0.0,0.0,0.0}; - double[] vision_pose_array = vision.getEntry("botpose_wpiblue").getDoubleArray(default_pose); - Pose2d cam_pose = new Pose2d(vision_pose_array[0],vision_pose_array[1],Rotation2d.fromDegrees(vision_pose_array[5])); + double[] defaultPose = {0.0,0.0,0.0,0.0,0.0,0.0}; + double[] visionPoseArray = vision.getEntry("botpose_wpiblue").getDoubleArray(defaultPose); + Pose2d camPose = new Pose2d(visionPoseArray[0],visionPoseArray[1],Rotation2d.fromDegrees(visionPoseArray[5])); double tv = vision.getEntry("tv").getDouble(0.0); double reverseAxis = DriverStation.getAlliance() == Alliance.Red ? -1.0 : 1.0; if (tv != 0.0) { - m_Drivetrain.drive(MathUtil.clamp(reverseAxis*xController.calculate(cam_pose.getX(), targetPose.getX()),-1.5,1.5), - (MathUtil.clamp(reverseAxis*yController.calculate(cam_pose.getY(),targetPose.getY()),-1.5,1.5)), + drivetrain.drive(MathUtil.clamp(reverseAxis*xController.calculate(camPose.getX(), targetPose.getX()),-1.5,1.5), + (MathUtil.clamp(reverseAxis*yController.calculate(camPose.getY(),targetPose.getY()),-1.5,1.5)), 0.0); - } else m_Drivetrain.drive (Math.pow(10,-7),Math.pow(10,-7),Math.pow(10,-7)); + } else drivetrain.drive (Math.pow(10,-7),Math.pow(10,-7),Math.pow(10,-7)); } @Override public boolean isFinished() { @@ -70,6 +69,6 @@ public boolean isFinished() { } @Override public void end(boolean interrupted) { - m_Drivetrain.setSpinLock(false); + drivetrain.setSpinLock(false); } } diff --git a/src/main/java/team3176/robot/commands/drivetrain/PathPlannerAuto.java b/src/main/java/team3176/robot/commands/drivetrain/PathPlannerAuto.java index 1adbebb..99426c1 100644 --- a/src/main/java/team3176/robot/commands/drivetrain/PathPlannerAuto.java +++ b/src/main/java/team3176/robot/commands/drivetrain/PathPlannerAuto.java @@ -1,6 +1,5 @@ package team3176.robot.commands.drivetrain; -import java.util.ArrayList; import java.util.HashMap; import java.util.List; @@ -11,39 +10,30 @@ import com.pathplanner.lib.auto.SwerveAutoBuilder; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.PrintCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import team3176.robot.commands.superstructure.intakecube.IntakeExtendFreeSpin; import team3176.robot.commands.superstructure.intakecube.PoopCube; import team3176.robot.constants.DrivetrainConstants; import team3176.robot.subsystems.drivetrain.Drivetrain; import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; -import team3176.robot.subsystems.superstructure.*; -import team3176.robot.subsystems.superstructure.Claw; import team3176.robot.subsystems.superstructure.Superstructure; public class PathPlannerAuto { Command auto; public PathPlannerAuto(String autoPathName, Command doBefore) { - Claw m_Claw = Claw.getInstance(); Drivetrain driveSubsystem = Drivetrain.getInstance(); - Superstructure m_Superstructure = Superstructure.getInstance(); + Superstructure superstructure = Superstructure.getInstance(); List pathGroup = PathPlanner.loadPathGroup(autoPathName, new PathConstraints(4.5,2.0)); //2.0, 1.5 //System.out.println("length" + pathGroup.size()); // This is just an example event map. It would be better to have a constant, global event map // in your code that will be used by all path following commands. HashMap eventMap = new HashMap<>(); - eventMap.put("scoreHighFirst", m_Superstructure.scoreGamePieceAuto()); - eventMap.put("scoreHigh", m_Superstructure.scoreGamePieceAuto()); + eventMap.put("scoreHighFirst", superstructure.scoreGamePieceAuto()); + eventMap.put("scoreHigh", superstructure.scoreGamePieceAuto()); eventMap.put("autoBalance", new AutoBalance().andThen(new SwerveDefense()).finallyDo((b) -> { driveSubsystem.setDriveMode(driveMode.DEFENSE); driveSubsystem.drive(0.0,0.0,0.0); })); - eventMap.put("groundCube",m_Superstructure.groundCube().withTimeout(3)); + eventMap.put("groundCube",superstructure.groundCube().withTimeout(3)); eventMap.put("poopCube",new PoopCube().withTimeout(.7)); // eventMap.put("intakeDown", new IntakeDown()); // Create the AutoBuilder. This only needs to be created once when robot code starts, not every time you want to create an auto command. A good place to put this is in RobotContainer along with your subsystems. diff --git a/src/main/java/team3176/robot/commands/drivetrain/SwerveDrive.java b/src/main/java/team3176/robot/commands/drivetrain/SwerveDrive.java index 118a355..969baed 100644 --- a/src/main/java/team3176/robot/commands/drivetrain/SwerveDrive.java +++ b/src/main/java/team3176/robot/commands/drivetrain/SwerveDrive.java @@ -1,8 +1,6 @@ package team3176.robot.commands.drivetrain; import java.util.function.DoubleSupplier; - -import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj2.command.CommandBase; import team3176.robot.constants.DrivetrainConstants; import team3176.robot.subsystems.drivetrain.Drivetrain; @@ -40,7 +38,4 @@ public void execute() { @Override public boolean isFinished() { return false; } - - @Override - public void end(boolean interrupted) { } } \ No newline at end of file diff --git a/src/main/java/team3176/robot/commands/drivetrain/teleopPath.java b/src/main/java/team3176/robot/commands/drivetrain/TeleopPath.java similarity index 58% rename from src/main/java/team3176/robot/commands/drivetrain/teleopPath.java rename to src/main/java/team3176/robot/commands/drivetrain/TeleopPath.java index e1d3da1..f1270d1 100644 --- a/src/main/java/team3176/robot/commands/drivetrain/teleopPath.java +++ b/src/main/java/team3176/robot/commands/drivetrain/TeleopPath.java @@ -8,49 +8,37 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import team3176.robot.constants.DrivetrainConstants; import team3176.robot.subsystems.drivetrain.Drivetrain; -public class teleopPath extends CommandBase{ - Drivetrain m_Drivetrain; +public class TeleopPath extends CommandBase{ + Drivetrain drivetrain; PathPlannerTrajectory traj1; PPSwerveControllerCommand swerveCommand; - public teleopPath() { - // super( null, - // Drivetrain.getInstance()::getPose, // Pose supplier - // DrivetrainConstants.DRIVE_KINEMATICS, // SwerveDriveKinematics - // new PIDController(5.0, 0, 0), // X controller. Tune these values for your robot. Leaving them 0 will only use feedforwards. - // new PIDController(5.0, 0, 0), // Y controller (usually the same values as X controller) - // new PIDController(0.5, 0, 0), // Rotation controller. Tune these values for your robot. Leaving them 0 will only use feedforwards. - // Drivetrain.getInstance()::setModuleStates, // Module states consumer - // true, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true - // Drivetrain.getInstance() // Requires this drive subsystem - // ); - m_Drivetrain = Drivetrain.getInstance(); - addRequirements(m_Drivetrain); - + public TeleopPath() { + drivetrain = Drivetrain.getInstance(); + addRequirements(drivetrain); } @Override public void initialize(){ - Pose2d pose = m_Drivetrain.getPose(); + Pose2d pose = drivetrain.getPose(); double xposition = pose.getX(); double yposition = pose.getY(); //System.out.println("pose" + xposition + "," + yposition); traj1 = PathPlanner.generatePath( new PathConstraints(1, 1), - new PathPoint(new Translation2d(xposition, yposition), pose.getRotation(), pose.getRotation(), m_Drivetrain.getCurrentChassisSpeed()), // position, heading + new PathPoint(new Translation2d(xposition, yposition), pose.getRotation(), pose.getRotation(), drivetrain.getCurrentChassisSpeed()), // position, heading new PathPoint(new Translation2d( 1.6, 6.74),Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(180),.01), // position, heading new PathPoint(new Translation2d( 1.1, 6.74),Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(180)) ); //System.out.println("traj" + traj1.getTotalTimeSeconds()); - swerveCommand = new PPSwerveControllerCommand(traj1, m_Drivetrain::getPose, DrivetrainConstants.DRIVE_KINEMATICS, // SwerveDriveKinematics + swerveCommand = new PPSwerveControllerCommand(traj1, drivetrain::getPose, DrivetrainConstants.DRIVE_KINEMATICS, // SwerveDriveKinematics new PIDController(5.0, 0, 0), // X controller. Tune these values for your robot. Leaving them 0 will only use feedforwards. new PIDController(5.0, 0, 0), // Y controller (usually the same values as X controller) new PIDController(0.5, 0, 0), // Rotation controller. Tune these values for your robot. Leaving them 0 will only use feedforwards. - m_Drivetrain::setModuleStates, // Module states consumer + drivetrain::setModuleStates, // Module states consumer false, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true - m_Drivetrain); + drivetrain); swerveCommand.initialize(); } @Override diff --git a/src/main/java/team3176/robot/commands/drivetrain/Turbo.java b/src/main/java/team3176/robot/commands/drivetrain/Turbo.java index df6bc2b..3d5a2ab 100644 --- a/src/main/java/team3176/robot/commands/drivetrain/Turbo.java +++ b/src/main/java/team3176/robot/commands/drivetrain/Turbo.java @@ -2,7 +2,6 @@ import java.util.function.DoubleSupplier; -import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj2.command.CommandBase; import team3176.robot.constants.DrivetrainConstants; import team3176.robot.subsystems.drivetrain.Drivetrain; @@ -40,7 +39,4 @@ public void execute() { @Override public boolean isFinished() { return false; } - - @Override - public void end(boolean interrupted) { } } \ No newline at end of file diff --git a/src/main/java/team3176/robot/commands/drivetrain/TurtleSpeed.java b/src/main/java/team3176/robot/commands/drivetrain/TurtleSpeed.java index 5e96fe5..080e544 100644 --- a/src/main/java/team3176/robot/commands/drivetrain/TurtleSpeed.java +++ b/src/main/java/team3176/robot/commands/drivetrain/TurtleSpeed.java @@ -15,7 +15,6 @@ public class TurtleSpeed extends CommandBase { private DoubleSupplier forwardCommand; private DoubleSupplier strafeCommand; private DoubleSupplier spinCommand; - private Double SpeedReductionFactor; public TurtleSpeed( DoubleSupplier forwardCommand, DoubleSupplier strafeCommand, DoubleSupplier spinCommand) { this.forwardCommand = forwardCommand; @@ -40,6 +39,4 @@ public void execute() { @Override public boolean isFinished() { return false; } - @Override - public void end(boolean interrupted) { } } \ No newline at end of file diff --git a/src/main/java/team3176/robot/commands/superstructure/OldPoopCube.java b/src/main/java/team3176/robot/commands/superstructure/OldPoopCube.java deleted file mode 100644 index 4859f5c..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/OldPoopCube.java +++ /dev/null @@ -1,86 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package team3176.robot.commands.superstructure; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import team3176.robot.constants.SuperStructureConstants; -import team3176.robot.subsystems.superstructure.Arm; -import team3176.robot.subsystems.superstructure.Claw; -import team3176.robot.subsystems.superstructure.IntakeCube; -import team3176.robot.subsystems.superstructure.Superstructure; -import team3176.robot.subsystems.superstructure.Superstructure.GamePiece; - -public class OldPoopCube extends CommandBase { - /** Creates a new ClawInhale. */ - Claw m_Claw = Claw.getInstance(); - Arm m_Arm = Arm.getInstance(); - IntakeCube m_IntakeCube = IntakeCube.getInstance(); - Superstructure m_Superstructure = Superstructure.getInstance(); - Double CarryDeadband = 5.0; - Double currentArmPosition; - Double kArmPoopUpperLimit, kArmPoopLowerLimit, kArmCarryUpperLimit, kArmCarryLowerLimit; - - public OldPoopCube() { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_Claw); - addRequirements(m_IntakeCube); - addRequirements(m_Arm); - addRequirements(m_Superstructure); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - //System.out.println("PoopCube Init"); - //m_Intake.extendAndFreeSpin(); - currentArmPosition = m_Arm.getArmPosition(); - kArmPoopLowerLimit = SuperStructureConstants.ARM_ZERO_POS - this.CarryDeadband; - kArmPoopUpperLimit = SuperStructureConstants.ARM_ZERO_POS + this.CarryDeadband; - kArmCarryLowerLimit = SuperStructureConstants.ARM_CARRY_POS - this.CarryDeadband; - kArmCarryUpperLimit = SuperStructureConstants.ARM_CARRY_POS + this.CarryDeadband; - - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() - { - //System.out.println("PoopCube Exec" + kArmPoopLowerLimit + ", " + kArmPoopUpperLimit + ", " + kArmCarryLowerLimit + ", " + kArmCarryUpperLimit); - //m_Intake.extendAndFreeSpin(); - m_Superstructure.preparePoop(); - new WaitCommand(2); - //if (m_Arm.getArmPosition() >= kArmPoopLowerLimit && m_Arm.getArmPosition() <= kArmPoopUpperLimit) { - m_Claw.scoreGamePiece(); - //} - if (m_Claw.getLinebreakOne() == false || m_Claw.getLinebreakTwo() == false) { - m_Claw.idle(); - new WaitCommand(2); - m_Superstructure.prepareCarry(); - } - this.currentArmPosition = m_Arm.getArmPosition(); - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - //System.out.println("PoopCube End"); - //m_Intake.Retract(); - //m_Intake.spinVelocityPercent(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - //System.out.println("PoopCube IsFinished"); - //if (currentArmPosition >= kArmCarryLowerLimit && currentArmPosition <= kArmCarryUpperLimit) { - // return true; - //} else{ - return false; - //} - } -} diff --git a/src/main/java/team3176/robot/commands/superstructure/arm/armAnalogUp.java b/src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogDown.java similarity index 63% rename from src/main/java/team3176/robot/commands/superstructure/arm/armAnalogUp.java rename to src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogDown.java index 7bf4de6..6e02378 100644 --- a/src/main/java/team3176/robot/commands/superstructure/arm/armAnalogUp.java +++ b/src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogDown.java @@ -4,43 +4,32 @@ package team3176.robot.commands.superstructure.arm; -import java.util.function.DoubleSupplier; import edu.wpi.first.wpilibj2.command.CommandBase; import team3176.robot.subsystems.superstructure.Arm; -public class armAnalogUp extends CommandBase { +public class ArmAnalogDown extends CommandBase { /** Creates a new IntakeExtendSpin. */ - private Arm m_Arm = Arm.getInstance(); - private DoubleSupplier analogInput; - private Double analogInputDeadband; + private Arm arm = Arm.getInstance(); + // private DoubleSupplier analogInput; + // private Double analogInputDeadband; - public armAnalogUp() { + public ArmAnalogDown() { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_Arm); + addRequirements(arm); } - // Called when the command is initially scheduled. - @Override - public void initialize() { - - } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_Arm.armAnalogUp(); + arm.armAnalogDown(); } //if ((updatedAnalogInput < (0 + analogInputDeadband)) && (updatedAnalogInput > (0 + analogInputDeadband))) { // m_Arm. //} - - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} // Returns true when the command should end. @Override diff --git a/src/main/java/team3176/robot/commands/superstructure/arm/armAnalogIdle.java b/src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogIdle.java similarity index 51% rename from src/main/java/team3176/robot/commands/superstructure/arm/armAnalogIdle.java rename to src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogIdle.java index 738f9a4..78f29d9 100644 --- a/src/main/java/team3176/robot/commands/superstructure/arm/armAnalogIdle.java +++ b/src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogIdle.java @@ -4,44 +4,27 @@ package team3176.robot.commands.superstructure.arm; -import java.util.function.DoubleSupplier; import edu.wpi.first.wpilibj2.command.CommandBase; import team3176.robot.subsystems.superstructure.Arm; -public class armAnalogIdle extends CommandBase { +public class ArmAnalogIdle extends CommandBase { /** Creates a new IntakeExtendSpin. */ - private Arm m_Arm = Arm.getInstance(); - private DoubleSupplier analogInput; - private Double analogInputDeadband; + private Arm arm = Arm.getInstance(); - public armAnalogIdle() { + public ArmAnalogIdle() { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_Arm); + addRequirements(arm); } // Called when the command is initially scheduled. @Override public void initialize() { - m_Arm.idle(); + arm.idle(); } - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - } - - //if ((updatedAnalogInput < (0 + analogInputDeadband)) && (updatedAnalogInput > (0 + analogInputDeadband))) { - // m_Arm. - //} - - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - // Returns true when the command should end. @Override public boolean isFinished() { diff --git a/src/main/java/team3176/robot/commands/superstructure/arm/armAnalogDown.java b/src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogUp.java similarity index 52% rename from src/main/java/team3176/robot/commands/superstructure/arm/armAnalogDown.java rename to src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogUp.java index 6d5ea14..4154ef7 100644 --- a/src/main/java/team3176/robot/commands/superstructure/arm/armAnalogDown.java +++ b/src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogUp.java @@ -4,43 +4,25 @@ package team3176.robot.commands.superstructure.arm; -import java.util.function.DoubleSupplier; - import edu.wpi.first.wpilibj2.command.CommandBase; import team3176.robot.subsystems.superstructure.Arm; -public class armAnalogDown extends CommandBase { +public class ArmAnalogUp extends CommandBase { /** Creates a new IntakeExtendSpin. */ - private Arm m_Arm = Arm.getInstance(); - private DoubleSupplier analogInput; - private Double analogInputDeadband; + private Arm arm = Arm.getInstance(); - public armAnalogDown() { + public ArmAnalogUp() { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_Arm); + addRequirements(arm); } - // Called when the command is initially scheduled. - @Override - public void initialize() { - - } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_Arm.armAnalogDown(); + arm.armAnalogUp(); } - - //if ((updatedAnalogInput < (0 + analogInputDeadband)) && (updatedAnalogInput > (0 + analogInputDeadband))) { - // m_Arm. - //} - - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} // Returns true when the command should end. @Override diff --git a/src/main/java/team3176/robot/commands/superstructure/arm/ArmFollowTrajectory.java b/src/main/java/team3176/robot/commands/superstructure/arm/ArmFollowTrajectory.java index 1737ca1..85beda0 100644 --- a/src/main/java/team3176/robot/commands/superstructure/arm/ArmFollowTrajectory.java +++ b/src/main/java/team3176/robot/commands/superstructure/arm/ArmFollowTrajectory.java @@ -8,14 +8,14 @@ public class ArmFollowTrajectory extends CommandBase { /** Creates a new IntakeExtendSpin. */ - private Arm m_Arm = Arm.getInstance(); + private Arm arm = Arm.getInstance(); TrapezoidProfile traj; double goalAngle; Timer timeElapsed; public ArmFollowTrajectory(double goalAngle) { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_Arm); + addRequirements(arm); this.goalAngle = goalAngle; } @@ -24,7 +24,7 @@ public ArmFollowTrajectory(double goalAngle) { public void initialize() { traj = new TrapezoidProfile(new Constraints(20, 20), new State(goalAngle,0.0), - new State(m_Arm.getArmPosition(),0.0)); + new State(arm.getArmPosition(),0.0)); timeElapsed.start(); } @@ -32,14 +32,9 @@ public void initialize() { @Override public void execute() { State setpoint = traj.calculate(timeElapsed.get()); - m_Arm.setAngleSetpoint(setpoint.position); + arm.setAngleSetpoint(setpoint.position); } - - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} // Returns true when the command should end. @Override diff --git a/src/main/java/team3176/robot/commands/superstructure/arm/manuallyPositionArm.java b/src/main/java/team3176/robot/commands/superstructure/arm/ManuallyPositionArm.java similarity index 67% rename from src/main/java/team3176/robot/commands/superstructure/arm/manuallyPositionArm.java rename to src/main/java/team3176/robot/commands/superstructure/arm/ManuallyPositionArm.java index af5118d..6e93310 100644 --- a/src/main/java/team3176/robot/commands/superstructure/arm/manuallyPositionArm.java +++ b/src/main/java/team3176/robot/commands/superstructure/arm/ManuallyPositionArm.java @@ -9,36 +9,31 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import team3176.robot.subsystems.superstructure.Arm; -public class manuallyPositionArm extends CommandBase { +public class ManuallyPositionArm extends CommandBase { /** Creates a new IntakeExtendSpin. */ - private Arm m_Arm = Arm.getInstance(); + private Arm arm = Arm.getInstance(); private DoubleSupplier analogInput; private Double analogInputDeadband; - public manuallyPositionArm(DoubleSupplier analogInput) { + public ManuallyPositionArm(DoubleSupplier analogInput) { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_Arm); + addRequirements(arm); this.analogInput = analogInput; this.analogInputDeadband = 0.01; } - // Called when the command is initially scheduled. - @Override - public void initialize() { - - } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { Double updatedAnalogInput = this.analogInput.getAsDouble(); if (updatedAnalogInput > (0 + analogInputDeadband)) { - m_Arm.armAnalogUpCommand(); + arm.armAnalogUpCommand(); } if (updatedAnalogInput < (0 - analogInputDeadband)) { - m_Arm.armAnalogDownCommand(); + arm.armAnalogDownCommand(); } //if ((updatedAnalogInput < (0 + analogInputDeadband)) && (updatedAnalogInput > (0 + analogInputDeadband))) { @@ -47,17 +42,12 @@ public void execute() { } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { Double updatedAnalogInput = this.analogInput.getAsDouble(); - if ((updatedAnalogInput < (0 + analogInputDeadband)) && (updatedAnalogInput > (0 - analogInputDeadband))) { - return true; - } else - return false; - } + return (updatedAnalogInput < (0 + analogInputDeadband)) && (updatedAnalogInput > (0 - analogInputDeadband)); + } + } diff --git a/src/main/java/team3176/robot/commands/superstructure/autoScoreConeHigh.java b/src/main/java/team3176/robot/commands/superstructure/autoScoreConeHigh.java deleted file mode 100644 index bb0ff2f..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/autoScoreConeHigh.java +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package team3176.robot.commands.superstructure; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import team3176.robot.constants.SuperStructureConstants; -import team3176.robot.subsystems.superstructure.Arm; -import team3176.robot.subsystems.superstructure.Claw; -import team3176.robot.subsystems.superstructure.IntakeCube; -import team3176.robot.subsystems.superstructure.Superstructure; -import team3176.robot.subsystems.superstructure.Superstructure.GamePiece; - -@Deprecated -public class autoScoreConeHigh extends CommandBase { - /** Creates a new ClawInhale. */ - Claw m_Claw = Claw.getInstance(); - Arm m_Arm = Arm.getInstance(); - IntakeCube m_IntakeCube = IntakeCube.getInstance(); - Superstructure m_Superstructure = Superstructure.getInstance(); - Double CarryDeadband = 5.0; - Double currentArmPosition; - Double kArmPoopUpperLimit, kArmPoopLowerLimit, kArmCarryUpperLimit, kArmCarryLowerLimit; - - public autoScoreConeHigh() { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_Claw); - addRequirements(m_IntakeCube); - addRequirements(m_Arm); - addRequirements(m_Superstructure); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_IntakeCube.extendAndFreeSpin(); - m_Superstructure.prepareScoreHigh(); - - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() - { - //this again is command so would do nothing just to call it. Would want to use command composition - //or would want to do m_claw.scoreGamePiece.execute() - m_Claw.scoreGamePiece(); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - //System.out.println("PoopCube End"); - m_Claw.idle(); - m_IntakeCube.Retract(); - m_IntakeCube.spinIntake(0); - //this will return a command and not actually run. Use the command composition .andThen() to schedule the command - m_Superstructure.prepareCarry().initialize();; - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - //System.out.println("PoopCube IsFinished"); - return m_Claw.isEmpty(); - } -} diff --git a/src/main/java/team3176/robot/commands/superstructure/claw/ClawIdle.java b/src/main/java/team3176/robot/commands/superstructure/claw/ClawIdle.java index 0f4e36d..2cf2f3e 100644 --- a/src/main/java/team3176/robot/commands/superstructure/claw/ClawIdle.java +++ b/src/main/java/team3176/robot/commands/superstructure/claw/ClawIdle.java @@ -4,39 +4,23 @@ package team3176.robot.commands.superstructure.claw; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.CommandBase; import team3176.robot.subsystems.superstructure.Claw; -import team3176.robot.subsystems.superstructure.Superstructure.GamePiece; public class ClawIdle extends CommandBase { /** Creates a new ClawInhale. */ - Claw m_Claw = Claw.getInstance(); + Claw claw = Claw.getInstance(); public ClawIdle() { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_Claw); + addRequirements(claw); } // Called when the command is initially scheduled. @Override public void initialize() { - m_Claw.idle(); + claw.idle(); } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() - { - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - - } - // Returns true when the command should end. @Override public boolean isFinished() { diff --git a/src/main/java/team3176/robot/constants/DrivetrainConstants.java b/src/main/java/team3176/robot/constants/DrivetrainConstants.java index 180ca88..a2e2adc 100644 --- a/src/main/java/team3176/robot/constants/DrivetrainConstants.java +++ b/src/main/java/team3176/robot/constants/DrivetrainConstants.java @@ -38,11 +38,5 @@ public class DrivetrainConstants extends DrivetrainHardwareMap { //MAX_ROT_SPEED_RADIANS_PER_SECOND, MAX_ROT_ACCELERATION_RADIANS_PER_SECOND_SQUARED); 2*Math.PI, 2*Math.PI ); - - public static final double P_X_Controller = 1; - public static final double P_Y_Controller = 1; - public static final double P_Theta_Controller = 1; - - } diff --git a/src/main/java/team3176/robot/util/God/Math3176.java b/src/main/java/team3176/robot/util/God/Math3176.java deleted file mode 100644 index bd6ba7c..0000000 --- a/src/main/java/team3176/robot/util/God/Math3176.java +++ /dev/null @@ -1,9 +0,0 @@ -package team3176.robot.util.God; - -import edu.wpi.first.wpilibj.Timer; -import team3176.robot.*; - - -public class Math3176{ - // I am a stub right now -} diff --git a/src/main/java/team3176/robot/util/God/PID3176.java b/src/main/java/team3176/robot/util/God/PID3176.java deleted file mode 100644 index dce1f86..0000000 --- a/src/main/java/team3176/robot/util/God/PID3176.java +++ /dev/null @@ -1,100 +0,0 @@ -package team3176.robot.util.God; - -import edu.wpi.first.wpilibj.Timer; -import team3176.robot.*; - -public class PID3176 { - private double kP; - private double kI; - private double kD; - private double kF; - private double error, previous_error, integral, derivative, output, integralMax = 0; - private double max_speed = 1.0; - private double currTime; - private double lastTime = Timer.getFPGATimestamp(); - private double deltaTime; - - public PID3176(double pG, double iG, double dG){ - kP = pG; - kI = iG; - kD = dG; - } - - public PID3176(double pG, double iG, double dG, double mS){ - kP = pG; - kI = iG; - kD = dG; - max_speed = mS; - } - - public PID3176(double pG, double iG, double dG, double mS, double f){ - kP = pG; - kI = iG; - kD = dG; - max_speed = mS; - kF = f; - } - - public PID3176(double pG, double iG, double dG, double mS, double f, double iMax){ - kP = pG; - kI = iG; - kD = dG; - max_speed = mS; - integralMax = iMax; - } - - public double returnOutput(double current, double setpoint) { - error = setpoint - current; - return returnOutput(error); - } - - public double returnOutput(double error) { - deltaTime = .02; - if(integral < integralMax || integralMax == 0) { - integral += (error*deltaTime); - } - else - { - if(integral>integralMax) - { - integral = integralMax; - } - else if(integral<-integralMax) - { - integral = -integralMax; - } - } - derivative = (error - previous_error)/deltaTime; - previous_error = error; - - output = (kP*error) + (kI*integral) + (kD*derivative); - - if(output>max_speed) { - output = max_speed; - } - else if(output<-max_speed) { - output = -max_speed; - } - - return output; - } - - //getters and setters for everything - public double getkP() {return kP;} - public void setkP(double kP) {this.kP = kP;} - - public double getkI() {return kI;} - public void setkI(double kI) {this.kI = kI;} - - public double getkD() {return kD;} - public void setkD(double kD) {this.kD = kD;} - - public double getkF() {return kF;} - public void setkF(double kF) {this.kF = kF;} - - public double getIntegralMax() {return integralMax;} - public void setIntegralMax(double integralMax) {this.integralMax = integralMax;} - - public double getMax_speed() {return max_speed;} - public void setMax_speed(double max_speed) {this.max_speed = max_speed;} -} diff --git a/src/main/java/team3176/robot/util/LoggedTunableNumber.java b/src/main/java/team3176/robot/util/LoggedTunableNumber.java index 9c4f716..c27811b 100644 --- a/src/main/java/team3176/robot/util/LoggedTunableNumber.java +++ b/src/main/java/team3176/robot/util/LoggedTunableNumber.java @@ -11,12 +11,14 @@ import java.util.Map; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; +import team3176.robot.Constants; + /** * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or * value not in dashboard. */ public class LoggedTunableNumber { - private static final String tableKey = "TunableNumbers"; + private static final String TABLE_KEY = "TunableNumbers"; private final String key; private boolean hasDefault = false; @@ -30,7 +32,7 @@ public class LoggedTunableNumber { * @param dashboardKey Key on dashboard */ public LoggedTunableNumber(String dashboardKey) { - this.key = tableKey + "/" + dashboardKey; + this.key = TABLE_KEY + "/" + dashboardKey; } /** @@ -68,7 +70,7 @@ public double get() { if (!hasDefault) { return 0.0; } else { - return true ? dashboardNumber.get() : defaultValue; + return Constants.TUNING_MODE ? dashboardNumber.get() : defaultValue; } } From c909a788080c0c2b53542ab3efd4a0aed29d171e Mon Sep 17 00:00:00 2001 From: Jonathan Date: Mon, 12 Jun 2023 17:42:13 -0600 Subject: [PATCH 6/7] vision isreal --- .../team3176/robot/subsystems/vision/VisionDual.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/team3176/robot/subsystems/vision/VisionDual.java b/src/main/java/team3176/robot/subsystems/vision/VisionDual.java index ef6c316..bc3c4e2 100644 --- a/src/main/java/team3176/robot/subsystems/vision/VisionDual.java +++ b/src/main/java/team3176/robot/subsystems/vision/VisionDual.java @@ -7,7 +7,10 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import team3176.robot.Constants; +import team3176.robot.Constants.Mode; import team3176.robot.subsystems.vision.VisionDualIO.VisionDualInputs; +import team3176.robot.subsystems.vision.VisionDualIO; public class VisionDual extends SubsystemBase{ private VisionDualIO io; private VisionDualInputs inputs; @@ -23,7 +26,12 @@ private VisionDual(VisionDualIO io) { } public static VisionDual getInstance() { if (instance == null) { - instance = new VisionDual(new VisionDualIOLime()); + if(Constants.getMode() == Mode.REAL) { + instance = new VisionDual(new VisionDualIOLime()); + } else { + instance = new VisionDual(new VisionDualIO() {}); + } + } return instance; } From db55ae8a1dfaef17d10dd102b6145414067c1b66 Mon Sep 17 00:00:00 2001 From: "char@yoda" Date: Tue, 27 Jun 2023 23:30:15 -0400 Subject: [PATCH 7/7] This code fails & may not build. This commit is to tx btwn machines. Don't judge me too harshly, plz. --- .../java/team3176/robot/RobotContainer.java | 11 +-- .../robot/commands/drivetrain/CubeChase.java | 66 ++++++++++++++++++ .../commands/drivetrain/CubeChaseAuto.java | 69 +++++++++++++++++++ .../constants/SuperStructureConstants.java | 7 +- .../team3176/robot/subsystems/RobotState.java | 2 +- .../subsystems/drivetrain/Drivetrain.java | 10 +-- .../drivetrain/LimelightHelpers.java | 10 +-- .../subsystems/vision/VisionCubeChase.java | 51 ++++++++++++++ .../subsystems/vision/VisionCubeChaseIO.java | 23 +++++++ .../vision/VisionCubeChaseIOLime.java | 20 ++++++ 10 files changed, 250 insertions(+), 19 deletions(-) create mode 100644 src/main/java/team3176/robot/commands/drivetrain/CubeChase.java create mode 100644 src/main/java/team3176/robot/commands/drivetrain/CubeChaseAuto.java create mode 100644 src/main/java/team3176/robot/subsystems/vision/VisionCubeChase.java create mode 100644 src/main/java/team3176/robot/subsystems/vision/VisionCubeChaseIO.java create mode 100644 src/main/java/team3176/robot/subsystems/vision/VisionCubeChaseIOLime.java diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index c21dfd6..6fba6d3 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -29,7 +29,8 @@ import team3176.robot.subsystems.superstructure.IntakeCone; import team3176.robot.subsystems.superstructure.Superstructure; -import team3176.robot.subsystems.vision.VisionDual; +//import team3176.robot.subsystems.vision.VisionDual; +import team3176.robot.subsystems.vision.VisionCubeChase; /** * This class is where the bulk of the robot should be declared. Since @@ -53,7 +54,7 @@ public class RobotContainer { // is this why we don't have a compressor? private final Compressor m_Compressor private final Drivetrain drivetrain; - private final VisionDual vision; + private final VisionCubeChase vision; private final Superstructure superstructure; private SendableChooser autonChooser; @@ -70,7 +71,7 @@ public RobotContainer() { intakeCone = IntakeCone.getInstance(); pdh = new PowerDistribution(Hardwaremap.PDH_CID, ModuleType.kRev); - vision = VisionDual.getInstance(); + vision = VisionCubeChase.getInstance(); superstructure = Superstructure.getInstance(); drivetrain.setDefaultCommand(new SwerveDrive( controller::getForward, @@ -107,13 +108,13 @@ private void configureBindings() { controller.transStick.button(4).whileTrue(superstructure.prepareScoreHigh()); controller.transStick.button(4).onFalse((superstructure.prepareCarry())); - controller.transStick.button(5).onTrue(new InstantCommand(drivetrain::resetPoseToVision,drivetrain)); +// controller.transStick.button(5).onTrue(new InstantCommand(drivetrain::resetPoseToVision,drivetrain)); controller.transStick.button(10).whileTrue(new InstantCommand(drivetrain::setBrakeMode).andThen(new SwerveDefense())); //m_Controller.getTransStick_Button10() // .onFalse(new InstantCommand(() -> m_Drivetrain.setDriveMode(driveMode.DRIVE), m_Drivetrain)); // m_Controller.getRotStick_Button2().whileTrue(new FlipField); - controller.rotStick.button(1).whileTrue(new Turbo( + controller.rotStick.button(1).whileTrue(new CubeChase( controller::getForward, controller::getStrafe, controller::getSpin diff --git a/src/main/java/team3176/robot/commands/drivetrain/CubeChase.java b/src/main/java/team3176/robot/commands/drivetrain/CubeChase.java new file mode 100644 index 0000000..6d6335e --- /dev/null +++ b/src/main/java/team3176/robot/commands/drivetrain/CubeChase.java @@ -0,0 +1,66 @@ +package team3176.robot.commands.drivetrain; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import team3176.robot.constants.DrivetrainConstants; +import team3176.robot.subsystems.drivetrain.Drivetrain; +import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; +import team3176.robot.subsystems.vision.VisionCubeChase; +import edu.wpi.first.math.controller.PIDController; +import team3176.robot.subsystems.drivetrain.LimelightHelpers; + + + + +public class CubeChase extends CommandBase { + private Drivetrain drivetrain = Drivetrain.getInstance(); + private VisionCubeChase visionCubeChase = VisionCubeChase.getInstance(); + + + private DoubleSupplier forwardCommand; + private DoubleSupplier strafeCommand; + private DoubleSupplier spinCommand; + private double splicingSpinCommand; + + PIDController txController = new PIDController(.01, 0, 0); + double tx; + + public CubeChase( DoubleSupplier forwardCommand, DoubleSupplier strafeCommand, DoubleSupplier spinCommand) { + this.forwardCommand = forwardCommand; + this.strafeCommand = strafeCommand; + this.spinCommand = spinCommand; + addRequirements(drivetrain); + addRequirements(visionCubeChase); + } + + @Override + public void initialize() { + drivetrain.setDriveMode(driveMode.DRIVE); + drivetrain.setSpinLock(false); + //drivetrain.setCoastMode(); + //this.tx = visionCubeChase.getTx(); + } + + @Override + public void execute() { + //this.tx = visionCubeChase.getTx(); + this.tx = LimelightHelpers.getTX("limelight-three"); + splicingSpinCommand = 1 * txController.calculate(tx, 0.0); + System.out.println("tx: "+ this.tx + ", splicingSpinCommand: " + splicingSpinCommand); + //drivetrain.drive(forwardCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND * 1.0, + //#strafeCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND * 1.0, + //spinCommand.getAsDouble()*7); + drivetrain.drive(forwardCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND * 1.0, + strafeCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND * 1.0, + splicingSpinCommand *7); + } + + @Override + public boolean isFinished() { return false; } + + @Override + public void end(boolean interrupted) { } +} \ No newline at end of file diff --git a/src/main/java/team3176/robot/commands/drivetrain/CubeChaseAuto.java b/src/main/java/team3176/robot/commands/drivetrain/CubeChaseAuto.java new file mode 100644 index 0000000..d933958 --- /dev/null +++ b/src/main/java/team3176/robot/commands/drivetrain/CubeChaseAuto.java @@ -0,0 +1,69 @@ +package team3176.robot.commands.drivetrain; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import team3176.robot.constants.DrivetrainConstants; +import team3176.robot.subsystems.drivetrain.Drivetrain; +import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; +import team3176.robot.subsystems.vision.VisionCubeChase; +import edu.wpi.first.math.controller.PIDController; +import team3176.robot.subsystems.drivetrain.LimelightHelpers; + + + + +public class CubeChaseAuto extends CommandBase { + private Drivetrain drivetrain = Drivetrain.getInstance(); + private VisionCubeChase visionCubeChase = VisionCubeChase.getInstance(); + + + //private DoubleSupplier forwardCommand; + //private DoubleSupplier strafeCommand; + //private DoubleSupplier spinCommand; + private double forwardCommand; + private double strafeCommand; + private double splicingSpinCommand; + + PIDController txController = new PIDController(.01, 0, 0); + double tx; + boolean tv; + + public CubeChaseAuto() { + addRequirements(drivetrain); + addRequirements(visionCubeChase); + } + + @Override + public void initialize() { + drivetrain.setDriveMode(driveMode.DRIVE); + drivetrain.setSpinLock(false); + //drivetrain.setCoastMode(); + //this.tx = visionCubeChase.getTx(); + } + + @Override + public void execute() { + //this.tx = visionCubeChase.getTx(); + this.tx = LimelightHelpers.getTX("limelight-three"); + splicingSpinCommand = 1 * txController.calculate(tx, 0.0); + //System.out.println("tx: "+ this.tx + ", splicingSpinCommand: " + splicingSpinCommand); + //drivetrain.drive(forwardCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND * 1.0, + //#strafeCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND * 1.0, + //spinCommand.getAsDouble()*7); + this.tv = LimelightHelpers.getTV("limelight-three"); + if (this.tv) this.forwardCommand = 1.0; else this.forwardCommand = 0.0; + drivetrain.drive(forwardCommand * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND * 1.0, + strafeCommand * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND * 1.0, + splicingSpinCommand *7); + } + + @Override + public boolean isFinished() { + return false; } + + @Override + public void end(boolean interrupted) { } +} \ No newline at end of file diff --git a/src/main/java/team3176/robot/constants/SuperStructureConstants.java b/src/main/java/team3176/robot/constants/SuperStructureConstants.java index b589325..b82361a 100644 --- a/src/main/java/team3176/robot/constants/SuperStructureConstants.java +++ b/src/main/java/team3176/robot/constants/SuperStructureConstants.java @@ -5,7 +5,7 @@ public class SuperStructureConstants { /** * How many amps the arm motor can use. */ - public static final int ARM_CURRENT_LIMIT_A = 10; + public static final int ARM_CURRENT_LIMIT_A = 15; /** * Percent output to run the arm up/down at @@ -43,10 +43,11 @@ public class SuperStructureConstants { public static final double ARM_kg = 0.2; public static final double ARM_TOLERANCE = 3; - public static final double ARM_ZERO_POS = 170; + public static final double ARM_ZERO_POS = 165; public static final double ARM_CARRY_POS = ARM_ZERO_POS; public static final double ARM_CATCH_POS = 45 + ARM_ZERO_POS; public static final double ARM_MID_POS = 100 + ARM_ZERO_POS; - public static final double ARM_HIGH_POS = 185 + ARM_ZERO_POS; + public static final double ARM_HIGH_POS = 200 + ARM_ZERO_POS; public static final double ARM_SIM_OFFSET = 70 + ARM_ZERO_POS; } + \ No newline at end of file diff --git a/src/main/java/team3176/robot/subsystems/RobotState.java b/src/main/java/team3176/robot/subsystems/RobotState.java index 333e01b..5bf8de0 100644 --- a/src/main/java/team3176/robot/subsystems/RobotState.java +++ b/src/main/java/team3176/robot/subsystems/RobotState.java @@ -373,7 +373,7 @@ public void update() { } public void setColorWantState(int LEDState) { - System.out.println("WAS CALLED"); + //System.out.println("WAS CALLED"); wantedLEDState = LEDState; if (wantedLEDState == 0) { isFlashing = false; diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index 735ad89..28f298c 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -80,7 +80,7 @@ public enum coordType { double angleAvgRollingWindow; public enum driveMode { - DEFENSE, DRIVE, VISION + DEFENSE, DRIVE, VISION, CUBECHASE } private SwervePod podFR; @@ -479,12 +479,12 @@ public void periodic() { SmartDashboard.putNumber("NavYaw",getPoseYawWrapped().getDegrees()); //Liam and Andrews work! - double[] visionPoseArray = NetworkTableInstance.getDefault().getTable("limelight-rfov").getEntry("botpose_wpiblue").getDoubleArray(new double[6]); - Pose3d visionPose3dNT = new Pose3d(visionPoseArray[0], visionPoseArray[1], visionPoseArray[2], new Rotation3d( Units.degreesToRadians(visionPoseArray[3]), Units.degreesToRadians(visionPoseArray[4]), Units.degreesToRadians(visionPoseArray[5]))); - Logger.getInstance().recordOutput("Drive/vision_pose", visionPose3dNT); + //double[] visionPoseArray = NetworkTableInstance.getDefault().getTable("limelight-rfov").getEntry("botpose_wpiblue").getDoubleArray(new double[6]); + //Pose3d visionPose3dNT = new Pose3d(visionPoseArray[0], visionPoseArray[1], visionPoseArray[2], new Rotation3d( Units.degreesToRadians(visionPoseArray[3]), Units.degreesToRadians(visionPoseArray[4]), Units.degreesToRadians(visionPoseArray[5]))); + //Logger.getInstance().recordOutput("Drive/vision_pose", visionPose3dNT); //new vision proposal - visionPose3d = VisionDual.getInstance().getPose3d(); + //visionPose3d = VisionDual.getInstance().getPose3d(); // double[] default_pose = {0.0,0.0,0.0,0.0,0.0,0.0}; // try { diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/LimelightHelpers.java b/src/main/java/team3176/robot/subsystems/drivetrain/LimelightHelpers.java index c0a82da..710bab9 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/LimelightHelpers.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/LimelightHelpers.java @@ -1,4 +1,4 @@ -//LimelightHelpers v1.2.0 (Feb 13, 2023) +//LimelightHelpers v1.2.1 (March 1, 2023) package team3176.robot.subsystems.drivetrain; @@ -385,7 +385,7 @@ static final String sanitizeName(String name) { private static Pose3d toPose3D(double[] inData){ if(inData.length < 6) { - System.err.println("Bad LL 3D Pose Data!"); + System.err.println("388:Bad LL 3D Pose Data!"); return new Pose3d(); } return new Pose3d( @@ -397,7 +397,7 @@ private static Pose3d toPose3D(double[] inData){ private static Pose2d toPose2D(double[] inData){ if(inData.length < 6) { - System.err.println("Bad LL 2D Pose Data!"); + System.err.println("400:Bad LL 2D Pose Data!"); return new Pose2d(); } Translation2d tran2d = new Translation2d(inData[0], inData[1]); @@ -521,7 +521,7 @@ public static double[] getBotPose_wpiBlue(String limelightName) { } public static double[] getBotPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetSpace"); + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); } public static double[] getCameraPose_TargetSpace(String limelightName) { @@ -743,7 +743,7 @@ private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) if (responseCode == 200) { return true; } else { - System.err.println("Bad LL Request"); + System.err.println("746:Bad LL Request"); } } catch (IOException e) { System.err.println(e.getMessage()); diff --git a/src/main/java/team3176/robot/subsystems/vision/VisionCubeChase.java b/src/main/java/team3176/robot/subsystems/vision/VisionCubeChase.java new file mode 100644 index 0000000..58ff2fa --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/vision/VisionCubeChase.java @@ -0,0 +1,51 @@ +package team3176.robot.subsystems.vision; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import team3176.robot.subsystems.vision.VisionCubeChaseIO.VisionCubeChaseInputs; + +public class VisionCubeChase extends SubsystemBase{ + private VisionCubeChaseIO io; + private Pose3d bestVisionPose3d; + private Pose2d bestVisionPose2d; + private static VisionCubeChase instance; + private final VisionCubeChaseInputs inputs = new VisionCubeChaseInputs(); + private VisionCubeChase(VisionCubeChaseIO io) { + this.io = io; + } + public static VisionCubeChase getInstance() { + if (instance == null) { + instance = new VisionCubeChase(new VisionCubeChaseIOLime()); + } + return instance; + } + public Pose3d getPose3d() { + return bestVisionPose3d; + } + public Pose2d getPose2d() { + return bestVisionPose2d; + } + + public boolean isValid() { + return inputs.rValid; + } + + public double getTx() { + return inputs.rTx; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + //Logger.getInstance().processInputs("VisionCubeChase", inputs); + + //Logger.getInstance().recordOutput("Vision/bestPose",bestVisionPose3d); + + + } +} diff --git a/src/main/java/team3176/robot/subsystems/vision/VisionCubeChaseIO.java b/src/main/java/team3176/robot/subsystems/vision/VisionCubeChaseIO.java new file mode 100644 index 0000000..b71c9d3 --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/vision/VisionCubeChaseIO.java @@ -0,0 +1,23 @@ +package team3176.robot.subsystems.vision; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Pose3d; + +public interface VisionCubeChaseIO { + + @AutoLog + public static class VisionCubeChaseInputs { + //public Pose3d rfovBlue = new Pose3d(); + //public Pose3d rfovRed = new Pose3d(); + public double rLatency = 0.0; + public int rNumTags = 0; + public boolean rValid = false; + public double rTx; + //constructor if needed for some inputs + VisionCubeChaseInputs() { + } + } + /** Updates the set of loggable inputs. */ + public default void updateInputs(VisionCubeChaseInputs inputs) {} +} diff --git a/src/main/java/team3176/robot/subsystems/vision/VisionCubeChaseIOLime.java b/src/main/java/team3176/robot/subsystems/vision/VisionCubeChaseIOLime.java new file mode 100644 index 0000000..1fa4f1c --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/vision/VisionCubeChaseIOLime.java @@ -0,0 +1,20 @@ +package team3176.robot.subsystems.vision; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Pose3d; +import team3176.robot.subsystems.drivetrain.LimelightHelpers; + +public class VisionCubeChaseIOLime implements VisionCubeChaseIO { + public static final String rfov = "limelight-three"; + //public static final String lfov = "lfov-limelight"; + /** Updates the set of loggable inputs. */ + public void updateInputs(VisionCubeChaseInputs inputs) { + //inputs.rfovBlue = LimelightHelpers.getBotPose3d_wpiBlue(rfov); + //inputs.rfovRed = LimelightHelpers.getBotPose3d_wpiRed(rfov); + inputs.rLatency = LimelightHelpers.getLatency_Capture(rfov) + LimelightHelpers.getLatency_Pipeline(rfov); + inputs.rNumTags = LimelightHelpers.getLatestResults(rfov).targetingResults.targets_Fiducials.length; + inputs.rValid = LimelightHelpers.getTV(rfov); + inputs.rTx = LimelightHelpers.getTX(rfov); + } +}