diff --git a/.gitignore b/.gitignore index ea15e79..34f9eb2 100644 --- a/.gitignore +++ b/.gitignore @@ -58,7 +58,7 @@ # virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml hs_err_pid* - +log/* ### Linux ### *~ diff --git a/simgui-ds.json b/simgui-ds.json index 690a4a2..17e0f72 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,10 +91,11 @@ ], "robotJoysticks": [ {}, - {}, { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard1" + }, + { + "guid": "Keyboard0" } ] } diff --git a/simgui-window.json b/simgui-window.json index dd2b597..81ae590 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -5,50 +5,45 @@ "MainWindow": { "GLOBAL": { "fps": "120", - "height": "1055", + "height": "720", "maximized": "0", "style": "0", "userScale": "2", - "width": "1924", - "xpos": "-1", - "ypos": "-1" + "width": "1280", + "xpos": "537", + "ypos": "110" } }, "Window": { - "###Addressable LEDs": { - "Collapsed": "0", - "Pos": "318,53", - "Size": "170,300" - }, "###FMS": { "Collapsed": "0", - "Pos": "10,1036", - "Size": "336,164" + "Pos": "5,540", + "Size": "283,146" }, "###Joysticks": { "Collapsed": "0", - "Pos": "483,801", - "Size": "976,278" + "Pos": "250,465", + "Size": "796,155" }, "###NetworkTables": { "Collapsed": "0", - "Pos": "271,365", - "Size": "1500,370" + "Pos": "250,277", + "Size": "750,185" }, "###Other Devices": { "Collapsed": "0", - "Pos": "1905,40", - "Size": "500,1390" + "Pos": "1025,20", + "Size": "250,695" }, "###System Joysticks": { "Collapsed": "0", - "Pos": "10,700", - "Size": "232,254" + "Pos": "5,350", + "Size": "192,218" }, "###Timing": { "Collapsed": "0", - "Pos": "10,300", - "Size": "169,168" + "Pos": "5,150", + "Size": "141,150" }, "Debug##Default": { "Collapsed": "0", @@ -58,7 +53,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "109,114" + "Size": "92,99" } } } diff --git a/simgui.json b/simgui.json index df6b883..5543b54 100644 --- a/simgui.json +++ b/simgui.json @@ -1,15 +1,37 @@ { "HALProvider": { - "Addressable LEDs": { - "window": { - "visible": true + "Other Devices": { + "navX-Sensor[0]": { + "header": { + "open": true + } } } }, "NTProvider": { "types": { + "/AdvantageKit/RealOutputs/Arm/mech2d": "Mechanism2d", "/FMSInfo": "FMSInfo", - "/SmartDashboard/Auton Choice": "String Chooser" + "/LiveWindow/Ungrouped/DigitalInput[0]": "Digital Input", + "/LiveWindow/Ungrouped/DigitalInput[1]": "Digital Input", + "/LiveWindow/Ungrouped/DigitalInput[2]": "Digital Input", + "/LiveWindow/Ungrouped/DigitalInput[3]": "Digital Input", + "/LiveWindow/Ungrouped/DigitalInput[4]": "Digital Input", + "/LiveWindow/Ungrouped/PIDController[10]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[11]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[2]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[3]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[4]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[5]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[6]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[7]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[8]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[9]": "PIDController", + "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro", + "/SmartDashboard/Auton Choice": "String Chooser", + "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/Scheduler": "Scheduler" } } } diff --git a/src/main/deploy/pathplanner/barrier_cone_exit_HP.path b/src/main/deploy/pathplanner/barrier_cone_exit_HP.path new file mode 100644 index 0000000..cdfb3b8 --- /dev/null +++ b/src/main/deploy/pathplanner/barrier_cone_exit_HP.path @@ -0,0 +1,79 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.9070702548184761, + "y": 4.950151324031462 + }, + "prevControl": null, + "nextControl": { + "x": 2.601481140462281, + "y": 4.907885128270257 + }, + "holonomicAngle": -177.0122233890705, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "scoreHigh" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 10.0 + } + }, + { + "anchorPoint": { + "x": 4.737149161798324, + "y": 5.1194000825501655 + }, + "prevControl": { + "x": 3.8913259734623296, + "y": 4.883129462498332 + }, + "nextControl": { + "x": 5.091721488360994, + "y": 5.218445633210695 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": 1.0, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.389217083121666, + "y": 7.233294505651512 + }, + "prevControl": { + "x": 6.493607323152147, + "y": 7.18789191484633 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": 1.0, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "isReversed": null, + "markers": [] +} \ No newline at end of file diff --git a/src/main/java/team3176/robot/Constants.java b/src/main/java/team3176/robot/Constants.java new file mode 100644 index 0000000..7420365 --- /dev/null +++ b/src/main/java/team3176/robot/Constants.java @@ -0,0 +1,78 @@ +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package team3176.robot; + +import edu.wpi.first.wpilibj.RobotBase; +import java.util.Map; + +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 boolean invalidRobotAlertSent = false; + + public static RobotType getRobot() { + if (!disableHAL && RobotBase.isReal()) { + if (robot == RobotType.ROBOT_SIMBOT) { // Invalid robot selected + if (!invalidRobotAlertSent) { + invalidRobotAlertSent = true; + } + return RobotType.ROBOT_2023C; + } else { + return robot; + } + } else { + return robot; + } + } + + public static Mode getMode() { + switch (getRobot()) { + case ROBOT_2023C: + case ROBOT_2023P: + return RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; + + case ROBOT_SIMBOT: + return Mode.SIM; + + default: + return Mode.REAL; + } + } + + public static final Map logFolders = + Map.of(RobotType.ROBOT_2023C, "/media/sda2/"); + + public static enum RobotType { + ROBOT_2023C, + ROBOT_2023P, + ROBOT_SIMBOT + } + + public static enum Mode { + REAL, + REPLAY, + SIM + } + + // Function to disable HAL interaction when running without native libs + public static boolean disableHAL = false; + + public static void disableHAL() { + disableHAL = true; + } + + /** Checks whether the robot the correct robot is selected when deploying. */ + public static void main(String... args) { + if (robot == RobotType.ROBOT_SIMBOT) { + System.err.println("Cannot deploy, invalid robot selected: " + robot.toString()); + System.exit(1); + } + } +} diff --git a/src/main/java/team3176/robot/Robot.java b/src/main/java/team3176/robot/Robot.java index 887efba..971a4a3 100644 --- a/src/main/java/team3176/robot/Robot.java +++ b/src/main/java/team3176/robot/Robot.java @@ -4,11 +4,19 @@ package team3176.robot; +import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggedPowerDistribution; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 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; @@ -34,15 +42,63 @@ public class Robot extends LoggedRobot{ 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("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); + switch (BuildConstants.DIRTY) { + case 0: + logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + logger.recordMetadata("GitDirty", "Unknown"); + break; + } + switch (Constants.getMode()) { + case REAL: + String folder = Constants.logFolders.get(Constants.getRobot()); + if (folder != null) { + logger.addDataReceiver(new WPILOGWriter(folder)); + } + logger.addDataReceiver(new NT4Publisher()); + if (Constants.getRobot() == RobotType.ROBOT_2023C) { + LoggedPowerDistribution.getInstance(50, ModuleType.kRev); + } + break; + + case SIM: + System.out.println("[init] starting simulation"); + logger.addDataReceiver(new WPILOGWriter("./log/")); + logger.addDataReceiver(new NT4Publisher()); + break; + + case REPLAY: + String path = LogFileUtil.findReplayLog(); + logger.setReplaySource(new WPILOGReader(path)); + logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(path, "_sim"))); + break; + } + setUseTiming(Constants.getMode() != Constants.Mode.REPLAY); + logger.start(); + + m_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(); + // 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(); } diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index 55107a8..2af390a 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -40,6 +40,8 @@ import team3176.robot.subsystems.superstructure.IntakeCone; 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 @@ -63,7 +65,7 @@ public class RobotContainer { // private final Compressor m_Compressor; private final Drivetrain m_Drivetrain; - private final RobotState m_RobotState; + private final VisionDual m_Vision; private final Superstructure m_Superstructure; private SendableChooser m_autonChooser; @@ -80,7 +82,7 @@ public RobotContainer() { m_IntakeCone = IntakeCone.getInstance(); m_PDH = new PowerDistribution(Hardwaremap.PDH_CID, ModuleType.kRev); - m_RobotState = RobotState.getInstance(); + m_Vision = VisionDual.getInstance(); m_Superstructure = Superstructure.getInstance(); m_Drivetrain.setDefaultCommand(new SwerveDrive( () -> m_Controller.getForward(), @@ -109,9 +111,6 @@ private void configureBindings() { 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()); - //.whileTrue(new InstantCommand(() -> m_Drivetrain.resetFieldOrientation(), m_Drivetrain)); - //m_Controller.getTransStick_Button3().whileTrue(m_Superstructure.prepareScoreMid()); - //m_Controller.getTransStick_Button3().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()); @@ -119,6 +118,7 @@ private void configureBindings() { 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())); //m_Controller.getTransStick_Button10() // .onFalse(new InstantCommand(() -> m_Drivetrain.setDriveMode(driveMode.DRIVE), m_Drivetrain)); @@ -135,32 +135,11 @@ private void configureBindings() { () -> m_Controller.getStrafe()) ); - //m_Controller.getRotStick_Button2() - // .whileTrue(new InstantCommand(() -> m_Drivetrain.setCoordType(coordType.ROBOT_CENTRIC), m_Drivetrain)); - //m_Controller.getRotStick_Button2() - // .onFalse(new InstantCommand(() -> m_Drivetrain.setCoordType(coordType.FIELD_CENTRIC), m_Drivetrain)); - //m_Controller.getRotStick_Button2().whileTrue(new teleopPath()); - //m_Controller.getRotStick_Button2().whileTrue(new FeederPID("left")); - //m_Controller.getRotStick_HAT_270().whileTrue(new FeederPID("left")); - //m_Controller.getRotStick_HAT_90().whileTrue(new FeederPID("right")); - //m_Controller.getRotStick_Button2().onFalse(new SwerveDrive( - // () -> m_Controller.getForward(), - // () -> m_Controller.getStrafe(), - // () -> m_Controller.getSpin()) - //); - - //m_Controller.getRotStick_Button3().whileTrue(m_Superstructure.intakeConeHumanPlayer()); - //m_Controller.getRotStick_Button3().onFalse(m_Superstructure.prepareCarry()); m_Controller.getRotStick_Button3().whileTrue(new InstantCommand(() -> m_Drivetrain.setBrakeMode()).andThen(new SwerveDefense())); m_Controller.getRotStick_Button4().whileTrue(m_Superstructure.intakeCubeHumanPlayer()); m_Controller.getRotStick_Button4().onFalse(m_Superstructure.prepareCarry()); - // m_Controller.getRotStick_Button4() - // .whileTrue(new InstantCommand(() -> m_Drivetrain.setCoordType(coordType.ROBOT_CENTRIC), m_Drivetrain)); - // m_Controller.getRotStick_Button4() - // .onFalse(new InstantCommand(() -> m_Drivetrain.setCoordType(coordType.FIELD_CENTRIC), m_Drivetrain)); - // m_Controller.getRotStick_Button4().whileTrue(new SpinLock()); m_Controller.getTransStick_Button8() .whileTrue(new InstantCommand(() -> m_Drivetrain.resetFieldOrientation(), m_Drivetrain)); @@ -194,18 +173,6 @@ private void configureBindings() { m_Controller.operator.y().whileTrue(m_Claw.scoreGamePiece()); m_Controller.operator.y().onFalse(new ClawIdle()); - - //m_Controller.operator.rightBumper().whileTrue(m_IntakeCube.extendAndFreeSpin()); - //m_Controller.operator.rightBumper().whileTrue(new InstantCommand( () -> m_IntakeCone.spinVelocityPercent(-80,20))); - //m_Controller.operator.rightBumper().onFalse(new InstantCommand( () -> m_IntakeCone.spinVelocityPercent(0, 20))); -// m_Controller.operator.rightBumper().and(m_Controller.operator.leftBumper().negate()).whileTrue((m_Superstructure.groundCube())); - - // m_Controller.operator.leftBumper().whileTrue(new manuallyPositionArm( () -> - //m_Controller.operator.leftBumper().whileTrue(new armAnalogDown()); - //m_Controller.operator.leftBumper().onFalse(new armAnalogIdle()); - - //m_Controller.operator.rightBumper().whileTrue(new armAnalogUp()); - //m_Controller.operator.rightBumper().onFalse(new armAnalogIdle()); 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()); diff --git a/src/main/java/team3176/robot/commands/drivetrain/AutoBalanceCenter.java b/src/main/java/team3176/robot/commands/drivetrain/AutoBalanceCenter.java deleted file mode 100644 index da78ae2..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/AutoBalanceCenter.java +++ /dev/null @@ -1,87 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.drivetrain.Drivetrain; -import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; -public class AutoBalanceCenter extends CommandBase { - private Drivetrain m_Drivetrain; - private enum states {DRIVE,CORRECTING,WAITING,DONE}; - private states currentState; - private double forward = 0.0; - private double deadbandDegrees = 8; - private Timer correctionTimer = new Timer(); - private Timer waitTimer = new Timer(); - public AutoBalanceCenter() { - m_Drivetrain = Drivetrain.getInstance(); - addRequirements(m_Drivetrain); - } - @Override - public void initialize() { - // TODO Auto-generated method stub - m_Drivetrain.setBrakeMode(); - currentState = states.DRIVE; - } - @Override - public void execute() { - //double Kp = 0.1; - switch(currentState) { - case CORRECTING: - if(correctionTimer.get() < 0.5){ - m_Drivetrain.drive(-forward, 0, 0, Drivetrain.coordType.ROBOT_CENTRIC); - } else { - currentState = states.WAITING; - waitTimer.restart(); - } - break; - case DONE: - break; - case DRIVE: - double forward = 0.0; - - if(m_Drivetrain.getChassisPitch() > 0 + deadbandDegrees) { - forward = 0.39; - } else if(m_Drivetrain.getChassisPitch() < 0 - deadbandDegrees) { - forward = -0.39; - } else { - currentState = states.CORRECTING; - correctionTimer.restart(); - } - - - m_Drivetrain.drive(forward, 0, 0, Drivetrain.coordType.ROBOT_CENTRIC); - break; - case WAITING: - if(waitTimer.get() < 0.5){ - m_Drivetrain.drive(0, 0, 0, Drivetrain.coordType.ROBOT_CENTRIC); - } else { - if(Math.abs(m_Drivetrain.getChassisPitch()) < deadbandDegrees) { - currentState = states.DONE; - }else { - currentState = states.DRIVE; - } - - } - - break; - default: - break; - - } - - - //Bang Bang controller! - SmartDashboard.putNumber("pitch", m_Drivetrain.getChassisPitch()); - - } - @Override - public void end(boolean interrupted) { - m_Drivetrain.setDriveMode(driveMode.DEFENSE); - } - @Override - public boolean isFinished() { - return Timer.getMatchTime() < 0.5 || currentState == states.DONE; - } - -} diff --git a/src/main/java/team3176/robot/commands/drivetrain/SubstationRun.java b/src/main/java/team3176/robot/commands/drivetrain/SubstationRun.java deleted file mode 100644 index 0b12d2c..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/SubstationRun.java +++ /dev/null @@ -1,124 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.PathPoint; -import com.pathplanner.lib.commands.PPSwerveControllerCommand; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.networktables.DoubleTopic; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; - -import edu.wpi.first.wpilibj.Timer; -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.Vision; -import edu.wpi.first.networktables.DoubleTopic; -import edu.wpi.first.networktables.DoubleSubscriber; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class SubstationRun { - private Drivetrain m_Drivetrain; - public NetworkTableInstance tableInstance; - public NetworkTable ll_lpov, ll_rpov; - public double ltv, rtv, tv, ltx, rtx, tx, lty, rty, ty, lta, rta, ta; - public DoubleTopic aprilIDTopic; - public DoubleSubscriber aprilID; - public double wantedAprilID, wantedYaw, wantedXpos, wantedYpos; - public Alliance alliance; - public PathPlannerTrajectory traja; - public PPSwerveControllerCommand swerveCommand; - private String side; - - public SubstationRun(String side){ - m_Drivetrain = Drivetrain.getInstance(); - this.side = side; - - } - - public void initialize(){ - //m_Drivetrain.setBrakeMode(); - tableInstance = NetworkTableInstance.getDefault(); - ll_lpov = tableInstance.getTable("limelight-lpov"); - ll_rpov = tableInstance.getTable("limelight-rpov"); - - if (DriverStation.isFMSAttached() && (alliance == null)) { - alliance = DriverStation.getAlliance(); - if (alliance == Alliance.Red){ - wantedAprilID = 5.0; - } else if (alliance == Alliance.Blue){ - wantedAprilID = 4.0; - } else if (alliance == Alliance.Invalid){ - wantedAprilID = 9.0; - } - } - - if (wantedAprilID == 5.0) { - if (this.side == "left") { - wantedXpos = 68; //placeholder until we get the bot - wantedYpos = 680; //placeholder until we get the bot - wantedYaw = 0;//placeholder until we get the bot - } else if (this.side == "right") { - wantedXpos = 68; //placeholder until we get the bot - wantedYpos = 68; //placeholder until we get the bot - wantedYaw = 0;//placeholder until we get the bot - } - } else if (wantedAprilID == 4.0) { - if (this.side == "left") { - wantedXpos = 68; //placeholder until we get the bot - wantedYpos = 68; //placeholder until we get the bot - wantedYaw = 0;//placeholder until we get the bot - } else if (this.side == "right") { - wantedXpos = 68; //placeholder until we get the bot - wantedYpos = 68; //placeholder until we get the bot - wantedYaw = 0;//placeholder until we get the bot - } - } - - ltv = ll_lpov.getEntry("tv").getDouble(0); - rtv = ll_rpov.getEntry("tv").getDouble(0); - - if (ltv == 1) { - - } - - - - } - public void execute(){ - Pose2d pose = m_Drivetrain.getPose(); - double xpos = pose.getX(); - double ypos = pose.getY(); - - //aprilIDTopic = limelightTable.getDoubleTopic("tid"); - aprilID = aprilIDTopic.subscribe(0.0); - double forward = 0.0; - if (aprilID.getAsDouble() == wantedAprilID ){ - - traja = PathPlanner.generatePath( - new PathConstraints(1, 1), - new PathPoint(new Translation2d(wantedXpos, wantedYpos),new Rotation2d(0), pose.getRotation()), // position, heading - new PathPoint(new Translation2d(wantedXpos,wantedYpos),new Rotation2d(0), new Rotation2d(wantedYaw)) // position, heading - ); - swerveCommand = new PPSwerveControllerCommand(traja, m_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 - false, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true - m_Drivetrain); - swerveCommand.initialize(); - } - } -} - diff --git a/src/main/java/team3176/robot/commands/drivetrain/SwerveDrive.java b/src/main/java/team3176/robot/commands/drivetrain/SwerveDrive.java index 8c11320..118a355 100644 --- a/src/main/java/team3176/robot/commands/drivetrain/SwerveDrive.java +++ b/src/main/java/team3176/robot/commands/drivetrain/SwerveDrive.java @@ -35,7 +35,7 @@ public void initialize() { public void execute() { drivetrain.drive(forwardCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND *0.7, strafeCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND *0.7, - spinCommand.getAsDouble()*100); + spinCommand.getAsDouble()*3); } @Override diff --git a/src/main/java/team3176/robot/commands/drivetrain/SwerveDriveTune.java b/src/main/java/team3176/robot/commands/drivetrain/SwerveDriveTune.java deleted file mode 100644 index 466cbc7..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/SwerveDriveTune.java +++ /dev/null @@ -1,51 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.constants.DrivetrainConstants; -import team3176.robot.subsystems.drivetrain.Drivetrain; -import team3176.robot.subsystems.drivetrain.Drivetrain.coordType; -import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; - - -public class SwerveDriveTune extends CommandBase { - private Drivetrain drivetrain = Drivetrain.getInstance(); - - - private BooleanSupplier isFieldCentric; - private BooleanSupplier isRobotCentric; - - public SwerveDriveTune() { - //this.isFieldCentric = isFieldCentric; - //this.isRobotCentric = isRobotCentric; - addRequirements(drivetrain); - - } - - @Override - public void initialize() { - drivetrain.setDriveMode(driveMode.DRIVE); - drivetrain.setSpinLock(false); - } - - @Override - public void execute() { - - if(drivetrain.currentCoordType == coordType.FIELD_CENTRIC) { - //drivetrain.setCoordType(coordType.FIELD_CENTRIC); - drivetrain.resetFieldOrientation(); - } - //if(isRobotCentric.getAsBoolean()) { - // drivetrain.setCoordType(coordType.ROBOT_CENTRIC); - //} - drivetrain.drive(0,0,0); - } - - @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/SwerveTimedDrive.java b/src/main/java/team3176/robot/commands/drivetrain/SwerveTimedDrive.java deleted file mode 100644 index 2de8a5b..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/SwerveTimedDrive.java +++ /dev/null @@ -1,43 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.drivetrain.*; -import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; - - -public class SwerveTimedDrive extends CommandBase{ - double forwardCommand, strafeCommand, spinCommand; - Drivetrain drivetrain = Drivetrain.getInstance(); - /** - * For use in Autonmous. Call with the decoration ".withTimeout(seconds)" - * example: SwerveTimedDrive(0.1,0.5,1.2).withTimeout(2); - * @param forwardCommand ft/s - * @param strafeCommand ft/s - * @param spinCommand rad/s - */ - public SwerveTimedDrive(Double forwardCommand, Double strafeCommand, Double spinCommand) { - this.forwardCommand = forwardCommand; - this.strafeCommand = strafeCommand; - this.spinCommand = spinCommand; - addRequirements(drivetrain); - } - - @Override - public void initialize() { - drivetrain.setDriveMode(driveMode.DRIVE); - drivetrain.setSpinLock(false); - } - - @Override - public void execute() { - drivetrain.drive(forwardCommand, strafeCommand, spinCommand); - } - - @Override - public boolean isFinished() { return false; } - - @Override - public void end(boolean interrupted) { - drivetrain.drive(0.0, 0.0, 0.0); - } -} diff --git a/src/main/java/team3176/robot/commands/drivetrain/Turbo.java b/src/main/java/team3176/robot/commands/drivetrain/Turbo.java index 41f2e38..df6bc2b 100644 --- a/src/main/java/team3176/robot/commands/drivetrain/Turbo.java +++ b/src/main/java/team3176/robot/commands/drivetrain/Turbo.java @@ -35,7 +35,7 @@ public void initialize() { public void execute() { 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()*120); + spinCommand.getAsDouble()*7); } @Override diff --git a/src/main/java/team3176/robot/commands/drivetrain/TurtleSpeed.java b/src/main/java/team3176/robot/commands/drivetrain/TurtleSpeed.java index 6c7310a..5e96fe5 100644 --- a/src/main/java/team3176/robot/commands/drivetrain/TurtleSpeed.java +++ b/src/main/java/team3176/robot/commands/drivetrain/TurtleSpeed.java @@ -34,7 +34,7 @@ public void initialize() { public void execute() { drivetrain.drive(forwardCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND *0.3, strafeCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND *0.3, - spinCommand.getAsDouble()*100); + spinCommand.getAsDouble()*6); } @Override diff --git a/src/main/java/team3176/robot/commands/superstructure/intakecube/PoopCube.java b/src/main/java/team3176/robot/commands/superstructure/intakecube/PoopCube.java index d31fab2..c468315 100644 --- a/src/main/java/team3176/robot/commands/superstructure/intakecube/PoopCube.java +++ b/src/main/java/team3176/robot/commands/superstructure/intakecube/PoopCube.java @@ -26,7 +26,7 @@ public void initialize() { m_Arm.armSetPositionOnce(SuperStructureConstants.ARM_ZERO_POS); m_IntakeCube.spinConveyor(0.4); - m_IntakeCube.spinIntake(.85); + m_IntakeCube.spinIntake(1); m_Claw.setClawMotor(-0.6, 5); } @@ -35,7 +35,7 @@ public void initialize() public void execute() { m_IntakeCube.spinConveyor(0.6); - m_IntakeCube.spinIntake(.85); + m_IntakeCube.spinIntake(1); m_Claw.setClawMotor(-0.6, 5); } diff --git a/src/main/java/team3176/robot/constants/DrivetrainConstants.java b/src/main/java/team3176/robot/constants/DrivetrainConstants.java index 65ef756..180ca88 100644 --- a/src/main/java/team3176/robot/constants/DrivetrainConstants.java +++ b/src/main/java/team3176/robot/constants/DrivetrainConstants.java @@ -5,75 +5,18 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; //import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint; import edu.wpi.first.math.util.Units; -import team3176.robot.Robot; -import team3176.robot.constants.RobotConstants; -import team3176.robot.constants.RobotConstants.RobotType; - public class DrivetrainConstants extends DrivetrainHardwareMap { // IDs for Drivetrain motors and solenoids - - - - public static final double POD0_LOCATION_X = .3; // <---TODO: REPLACE WITH MEASURED VALUES - public static final double POD0_LOCATION_Y = .3; // <---TODO: REPLACE WITH MEASURED VALUES - public static final double POD1_LOCATION_X = .3; // <---TODO: REPLACE WITH MEASURED VALUES - public static final double POD1_LOCATION_Y = -.3; // <---TODO: REPLACE WITH MEASURED VALUES - public static final double POD2_LOCATION_X = -0.3; // <---TODO: REPLACE WITH MEASURED VALUES - public static final double POD2_LOCATION_Y = 0.3; // <---TODO: REPLACE WITH MEASURED VALUES - public static final double POD3_LOCATION_X = -0.3; // <---TODO: REPLACE WITH MEASURED VALUES - public static final double POD3_LOCATION_Y = -0.3; // <---TODO: REPLACE WITH MEASURED VALUES // Drivetrain dimensions for kinematics and odometry - public static final double EBOT_LENGTH_IN_INCHES_2023 = 29.5; - public static final double EBOT_LENGTH_IN_METERS_2023 = Units.inchesToMeters(EBOT_LENGTH_IN_INCHES_2023); - public static final double PROTCHASSIS_LENGTH_IN_INCHES_2023 = 24.35; - public static final double PROTCHASSIS_LENGTH_IN_METERS_2023 = Units.inchesToMeters(PROTCHASSIS_LENGTH_IN_INCHES_2023); - public static final double PRODBOT_LENGTH_IN_INCHES_2023 = 30; - public static final double PRODBOT_LENGTH_IN_METERS_2023 = Units.inchesToMeters(PRODBOT_LENGTH_IN_INCHES_2023); - - - public static final double EBOT_WIDTH_IN_INCHES_2023 = 30; - public static final double EBOT_WIDTH_IN_METERS_2023 = Units.inchesToMeters(EBOT_WIDTH_IN_INCHES_2023); - public static final double PROTCHASSIS_WIDTH_IN_INCHES_2023 = 28.75; - public static final double PROTCHASSIS_WIDTH_IN_METERS_2023 = Units.inchesToMeters(PROTCHASSIS_WIDTH_IN_INCHES_2023); - public static final double PRODBOT_WIDTH_IN_INCHES_2023 = 29.5; - public static final double PRODBOT_WIDTH_IN_METERS_2023 = Units.inchesToMeters(PRODBOT_WIDTH_IN_INCHES_2023); - - -// public static double LENGTH = (RobotConstants.isEBOT()) ? EBOT_LENGTH_IN_METERS_2023 : 0 ; -// public static double WIDTH = (RobotConstants.isEBOT()) ? EBOT_WIDTH_IN_METERS_2023 : 0 ; - - public static double LENGTH = PROTCHASSIS_LENGTH_IN_METERS_2023; - public static double WIDTH = PROTCHASSIS_WIDTH_IN_METERS_2023; - - - public static double DRIVE_ENCODER_UNITS_PER_REVOLUTION; + public static final double EBOT_LENGTH_IN_METERS_2023 = Units.inchesToMeters(24.3); + public static final double EBOT_WIDTH_IN_METERS_2023 = Units.inchesToMeters(28.75); + public static final double LENGTH = EBOT_LENGTH_IN_METERS_2023; + public static final double WIDTH = EBOT_WIDTH_IN_METERS_2023; public static final double WHEEL_DIAMETER_INCHES = 3.00; // Inches - public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(WHEEL_DIAMETER_INCHES); // Inches - public static final double WHEEL_DIAMETER_FEET = WHEEL_DIAMETER_INCHES / 12.0 ; // Inches - public static final double WHEEL_DIAMETER = WHEEL_DIAMETER_INCHES; - - public static final double MAX_WHEEL_SPEED_INCHES_PER_SECOND = 192.00; //165.48; // inches/s - public static final double MAX_WHEEL_SPEED_METERS_PER_SECOND = Units.inchesToMeters(MAX_WHEEL_SPEED_INCHES_PER_SECOND); - public static final double MAX_WHEEL_SPEED_FEET_PER_SECOND = MAX_WHEEL_SPEED_INCHES_PER_SECOND / 12.0 ; // 13.5 ft/s - public static final double MAX_WHEEL_SPEED = MAX_WHEEL_SPEED_INCHES_PER_SECOND; - - public static final double MAX_ACCEL_INCHES_PER_SECOND = 60.0; // inches/s*s - public static final double MAX_ACCEL_METERS_PER_SECOND = Units.inchesToMeters(MAX_ACCEL_INCHES_PER_SECOND); // meters/(s*s) - public static final double MAX_ACCEL_FEET_PER_SECOND = MAX_ACCEL_INCHES_PER_SECOND / 12.0; // Unknown units - likely ft/(s*s) - public static final double MAX_ACCEL = MAX_ACCEL_INCHES_PER_SECOND; - - public static final double MAX_ROT_SPEED_RADIANS_PER_SECOND = 8.0; // 5.0; // rad/s - public static final double MAX_ROT_SPEED = 5.0; // rad/s - - public static final double MAX_ROT_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 2; // rad/s*s - - public static final double NON_TURBO_PERCENT_OUT_CAP = 0.7; //1; //TODO: CHANGE BACK and fix for AUTON if needed - - //public static final double LENGTH_CENTER_TO_CENTER = 23.5; - //public static final double WIDTH_CENTER_TO_CENTER = 23.5; + public static final double MAX_WHEEL_SPEED_METERS_PER_SECOND = Units.feetToMeters(14.0); public static final SwerveDriveKinematics DRIVE_KINEMATICS = new SwerveDriveKinematics( new Translation2d(LENGTH / 2.0, -WIDTH / 2.0), //FR where +x=forward and -y=starboard new Translation2d(LENGTH / 2.0, WIDTH / 2.0), //FL where +x=forward and +y=port @@ -81,35 +24,12 @@ public class DrivetrainConstants extends DrivetrainHardwareMap { new Translation2d(-LENGTH / 2.0, -WIDTH/ 2.0) //BR where -x=backward(aft) and -y=starboard ); - /* NOTE: Related to above decomposition of pod locations where - * O (Chassis center) = 0,0, - * FR (front right pod) = -x -y, - * FL (front left pod) = +x, +y, - * BL (back left pod) = -x, +y, - * BR (back right pod) = -x -y - * and +x is considered the forward direction (fore) and +y is - * considered the direction to port on the chassis - * IT MUST BE NOTED that with current orientation of NavX-MXP on the 2021 bot - * the raw navx gyro/angle values from NavX-MXP give +y = fore and +x = starboard. - * THEREFORE, all raw gyro angles retrieved from the NavX-MXP must be - * rotated by 90-degrees such that +x = fore and +y = port. - * - * Quick primer on directions: - * - * Forward facing side = "fore" - * FL-------FR - * | | - * This side = "port" | O | This side = "starboard" - * | | - * BL-------BR - * Backward direction side = "aft" - * - */ + //SwervePod Constants + private static final double AZIMUTH_GEAR_RATIO = 70.0 / 1.0; // Is the Versa gearbox btwn motor & encoder + public static final double THRUST_GEAR_RATIO = (14.0/22.0) * (15.0/45.0); - // Below line contains offset needed to rotate raw navx angle output such that +x=fore and +y=port - public static final double GYRO_ROTATIONAL_OFFSET_2022_practiceBot = 0; - public static final double GYRO_ROTATIONAL_OFFSET_2022_actualBot = 0; - public static final double GYRO_ROTATIONAL_OFFSET_FOR_RIO_MOUNTING = (LoggerConstants.IS_PRACTICE_BOT) ? GYRO_ROTATIONAL_OFFSET_2022_practiceBot : GYRO_ROTATIONAL_OFFSET_2022_actualBot; + public static final double AZIMUTH_ENCODER_UNITS_PER_REVOLUTION = 4096; + public static final double THRUST_ENCODER_UNITS_PER_REVOLUTION = 2048; public static final double[] AUTON_THETA_CONTROLLER_PIDF = { 3.0 /*kP*/, 0.0 /*kI*/, 0.0 /*kD*/, 0.0 /*kF*/}; @@ -122,9 +42,7 @@ public class DrivetrainConstants extends DrivetrainHardwareMap { public static final double P_X_Controller = 1; public static final double P_Y_Controller = 1; public static final double P_Theta_Controller = 1; - - //public static final double DEGREES_PER_SECOND_TO_METERS_PER_SECOND_OF_WHEEL = (3.25*Math.PI)/360; - public static final double DEGREES_PER_SECOND_TO_METERS_PER_SECOND_OF_WHEEL = (WHEEL_DIAMETER_METERS * Math.PI)/360; + } diff --git a/src/main/java/team3176/robot/constants/DrivetrainHardwareMap.java b/src/main/java/team3176/robot/constants/DrivetrainHardwareMap.java index bd17712..8174699 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 final SwervePodHardwareID pod001 = + public static SwervePodHardwareID pod001 = new SwervePodHardwareID( 10, 12, -172.135); - public static final SwervePodHardwareID pod002 = + public static SwervePodHardwareID pod002 = new SwervePodHardwareID( 20, 22, -225.186); - public static final SwervePodHardwareID pod003 = + public static SwervePodHardwareID pod003 = new SwervePodHardwareID( 30, 32, -40); - public static final SwervePodHardwareID pod004 = + public static SwervePodHardwareID pod004 = new SwervePodHardwareID( 40, 42, 140.463); //120.5 - public static final SwervePodHardwareID pod005 = + public static SwervePodHardwareID pod005 = new SwervePodHardwareID( 13, 14, -30.525); - public static final SwervePodHardwareID pod006 = + public static SwervePodHardwareID pod006 = new SwervePodHardwareID( 23, 24, 120.556); - public static final SwervePodHardwareID pod007 = + public static SwervePodHardwareID pod007 = new SwervePodHardwareID( 33, 34, 125.508); - public static final SwervePodHardwareID pod008 = + public static SwervePodHardwareID pod008 = new SwervePodHardwareID( 43, 44, -173); - public static final SwervePodHardwareID pod009 = + public static SwervePodHardwareID pod009 = new SwervePodHardwareID( 15, 16, -358.330); - 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 SwervePodHardwareID FR = pod009; + public static SwervePodHardwareID FL = pod008; + public static SwervePodHardwareID BL = pod006; + public static SwervePodHardwareID BR = pod003; // public static final int THRUST_FR_CID = FR.THRUST_CID; // public static final int THRUST_FL_CID = FL.THRUST_CID; @@ -45,6 +45,7 @@ public class DrivetrainHardwareMap { AZIMUTH_ABS_ENCODER_OFFSET_POSITION = { FR.OFFSET+180, FL.OFFSET+90, BL.OFFSET, BR.OFFSET-90}; //TODO: I think these offsets are wrong. I would double check the pods by setting them each as if they were FR zero and remeasuring the offsets + //CAN IDs static to the frame public static final int STEER_FR_CID = 11; diff --git a/src/main/java/team3176/robot/constants/SuperStructureConstants.java b/src/main/java/team3176/robot/constants/SuperStructureConstants.java index e6a81a1..b589325 100644 --- a/src/main/java/team3176/robot/constants/SuperStructureConstants.java +++ b/src/main/java/team3176/robot/constants/SuperStructureConstants.java @@ -48,4 +48,5 @@ public class SuperStructureConstants { 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_SIM_OFFSET = 70 + ARM_ZERO_POS; } diff --git a/src/main/java/team3176/robot/constants/SwervePodConstants2022.java b/src/main/java/team3176/robot/constants/SwervePodConstants2022.java deleted file mode 100644 index c0acb6d..0000000 --- a/src/main/java/team3176/robot/constants/SwervePodConstants2022.java +++ /dev/null @@ -1,87 +0,0 @@ -package team3176.robot.constants; - -public final class SwervePodConstants2022 extends DrivetrainHardwareMap { - - private static final double WHEEL_DIAMETER = DrivetrainConstants.WHEEL_DIAMETER; // in inches - //private static final double AZIMUTH_GEAR_RATIO = 70.0 / 1.0; // Is the Versa gearbox btwn motor & encoder - private static final double AZIMUTH_GEAR_RATIO = 70.0 / 1.0; // Is the Versa gearbox btwn motor & encoder - //private static final double AZIMUTH_GEAR_RATIO = 1.0 / 1.0; // Is the Versa gearbox btwn motor & encoder - //private static final double THRUST_GEAR_RATIO = (54.0 / 14.0) * (48.0 / 30.0); // 216/35? - public static final double THRUST_GEAR_RATIO = (14.0/22.0) * (15.0/45.0); - - - - /* Choose so that Talon does not report sensor out of phase */ - public static boolean[] SENSOR_PHASE_ = {false, false, false, false}; - - /** - * Choose based on what direction you want to be positive, - * this does not affect motor invert. - */ - public static boolean[] MOTOR_INVERTED_ = {false, false, false, false}; - - - - public static final double AZIMUTH_ENCODER_UNITS_PER_REVOLUTION = 4096; - public static final int AZIMUTH_PID_SLOT_ID = 0; - public static final int AZIMUTH_PID_LOOP_ID = 0; - public static final int AZIMUTH_ENCODER_TIMEOUT_MS = 0; - - public static final double THRUST_ENCODER_UNITS_PER_REVOLUTION = 2048; - public static final int[] TALON_THRUST_PID_SLOT_ID = {0, 0, 0, 0}; - public static final int[] TALON_THRUST_PID_LOOP_ID = {0, 0, 0, 0}; - public static final int[] TALON_THRUST_PID_TIMEOUT_MS = {0, 0, 0, 0}; - - public static final double[][] THRUST_PID_2022 = { - /* kP */ {0.15, 0.15, 0.15, 0.15}, - /* kI */ {0.001, 0.001, 0.001, 0.001}, - /* kD */ {2.0, 2.0, 2.0, 2.0}, - /* kF */ {0.0065, 0.0065, 0.0065, 0.0065}, // Feed forward gain constant - /* I-Zne */ {150.0, 150.0, 150.0, 150.0} // The range of error for kI to take affect (like a reverse deadband) - }; - - public static double THRUST_PID[][] = THRUST_PID_2022; - - // BR P: 2.41, I: 0.0, D: 152.0, F: 0.0 - - public static final double[][] AZIMUTH_PID_2022 = { - - {1.3, 1.3, 1.3, 1.3}, - {0.00,0.00,0.00,0.00}, - {.08, .08, .08, .08}, - {0.0, 0.0, 0.0, 0.0}, // Feed forward gain constant//<-Jared N550 internal encoder - {0, 0.0, 0.0, 0.0}, //kIz constant//<-Jared N550 internal encoder - {0.6000000238418579, 1.0, 1.0, 1.0}, //kMaxOutput//<-Jared N550 internal encoder - {-0.6000000238418579, -1.0, -1.0, -1.0} //kMinOutput//<-Jared N550 internal encoder - }; - - public static final double[] AZIMUTH_RAMPRATE = { 0.0, 0.0, 0.0, 0.0 }; - - public static double AZIMUTH_PID[][] = AZIMUTH_PID_2022; - - - /* OFFSETS: Corresponds to selftest output from CTRE Phoenix tool. - * Look for the absolute position encoder value. Should say something - like: "Pulsewidth/MagEnc(abs)" - * Used solely for the AZIMUTH Encoder. - */ - - - public static final double[] AZIMUTH_OFFSET_2022 = {0.0, 0.0, 0.0, 0.0}; - //public static final double[] AZIMUTH_OFFSET_2022 = {2703, 339, 2863, 2757}; // 2021 Bot //WTFOffsets - public static final double[] AZIMUTH_OFFSET =AZIMUTH_OFFSET_2022; - - public static final double CHASSIS_SPEED_MAX_EMPIRICAL_FEET_PER_SECOND = 14.00; //estimated loaded speed per Nathan; - - - public static final double P_MODULE_THRUST_CONTROLLER = 1; - public static final double[] P_MODULE_TURNING_CONTROLLER = {/*OLD P VALUES 1, 0, 0.3, 0.03*/ 0.08 /*kP*/, 0.0 /*kI*/, 0, 0.0}; - public static final double MAX_MODULE_ANGULAR_SPEED_RADIANS_PER_SECOND = 18000 * Math.PI; - //public static final double MAX_MODULE_ANGULAR_SPEED_RADIANS_PER_SECOND = 1; - public static final double MAX_MODULE_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 144000 * Math.PI; - //public static final double MAX_MODULE_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 1; - - public static final double AZIMUTH_SPARKMAX_MAX_OUTPUTPERCENT = .50; - -} - diff --git a/src/main/java/team3176/robot/subsystems/controller/Controller.java b/src/main/java/team3176/robot/subsystems/controller/Controller.java index d6badfa..49b8faa 100644 --- a/src/main/java/team3176/robot/subsystems/controller/Controller.java +++ b/src/main/java/team3176/robot/subsystems/controller/Controller.java @@ -251,7 +251,7 @@ public double getStrafe() { public double getSpin() { if(Math.abs(rotStick.getX()) < 0.06) return 0.0; - return 0.2 * ControllerConstants.SPIN_AXIS_INVERSION * (Math.pow(rotStick.getX(), 1) / 7.0); + return ControllerConstants.SPIN_AXIS_INVERSION * rotStick.getX(); } /** diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index e3d76a0..eb305ff 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -3,79 +3,47 @@ // the WPILib BSD license file in the root directory of this project. package team3176.robot.subsystems.drivetrain; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonFX; -import com.kauailabs.navx.frc.AHRS; -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.PathPoint; -import com.pathplanner.lib.commands.PPSwerveControllerCommand; -import com.revrobotics.CANSparkMax; -import com.revrobotics.*; -import com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; + import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -// import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.networktables.DoubleArraySubscriber; -import edu.wpi.first.networktables.DoubleArrayTopic; +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; -import edu.wpi.first.networktables.NetworkTableValue; -import edu.wpi.first.networktables.TimestampedDoubleArray; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -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.SubsystemBase; -import team3176.robot.util.God.PID3176; +import team3176.robot.Constants; +import team3176.robot.Constants.Mode; import team3176.robot.constants.DrivetrainConstants; import team3176.robot.constants.SwervePodHardwareID; -import team3176.robot.constants.SwervePodConstants2022; -// import team3176.robot.util.God.PID3176; -import team3176.robot.subsystems.drivetrain.SwervePod; +import team3176.robot.subsystems.drivetrain.GyroIO.GyroIOInputs; +import team3176.robot.subsystems.vision.VisionDual; import java.util.ArrayList; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import team3176.robot.subsystems.controller.Controller; - import org.littletonrobotics.junction.Logger; -import team3176.robot.subsystems.drivetrain.DrivetrainIO.DrivetrainIOInputs; public class Drivetrain extends SubsystemBase { private static Drivetrain instance; - private AHRS m_NavX; public SwerveDriveOdometry odom; public SwerveDrivePoseEstimator poseEstimator; @@ -98,23 +66,14 @@ public enum coordType { private driveMode currentDriveMode = driveMode.DRIVE; - public TalonFX[] driveControllers = { new TalonFX(DrivetrainConstants.FR.THRUST_CID), - new TalonFX(DrivetrainConstants.FL.THRUST_CID), new TalonFX(DrivetrainConstants.BL.THRUST_CID), - new TalonFX(DrivetrainConstants.BR.THRUST_CID) }; - - public CANSparkMax[] azimuthControllers = { new CANSparkMax(DrivetrainConstants.STEER_FR_CID, MotorType.kBrushless), - new CANSparkMax(DrivetrainConstants.STEER_FL_CID, MotorType.kBrushless), - new CANSparkMax(DrivetrainConstants.STEER_BL_CID, MotorType.kBrushless), - new CANSparkMax(DrivetrainConstants.STEER_BR_CID, MotorType.kBrushless) }; + Rotation2d FieldAngleOffset = Rotation2d.fromDegrees(0.0); - private double relMaxSpeed; private double forwardCommand; private double strafeCommand; private double spinCommand; - private double spinCommandInit; // spin lock private PIDController spinLockPID; @@ -139,17 +98,21 @@ public enum driveMode { NetworkTable vision; - private boolean hasvisionsubed = false; NetworkTableEntry vision_pose; Pose2d last_pose = new Pose2d(); double lastVisionTimeStamp = 0.0; double lastVisionX = 0.0; - private final DrivetrainIO io; + Rotation2d wheelOnlyHeading = new Rotation2d(); + private final GyroIO io; + private GyroIOInputs inputs; Field2d field; + Pose3d visionPose3d; // private final DrivetrainIOInputs inputs = new DrivetrainIOInputs(); - private Drivetrain(DrivetrainIO io) { + private Drivetrain(GyroIO io) { + this.io = io; + inputs = new GyroIOInputs(); inst = NetworkTableInstance.getDefault(); table = inst.getTable("datatable"); @@ -158,17 +121,42 @@ private Drivetrain(DrivetrainIO io) { dblPub = dblTopic.publish(); field = new Field2d(); - this.io = io; - // check for duplicates assert (!SwervePodHardwareID.check_duplicates_all(DrivetrainConstants.FR, DrivetrainConstants.FL, DrivetrainConstants.BR, DrivetrainConstants.BL)); // Instantiate pods - - podFR = new SwervePod(0, driveControllers[0], azimuthControllers[0]); - podFL = new SwervePod(1, driveControllers[1], azimuthControllers[1]); - podBL = new SwervePod(2, driveControllers[2], azimuthControllers[2]); - podBR = new SwervePod(3, driveControllers[3], azimuthControllers[3]); + 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)); + break; + case ROBOT_2023P: + break; + case ROBOT_SIMBOT: + System.out.println("[init] simulated swervePods"); + podFR = new SwervePod(0, new SwervePodIOSim()); + podFL = new SwervePod(1, new SwervePodIOSim()); + podBL = new SwervePod(2, new SwervePodIOSim()); + podBR = new SwervePod(3, new SwervePodIOSim()); + break; + default: + break; + + } + } else { + podFR = new SwervePod(0, new SwervePodIO(){}); + podFL = new SwervePod(1, new SwervePodIO(){}); + podBL = new SwervePod(2, new SwervePodIO(){}); + podBR = new SwervePod(3, new SwervePodIO(){}); + } // Instantiate array list then add instantiated pods to list pods = new ArrayList(); @@ -177,8 +165,8 @@ private Drivetrain(DrivetrainIO io) { pods.add(podBL); pods.add(podBR); - m_NavX = new AHRS(SPI.Port.kMXP); - + + visionPose3d = new Pose3d(); odom = new SwerveDriveOdometry(DrivetrainConstants.DRIVE_KINEMATICS, this.getSensorYaw(), new SwerveModulePosition[] { podFR.getPosition(), @@ -188,9 +176,6 @@ private Drivetrain(DrivetrainIO io) { }, new Pose2d(0.0, 0.0, new Rotation2d())); poseEstimator = new SwerveDrivePoseEstimator(DrivetrainConstants.DRIVE_KINEMATICS, getSensorYaw(), getSwerveModulePositions(), odom.getPoseMeters()); - // TODO: update covariance matrix for vision - // poseEstimator.setVisionMeasurementStdDevs(new MatBuilder<>(Nat.N3(), - // Nat.N1()).fill(0.1,0.1,0.01)); spinLockPID = new PIDController(0.1, 0.0, 0.0); // set for max and min of degrees for Rotation2D spinLockPID.enableContinuousInput(-180, 180); @@ -198,11 +183,6 @@ private Drivetrain(DrivetrainIO io) { arraytrack = 0; angleAvgRollingWindow = 0; - // TODO: We initialize to face forward but how do we make this into a command? - // Maybe we say drive with the below parameters, but where? - /* - * // Start wheels in a forward facing direction - */ this.forwardCommand = Math.pow(10, -15); // Has to be positive to turn that direction? this.strafeCommand = 0.0; @@ -214,22 +194,16 @@ private Drivetrain(DrivetrainIO io) { // Prevents more than one instance of drivetrian public static Drivetrain getInstance() { if (instance == null) { - instance = new Drivetrain(new DrivetrainIO() { - }); + if(Constants.getMode() != Mode.REPLAY) { + instance = new Drivetrain(new GyroIONavX()); + } + else{ + instance = new Drivetrain(new GyroIO() {}); + } } return instance; } - /** - * public void drive(double forwardCommand, double strafeCommand, double - * spinCommand, int uselessVariable) { double smallNum = Math.pow(10, -15); - * //spinCommand = (spinCommand - (-1))/(1 - (-1)); //rescales spinCommand to a - * 0..1 range double angle = (spinCommand * Math.PI) + Math.PI; // <- diff coord - * system than -1..1 = 0..2Pi // This coord system is 0..1 = Pi..2Pi, & // 0..-1 - * = Pi..-2PI // right? // Fixed by new rescaling at line 140? - * pods.get(0).set(smallNum, angle); } - */ - /** * public facing drive command that allows command to specify if the command is * field centric or not @@ -240,15 +214,12 @@ public static Drivetrain getInstance() { * @param type FIELD CENTRIC or ROBOT_CENTRIC */ public void drive(double forwardCommand, double strafeCommand, double spinCommand, coordType type) { - ChassisSpeeds speed = new ChassisSpeeds(forwardCommand, strafeCommand, spinCommand); - if (type == coordType.FIELD_CENTRIC) { - Rotation2d fieldOffset = this.getPose().getRotation(); - if (DriverStation.getAlliance() == Alliance.Red) { - fieldOffset.plus(Rotation2d.fromDegrees(180)); - } - speed = ChassisSpeeds.fromFieldRelativeSpeeds(speed, fieldOffset); - } - p_drive(speed.vxMetersPerSecond, speed.vyMetersPerSecond, speed.omegaRadiansPerSecond); + + this.currentCoordType = type; + this.forwardCommand = forwardCommand; + this.strafeCommand = strafeCommand; + this.spinCommand = spinCommand; + } /** @@ -262,25 +233,6 @@ public void drive(double forwardCommand, double strafeCommand, double spinComman drive(forwardCommand, strafeCommand, spinCommand, currentCoordType); } - /** - * - * @param forwardCommand meters per second - * @param strafeCommand meters per second - * @param spinCommand meters per second - */ - private void p_drive(double forwardCommand, double strafeCommand, double spinCommand) { - this.spinCommandInit = spinCommand; - this.forwardCommand = forwardCommand; - this.strafeCommand = strafeCommand; - this.spinCommand = spinCommand; - if (isSpinLocked) { - this.spinCommand = spinLockPID.calculate(getPoseYawWrapped().getDegrees(), spinLockAngle.getDegrees()); - SmartDashboard.putNumber("SpinLockYaw",getPoseYawWrapped().getDegrees()); - } - - calculateNSetPodPositions(this.forwardCommand, this.strafeCommand, this.spinCommand); - - } /** * Robot Centric Forward, strafe, and spin to set individual pods commanded spin @@ -290,60 +242,45 @@ private void p_drive(double forwardCommand, double strafeCommand, double spinCom * @param strafeCommand meters per second * @param spinCommand meters per second */ - private void calculateNSetPodPositions(double forwardCommand, double strafeCommand, double spinCommand) { - + private void calculateNSetPodPositions() { if (currentDriveMode != driveMode.DEFENSE) { - // Create arrays for the speed and angle of each pod - // double[] podDrive = new double[4]; - // double[] podSpin = new double[4]; ChassisSpeeds curr_chassisSpeeds = 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); + } + if (isSpinLocked) { + curr_chassisSpeeds.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[] optimizedStates = new SwerveModuleState[4]; + SwerveModuleState[] realStates = new SwerveModuleState[4]; for (int idx = 0; idx < (pods.size()); idx++) { - - pods.get(idx).set_module(pod_states[idx]); - + optimizedStates[idx]=pods.get(idx).set_module(pod_states[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/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); } else if (currentDriveMode == driveMode.DEFENSE) { // Enter defensive position double smallNum = Math.pow(10, -5); - pods.get(0).set(smallNum, Rotation2d.fromRadians(1.0 * Math.PI / 8.0)); - pods.get(1).set(smallNum, Rotation2d.fromRadians(-1.0 * Math.PI / 8.0)); - pods.get(2).set(smallNum, Rotation2d.fromRadians(-3.0 * Math.PI / 8.0)); - pods.get(3).set(smallNum, Rotation2d.fromRadians(3.0 * Math.PI / 8.0)); + 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)); } } - public void stopMotors() { - // TODO: this seems to voilate a data flow overiding pods and could cause issues - // should be a state variable - for (int idx = 0; idx < (pods.size()); idx++) { - driveControllers[idx].set(ControlMode.PercentOutput, 0); - azimuthControllers[idx].set(0); - } - - } - - public void setPodsAzimuthHome() { - double smallNum = Math.pow(10, -5); - pods.get(0).set(smallNum, Rotation2d.fromRadians(0.0)); - pods.get(1).set(smallNum, Rotation2d.fromRadians(0.0)); - pods.get(2).set(smallNum, Rotation2d.fromRadians(0.0)); - pods.get(3).set(smallNum, Rotation2d.fromRadians(0.0)); - } - - public void sendPodsAzimuthToHome() { - for (int idx = 0; idx < (pods.size()); idx++) { - pods.get(idx).goHome(); - } - } - - public void setCurrentPodPosAsHome() { - for (int idx = 0; idx < (pods.size()); idx++) { - pods.get(idx).setCurrentPositionAsHome(); - } - } public void setDriveMode(driveMode wantedDriveMode) { currentDriveMode = wantedDriveMode; @@ -358,6 +295,7 @@ public Pose2d getPose() { } public void resetPose(Pose2d pose) { + wheelOnlyHeading = pose.getRotation(); odom.resetPosition(getSensorYaw(), new SwerveModulePosition[] { podFR.getPosition(), podFL.getPosition(), @@ -368,6 +306,10 @@ public void resetPose(Pose2d pose) { podFL.getPosition(), podBL.getPosition(), podBR.getPosition() }, pose); + + } + public void resetPoseToVision() { + this.resetPose(visionPose3d.toPose2d()); } public void setModuleStates(SwerveModuleState[] states) { @@ -419,7 +361,15 @@ public Rotation2d getPoseYawWrapped() { * @return Rotation2d of the yaw */ private Rotation2d getSensorYaw() { - return m_NavX.getRotation2d(); + if(Constants.getMode() == Mode.SIM) { + if(this.odom == null || this.poseEstimator == null) { + return new Rotation2d(); + } + return wheelOnlyHeading; + } + return inputs.rotation2d; + + } public Rotation2d getChassisYaw() { @@ -431,7 +381,7 @@ public Rotation2d getChassisYaw() { * @return navx pitch -180 to 180 around the X axis of the Navx */ public double getChassisPitch() { - return m_NavX.getPitch(); + return inputs.pitch; } /** @@ -439,7 +389,7 @@ public double getChassisPitch() { * @return navx roll -180 to 180 around the X axis of the Navx */ public double getChassisRoll() { - return m_NavX.getRoll(); + return inputs.roll; } @@ -492,9 +442,11 @@ public double getCurrentChassisSpeed(){ * } */ + @Override public void periodic() { - dblPub.set(3.0); + io.updateInputs(inputs); + Logger.getInstance().processInputs("Drive/gyro", inputs); //vision_lfov_pose = NetworkTableInstance.getDefault().getTable("limelight-lfov").getEntry("botpose_wpiblue"); //vision_rfov_pose = NetworkTableInstance.getDefault().getTable("limelight-rfov").getEntry("botpose_wpiblue"); @@ -528,12 +480,26 @@ public void periodic() { // SmartDashboard.putNumber("camX",cam_pose.getX()); // } last_pose = odom.getPoseMeters(); - + SwerveModulePosition[] deltas = new SwerveModulePosition[4]; + for(int i=0;i< pods.size(); i++) { + deltas[i] = pods.get(i).getDelta(); + } + Twist2d twist = DrivetrainConstants.DRIVE_KINEMATICS.toTwist2d(deltas); + wheelOnlyHeading = getPose().exp(twist).getRotation(); // update encoders this.poseEstimator.update(getSensorYaw(), getSwerveModulePositions()); this.odom.update(getSensorYaw(), getSwerveModulePositions()); + Logger.getInstance().recordOutput("Drive/Odom", getPose()); 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]))); + Logger.getInstance().recordOutput("Drive/vision_pose", visionPose3dNT); + + //new vision proposal + visionPose3d = VisionDual.getInstance().getPose3d(); + // double[] default_pose = {0.0,0.0,0.0,0.0,0.0,0.0}; // try { // double[] vision_pose_array = vision_pose.getDoubleArray(default_pose); @@ -564,7 +530,7 @@ public void periodic() { // catch (ClassCastException e) { // System.out.println("vision error" + e); // } - + calculateNSetPodPositions(); field.setRobotPose(getPose()); SmartDashboard.putData(field); @@ -591,22 +557,22 @@ public void simulationPeriodic() { } public void publishSwervePodPIDErrors(){ - double FRAzError = podFR.getAzimuthSetpoint() - podFR.getAzimuthEncoderPosition(); + double FRAzError = podFR.getAzimuthSetpoint() - podFR.getAzimuth(); double FRThrustError = podFR.getThrustSetpoint() - podFR.getThrustEncoderVelocity(); SmartDashboard.putNumber("FRAzError", FRAzError); SmartDashboard.putNumber("FRThrustError", FRThrustError); - double FLAzError = podFL.getAzimuthSetpoint() - podFL.getAzimuthEncoderPosition(); + double FLAzError = podFL.getAzimuthSetpoint() - podFL.getAzimuth(); double FLThrustError = podFL.getThrustSetpoint() - podFL.getThrustEncoderVelocity(); SmartDashboard.putNumber("FLAzError", FLAzError); SmartDashboard.putNumber("FLThrustError", FLThrustError); - double BRAzError = podBR.getAzimuthSetpoint() - podBR.getAzimuthEncoderPosition(); + double BRAzError = podBR.getAzimuthSetpoint() - podBR.getAzimuth(); double BRThrustError = podBR.getThrustSetpoint() - podBR.getThrustEncoderVelocity(); SmartDashboard.putNumber("BRAzError", BRAzError); SmartDashboard.putNumber("BRThrustError", BRThrustError); - double BLAzError = podBL.getAzimuthSetpoint() - podBL.getAzimuthEncoderPosition(); + double BLAzError = podBL.getAzimuthSetpoint() - podBL.getAzimuth(); double BLThrustError = podBL.getThrustSetpoint() - podBL.getThrustEncoderVelocity(); SmartDashboard.putNumber("BLAzError", BLAzError); SmartDashboard.putNumber("BLThrustError", BLThrustError); diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/DrivetrainIO.java b/src/main/java/team3176/robot/subsystems/drivetrain/DrivetrainIO.java deleted file mode 100644 index df2c67d..0000000 --- a/src/main/java/team3176/robot/subsystems/drivetrain/DrivetrainIO.java +++ /dev/null @@ -1,130 +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.subsystems.drivetrain; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import org.littletonrobotics.junction.LogTable; -import org.littletonrobotics.junction.inputs.LoggableInputs; - -/** Template hardware interface for a closed loop subsystem. */ -public interface DrivetrainIO{ - /** Contains all of the input data received from hardware. */ - public static class DrivetrainIOInputs implements LoggableInputs { - public double position_1 = 0.0; - public double position_2 = 0.0; - public double position_3 = 0.0; - public double position_4 = 0.0; - public double velocity_1 = 0.0; - public double velocity_2 = 0.0; - public double velocity_3 = 0.0; - public double velocity_4 = 0.0; - public double appliedVolts_1 = 0.0; - public double appliedVolts_2 = 0.0; - public double appliedVolts_3 = 0.0; - public double appliedVolts_4 = 0.0; - public double appliedVolts_5 = 0.0; - public double appliedVolts_6 = 0.0; - public double appliedVolts_7 = 0.0; - public double appliedVolts_8 = 0.0; - public double[] currentAmps_1 = new double[] {}; - public double[] currentAmps_2 = new double[] {}; - public double[] currentAmps_3 = new double[] {}; - public double[] currentAmps_4 = new double[] {}; - public double[] currentAmps_5 = new double[] {}; - public double[] currentAmps_6 = new double[] {}; - public double[] currentAmps_7 = new double[] {}; - public double[] currentAmps_8 = new double[] {}; - public double[] tempCelcius_1 = new double[] {}; - public double[] tempCelcius_2 = new double[] {}; - public double[] tempCelcius_3 = new double[] {}; - public double[] tempCelcius_4 = new double[] {}; - public double[] tempCelcius_5 = new double[] {}; - public double[] tempCelcius_6 = new double[] {}; - public double[] tempCelcius_7 = new double[] {}; - public double[] tempCelcius_8 = new double[] {}; - - public void toLog(LogTable table) { - table.put("Position 1", position_1); - table.put("Position 2", position_2); - table.put("Position 3", position_3); - table.put("Position 4", position_4); - table.put("Velocity 1", velocity_1); - table.put("Velocity 2", velocity_2); - table.put("Velocity 3", velocity_3); - table.put("Velocity 4", velocity_4); - table.put("AppliedVolts 1", appliedVolts_1); - table.put("AppliedVolts 2", appliedVolts_2); - table.put("AppliedVolts 3", appliedVolts_3); - table.put("AppliedVolts 4", appliedVolts_4); - table.put("AppliedVolts 5", appliedVolts_5); - table.put("AppliedVolts 6", appliedVolts_6); - table.put("AppliedVolts 7", appliedVolts_7); - table.put("AppliedVolts 8", appliedVolts_8); - table.put("CurrentAmps 1", currentAmps_1); - table.put("CurrentAmps 2", currentAmps_2); - table.put("CurrentAmps 3", currentAmps_3); - table.put("CurrentAmps 4", currentAmps_4); - table.put("CurrentAmps 5", currentAmps_5); - table.put("CurrentAmps 6", currentAmps_6); - table.put("CurrentAmps 7", currentAmps_7); - table.put("CurrentAmps 8", currentAmps_8); - table.put("TempCelcius 1", tempCelcius_1); - table.put("TempCelcius 2", tempCelcius_2); - table.put("TempCelcius 3", tempCelcius_3); - table.put("TempCelcius 4", tempCelcius_4); - table.put("TempCelcius 5", tempCelcius_5); - table.put("TempCelcius 6", tempCelcius_6); - table.put("TempCelcius 7", tempCelcius_7); - table.put("TempCelcius 8", tempCelcius_8); - } - - public void fromLog(LogTable table) { - position_1 = table.getDouble("Position 1", position_1); - position_2 = table.getDouble("Position 2", position_2); - position_3 = table.getDouble("Position 3", position_3); - position_4 = table.getDouble("Position 4", position_4); - velocity_1 = table.getDouble("Velocity 1", velocity_1); - velocity_2 = table.getDouble("Velocity 2", velocity_2); - velocity_3 = table.getDouble("Velocity 3", velocity_3); - velocity_4 = table.getDouble("Velocity 4", velocity_4); - appliedVolts_1 = table.getDouble("AppliedVolts 1", appliedVolts_1); - appliedVolts_2 = table.getDouble("AppliedVolts 2", appliedVolts_2); - appliedVolts_3 = table.getDouble("AppliedVolts 3", appliedVolts_3); - appliedVolts_4 = table.getDouble("AppliedVolts 4", appliedVolts_4); - appliedVolts_5 = table.getDouble("AppliedVolts 5", appliedVolts_5); - appliedVolts_6 = table.getDouble("AppliedVolts 6", appliedVolts_6); - appliedVolts_7 = table.getDouble("AppliedVolts 7", appliedVolts_7); - appliedVolts_8 = table.getDouble("AppliedVolts 8", appliedVolts_8); - currentAmps_1 = table.getDoubleArray("CurrentAmps 1", currentAmps_1); - currentAmps_2 = table.getDoubleArray("CurrentAmps 2", currentAmps_2); - currentAmps_3 = table.getDoubleArray("CurrentAmps 3", currentAmps_3); - currentAmps_4 = table.getDoubleArray("CurrentAmps 4", currentAmps_4); - currentAmps_5 = table.getDoubleArray("CurrentAmps 5", currentAmps_5); - currentAmps_6 = table.getDoubleArray("CurrentAmps 6", currentAmps_6); - currentAmps_7 = table.getDoubleArray("CurrentAmps 7", currentAmps_7); - currentAmps_8 = table.getDoubleArray("CurrentAmps 8", currentAmps_8); - tempCelcius_1 = table.getDoubleArray("TempCelcius 1", tempCelcius_1); - tempCelcius_2 = table.getDoubleArray("TempCelcius 2", tempCelcius_2); - tempCelcius_3 = table.getDoubleArray("TempCelcius 3", tempCelcius_3); - tempCelcius_4 = table.getDoubleArray("TempCelcius 4", tempCelcius_4); - tempCelcius_5 = table.getDoubleArray("TempCelcius 5", tempCelcius_5); - tempCelcius_6 = table.getDoubleArray("TempCelcius 6", tempCelcius_6); - tempCelcius_7 = table.getDoubleArray("TempCelcius 7", tempCelcius_7); - tempCelcius_8 = table.getDoubleArray("TempCelcius 8", tempCelcius_8); - } - } - - /** Updates the set of loggable inputs. */ - public default void updateInputs(DrivetrainIOInputs inputs) {} - - /** Run open loop at the specified voltage. */ - public default void setVoltage(double volts) {} - - /** Encoder Position of the Drivetrain */ - public default void setDrivetrainPosition(double position) {} - - public default void setDrivetrainVelocity(double velocity) {} -} diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/GyroIO.java b/src/main/java/team3176/robot/subsystems/drivetrain/GyroIO.java new file mode 100644 index 0000000..6e3d36a --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/drivetrain/GyroIO.java @@ -0,0 +1,46 @@ +// 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.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{ + /** Contains all of the input data received from hardware. */ + public static class GyroIOInputs implements LoggableInputs{ + double pitch =0.0; + double yaw =0.0; + double roll =0.0; + Rotation2d rotation2d; + GyroIOInputs() { + rotation2d = new Rotation2d(); + } + @Override + public void toLog(LogTable table) { + table.put("Pitch", pitch); + table.put("Yaw", yaw); + table.put("Roll", roll); + table.put("rotation2d",rotation2d.getRadians()); + } + + @Override + public void fromLog(LogTable table) { + pitch = table.getDouble("Pitch", pitch); + yaw = table.getDouble("Yaw", yaw); + roll = table.getDouble("Roll", roll); + rotation2d = Rotation2d.fromRadians(table.getDouble("rotation2d", rotation2d.getRadians())); + } + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(GyroIOInputs inputs) {} + + public default void reset() {} +} diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/GyroIONavX.java b/src/main/java/team3176/robot/subsystems/drivetrain/GyroIONavX.java new file mode 100644 index 0000000..602a498 --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/drivetrain/GyroIONavX.java @@ -0,0 +1,24 @@ +package team3176.robot.subsystems.drivetrain; + +import com.kauailabs.navx.frc.AHRS; + +import edu.wpi.first.wpilibj.SPI; + +public class GyroIONavX implements GyroIO{ + AHRS navX; + public GyroIONavX() { + navX = new AHRS(SPI.Port.kMXP); + } + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.pitch = navX.getPitch(); + inputs.roll = navX.getRoll(); + inputs.yaw = navX.getYaw(); + inputs.rotation2d = navX.getRotation2d(); + } + @Override + public void reset() { + navX.reset(); + } + +} diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java index cdd9e1e..6a5e3c4 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java @@ -1,45 +1,35 @@ package team3176.robot.subsystems.drivetrain; import java.util.Map; -import com.ctre.phoenix.motorcontrol.can.TalonFX; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.revrobotics.CANSparkMax; +import org.littletonrobotics.junction.Logger; 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; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import team3176.robot.constants.SwervePodConstants2022; - -import com.ctre.phoenix.sensors.AbsoluteSensorRange; -import com.ctre.phoenix.sensors.CANCoder; +import team3176.robot.constants.DrivetrainConstants; +import team3176.robot.util.LoggedTunableNumber; import team3176.robot.util.God.*; public class SwervePod { /** Class Object holding the Motor controller for Drive Motor on the SwervePod */ - private TalonFX thrustController; - /** Class Object holding the Motor controller for azimuth (aka azimuth) Motor on the SwervePod */ - private CANSparkMax azimuthController; - /** Class Object holding the CAN-based encoder for azimuth (aka azimuth) position of the SwervePod */ - CANCoder azimuthEncoder; /** Current value in radians of the azimuthEncoder's position */ double azimuthEncoderRelPosition; double azimuthEncoderAbsPosition; - Double desired_optimized_azimuth_position; + double desired_optimized_azimuth_position; + double velTicsPer100ms; boolean lastHasResetOccurred; - + /** Numerical identifier to differentiate between pods. * For 4 Pods: 0 = FrontRight (FR), * 1 = FrontLeft (FL), @@ -53,137 +43,53 @@ public class SwervePod { //private double kAzimuthEncoderUnitsPerRevolution; private double lastEncoderPos; - private double radianError; - private double radianPos; - private double encoderError; - private double encoderPos; - - private double azimuthCommand; - private double velTicsPer100ms; public int kSlotIdx_Azimuth, kPIDLoopIdx_Azimuth, kTimeoutMs_Azimuth,kSlotIdx_Thrust, kPIDLoopIdx_Thrust, kTimeoutMs_Thrust; public double podThrust, podAzimuth, podAbsAzimuth; - private double kP_Azimuth; - private double kI_Azimuth; + //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 double kRampRate_Azimuth = 0.0; - - private double kP_Thrust; - private double kI_Thrust; - private double kD_Thrust; - private double kF_Thrust; + 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 boolean isAutonSwerveControllerOptimizingAzimuthPos = false; - private double PI = Math.PI; - private final PIDController m_turningPIDController2; + private final PIDController turningPIDController; //private final ProfiledPIDController m_turningProfiledPIDController; //private ProfiledPIDController m_turningPIDController; + private SwervePodIO io; + private SwervePodIOInputsAutoLogged inputs = new SwervePodIOInputsAutoLogged(); - - public SwervePod(int id, TalonFX thrustController, CANSparkMax azimuthController) { + public SwervePod(int id, SwervePodIO io) { this.id = id; - azimuthEncoder = new CANCoder(SwervePodConstants2022.STEER_CANCODER_CID[id]); - azimuthEncoder.configAbsoluteSensorRange(AbsoluteSensorRange.Signed_PlusMinus180); - azimuthEncoder.configMagnetOffset(SwervePodConstants2022.AZIMUTH_ABS_ENCODER_OFFSET_POSITION[id]); - azimuthEncoder.configSensorDirection(true,100); - updateAzimuthAbsEncoder(); - initializeSmartDashboard(); + this.io = io; this.desired_optimized_azimuth_position = 0.0; - ///System.out.println("P"+(this.id+1)+" kEncoderOffset: "+this.kEncoderOffset); - - - //kAzimuthEncoderUnitsPerRevolution = SwervePodConstants2022.AZIMUTH_ENCODER_UNITS_PER_REVOLUTION; - kSlotIdx_Azimuth = SwervePodConstants2022.AZIMUTH_PID_SLOT_ID; - kPIDLoopIdx_Azimuth = SwervePodConstants2022.AZIMUTH_PID_LOOP_ID; - kTimeoutMs_Azimuth = SwervePodConstants2022.AZIMUTH_ENCODER_TIMEOUT_MS; - - kP_Thrust = 0.03; // SwervePodConstants.THRUST_PID[0][id]; - kI_Thrust = 0.0; // SwervePodConstants.THRUST_PID[1][id]; - kD_Thrust = 0.0; // SwervePodConstants.THRUST_PID[2][id]; - kF_Thrust = .045; // SwervePodConstants.THRUST_PID[3][id]; - - this.kP_Azimuth = 0.006; - this.kI_Azimuth = 0.0; + //this.kP_Azimuth = 0.006; + kP_azimuth.initDefault(.007); + this.kI_Azimuth.initDefault(0.0); this.kD_Azimuth = 0.0; - - - - - m_turningPIDController2 = new PIDController(kP_Azimuth, kI_Azimuth, kD_Azimuth); - m_turningPIDController2.setTolerance(4); - m_turningPIDController2.enableContinuousInput(-180, 180); - - m_turningPIDController2.reset(); - m_turningPIDController2.setP(this.kP_Azimuth); - m_turningPIDController2.setI(this.kI_Azimuth); - m_turningPIDController2.setD(this.kD_Azimuth); + velMax.initDefault(900); + velAcc.initDefault(900); + + turningPIDController = new PIDController(kP_azimuth.get(), kI_Azimuth.get(), kD_Azimuth);//,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); - - //this.azimuthController.setOpenLoopRampRate(this.kRampRate_Azimuth); - - /** - * Config the allowable closed-loop error, Closed-Loop output will be - * neutral within this range. See Table in Section 17.2.1 for native - * units per rotation. - */ - //azimuthController.configAllowableClosedloopError(0, SwervePodConstants.kPIDLoopIdx, SwervePodConstants.kTimeoutMs); - kSlotIdx_Thrust = SwervePodConstants2022.TALON_THRUST_PID_SLOT_ID[this.id]; - kPIDLoopIdx_Thrust = SwervePodConstants2022.TALON_THRUST_PID_LOOP_ID[this.id]; - kTimeoutMs_Thrust = SwervePodConstants2022.TALON_THRUST_PID_TIMEOUT_MS[this.id]; - - - - //m_encoder = azimuthController.getEncoder(); - - this.thrustController = thrustController; - this.azimuthController = azimuthController; - //this.AZIMUTHPIDController = azimuthController.getPIDController(); - - this.thrustController.configFactoryDefault(); - //this.azimuthController.restoreFactoryDefaults(); - this.azimuthController.setOpenLoopRampRate(0.5); - this.azimuthController.setSmartCurrentLimit(20); - this.azimuthController.setInverted(true); - //this.azimuthController.setClosedLoopRampRate(0.5); - //this.azimuthController.burnFlash(); - - this.thrustController.configClosedloopRamp(0.5); - - - this.thrustController.setInverted(false); - - //TODO: check out "Feedback Device Not Continuous" under config tab in CTRE-tuner. Is the available via API and set-able? Caps encoder to range[-4096,4096], correct? - //this.azimuthController.configSelectedFeedbackSensor(FeedbackDevice.PulseWidthEncodedPosition), 0, 0); - //this.azimuthController.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute), 0, 0); - - this.thrustController.config_kP(kPIDLoopIdx_Thrust, kP_Thrust, kTimeoutMs_Thrust); - this.thrustController.config_kI(kPIDLoopIdx_Thrust, kI_Thrust, kTimeoutMs_Thrust); - this.thrustController.config_kD(kPIDLoopIdx_Thrust, kD_Thrust, kTimeoutMs_Thrust); - this.thrustController.config_kF(kPIDLoopIdx_Thrust, kF_Thrust, kTimeoutMs_Thrust); - - - switch(id+1){ - case 1: this.idString="podFR"; - break; - case 2: this.idString="podFL"; - break; - case 3: this.idString="podBL"; - break; - case 4: this.idString="podBR"; - break; - default: this.idString="podNoExist"; - break; - } } - public void set(double speedMetersPerSecond, Rotation2d angle) { + public void set_module(double speedMetersPerSecond, Rotation2d angle) { set_module(new SwerveModuleState(speedMetersPerSecond,angle)); } @@ -191,89 +97,57 @@ public void set(double speedMetersPerSecond, Rotation2d angle) { * alternative method for setting swervepod in line with WPILIB standard library * @param desiredState */ - public void set_module(SwerveModuleState desiredState) { - updateAzimuthAbsEncoder(); - this.azimuthEncoderAbsPosition = azimuthEncoder.getAbsolutePosition(); + public SwerveModuleState set_module(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(); + if (desiredState.speedMetersPerSecond > (-Math.pow(10,-10)) && desiredState.speedMetersPerSecond < (Math.pow(10,-10))) { - this.turnOutput = m_turningPIDController2.calculate(this.azimuthEncoderAbsPosition, this.lastEncoderPos); + this.turnOutput = turningPIDController.calculate(this.azimuthEncoderAbsPosition, this.lastEncoderPos); } else { - this.turnOutput = m_turningPIDController2.calculate(this.azimuthEncoderAbsPosition, desired_optimized.angle.getDegrees()); + this.turnOutput = turningPIDController.calculate(this.azimuthEncoderAbsPosition, desired_optimized.angle.getDegrees()); this.lastEncoderPos = desired_optimized.angle.getDegrees(); } - - //azimuthController.set(); + // 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())); + //Logger.getInstance().recordOutput("Drive/Module" + Integer.toString(this.id) + "", id); - azimuthController.set(MathUtil.clamp(this.turnOutput, -0.4, 0.4)); + io.setTurn(MathUtil.clamp(this.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); - thrustController.set(TalonFXControlMode.Velocity, velTicsPer100ms); - // if(this.id == 0) { - // System.out.println(Math.round(desired_optimized.speedMetersPerSecond * 100 - // )/100.0 + " " + Math.round(desired_optimized.angle.getDegrees()) + " " +this.velTicsPer100ms); + io.setDrive(desired_optimized.speedMetersPerSecond); + + if(kP_azimuth.hasChanged(hashCode()) || kI_Azimuth.hasChanged(hashCode())) { + turningPIDController.setP(kP_azimuth.get()); + turningPIDController.setI(kI_Azimuth.get()); + } + // if(velAcc.hasChanged(hashCode()) || velMax.hasChanged(hashCode())){ + // turningPIDController.setConstraints(new Constraints(velMax.get(),velAcc.get())); // } + + return desired_optimized; } /* * odometry calls */ public SwerveModulePosition getPosition() { - double mps = Units.feetToMeters(1.0 / Units3176.conversion_feet_to_tics * thrustController.getSelectedSensorPosition()); - return new SwerveModulePosition(mps,Rotation2d.fromDegrees(azimuthEncoder.getAbsolutePosition())); - } - - - public void goHome() { - SwerveModuleState s = SwerveModuleState.optimize(new SwerveModuleState(0.0,Rotation2d.fromDegrees(0.0)), Rotation2d.fromDegrees(this.azimuthEncoderAbsPosition)); - - // SmartDashboard.putNumber("P"+(this.id)+".optmizdazimuthAbsPos", optmizdAzimuthAbsPos); - - double turnOutput = m_turningPIDController2.calculate(this.azimuthEncoderAbsPosition, s.angle.getDegrees()); - //double turnOutput = m_turningPIDController.calculate(this.azimuthEncoderPosition, this.podAzimuth); - - // SmartDashboard.putNumber("P"+(this.id)+".turnOutput", turnOutput); - azimuthController.set(turnOutput * SwervePodConstants2022.AZIMUTH_SPARKMAX_MAX_OUTPUTPERCENT); - - //this.azimuthPIDController.setReference(homePos, CANSparkMax.ControlType.kPosition); - - } - - public void setCurrentPositionAsHome() { - this.azimuthEncoder.configMagnetOffset(0); - double newAbsMagnetOffset = this.azimuthEncoder.getAbsolutePosition(); - this.azimuthEncoder.configMagnetOffset(-1 * newAbsMagnetOffset); - } - - - public boolean isInverted() { return azimuthController.getInverted(); } - public void setInverted() { azimuthController.setInverted(!isInverted()); } - - public void updateAzimuthRelEncoder() { - this.azimuthEncoderRelPosition = Math.toRadians(azimuthEncoder.getPosition()); - SmartDashboard.putNumber("P"+this.id+".azimuthEncoderPositionRad",Math.toRadians(azimuthEncoder.getPosition())); - SmartDashboard.putNumber("P"+this.id+".azimuthEncoderPositionDeg",azimuthEncoder.getPosition()); + double mps = Units.feetToMeters((DrivetrainConstants.WHEEL_DIAMETER_INCHES/12.0 * Math.PI) * inputs.drivePositionRad / (2*Math.PI)); + return new SwerveModulePosition(mps,Rotation2d.fromDegrees(inputs.turnAbsolutePositionDegrees)); } - - public void updateAzimuthAbsEncoder() { - this.azimuthEncoderAbsPosition = Math.toRadians(azimuthEncoder.getAbsolutePosition()); - SmartDashboard.putNumber("P"+this.id+".azimuthEncoderAbsPositionRad",this.azimuthEncoderAbsPosition); - SmartDashboard.putNumber("P"+this.id+".azimuthEncoderAbsPositionDeg",azimuthEncoder.getAbsolutePosition()); - // SmartDashboard.putNumber("P"+this.id+"azimuthEncoderCANOffset", SwervePodConstants2022.AZIMUTH_ABS_ENCODER_OFFSET_POSITION[id]); - //NT_encoderPos.setDouble(this.azimuthEncoderAbsPosition); + public SwerveModulePosition getDelta() { + return new SwerveModulePosition(this.delta,Rotation2d.fromDegrees(inputs.turnAbsolutePositionDegrees)); } - - public double getEncoderRelPos() { - updateAzimuthRelEncoder(); - return this.azimuthEncoderRelPosition; - } - - public double getEncoderAbsPos() { - updateAzimuthAbsEncoder(); - return this.azimuthEncoderAbsPosition; - } public double getVelocity() { - double motorShaftVelocity = thrustController.getSelectedSensorVelocity(); - double wheelVelocityInFeetPerSecond = Units3176.ums2fps(motorShaftVelocity); + double wheelVelocityInFeetPerSecond = inputs.driveVelocityRadPerSec / (Math.PI *2) * DrivetrainConstants.WHEEL_DIAMETER_INCHES/12.0 * Math.PI; double wheelVelocityInMetersPerSecond = Units3176.feetPerSecond2metersPerSecond(wheelVelocityInFeetPerSecond); return wheelVelocityInMetersPerSecond; } @@ -283,60 +157,37 @@ public double getVelocity() { * @return */ public double getAzimuth() { - return azimuthEncoder.getPosition(); - } - - public void boostThrustAcceleration() { - this.thrustController.configClosedloopRamp(0.25); - } - - public void unboostThrustAcceleration() { - this.thrustController.configClosedloopRamp(0.5); + return inputs.turnAbsolutePositionDegrees; } public void setThrustCoast() { - this.thrustController.setNeutralMode(NeutralMode.Coast); + io.setDriveBrakeMode(false); } - public void setThrustBrake() { - this.thrustController.setNeutralMode(NeutralMode.Brake); + io.setDriveBrakeMode(true); } - public void initializeSmartDashboard() { - - // SmartDashboard.putNumber("P"+(this.id)+".podAzimuth_setpoint_angle", 0); - // SmartDashboard.putNumber("P"+(this.id)+".kP_Azimuth", 0); - // SmartDashboard.putNumber("P"+(this.id)+".kI_Azimuth", 0); - // SmartDashboard.putNumber("P"+(this.id)+".kD_Azimuth", 0); - // SmartDashboard.putNumber("P"+(this.id)+".kRampRate_Azimuth", 0); - SmartDashboard.putNumber("P"+(this.id)+".azimuthEncoderAbsPosition", this.azimuthEncoderAbsPosition); - // SmartDashboard.putBoolean("P"+(this.id)+".On", false); + public double getAzimuthSetpoint() { + return this.desired_optimized_azimuth_position; } - public double getThrustSetpoint() { return this.velTicsPer100ms; } - public double getAzimuthSetpoint() { - return this.desired_optimized_azimuth_position; - } public double getThrustEncoderVelocity() { - return thrustController.getSelectedSensorVelocity(); - } - public double getAzimuthEncoderPosition() { - return azimuthEncoder.getAbsolutePosition(); + return inputs.driveVelocityRadPerSec; } public void setupShuffleboard() { Shuffleboard.getTab(this.idString) - .add(idString+"/podAzimuth_setpoint_angle",SwervePodConstants2022.AZIMUTH_ABS_ENCODER_OFFSET_POSITION[id]) + .add(idString+"/podAzimuth_setpoint_angle",DrivetrainConstants.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) + .add(idString+"/kP_Azimuth", this.kP_azimuth.get()) .withSize(1,1) .withPosition(4,1) .getEntry(); @@ -350,96 +201,5 @@ public void setupShuffleboard() { .withSize(1,1) .withPosition(6,1) .getEntry(); - - - - - - //public void calculate() { - // double distance = ...; - // distanceEntry.setDouble(distance); - //} } - public void SwervePod2022SmartDashboardComments () { - //SwervePod2022 comments start - // SmartDashboard.putNumber("P", kP_Thrust); - // SmartDashboard.putNumber("I", kI_Thrust); - // SmartDashboard.putNumber("D", kD_Thrust); - // SmartDashboard.putNumber("F", kF_Thrust); - // SmartDashboard.putNumber("driveSet",0); - - // SmartDashboard.putNumber("P" + (id + 1) + " podDrive", this.podDrive); - - // SmartDashboard.putNumber("P" + (id + 1) + " podAzimuth", this.podAzimuth); - //SwervePod2022 comments end - - - //set(double podThrust, double podAzimuth) comments start - // SmartDashboard.putNumber("P" + (id + 1) + " podThrust", this.podThrust); - // SmartDashboard.putNumber("P" + (id + 1) + " podAzimuth", this.podAzimuth); - - // SmartDashboard.putNumber("fps2ums:velTicsPer100ms", velTicsPer100ms); - // SmartDashboard.putNumber("podThrust", this.podThrust); - - // SmartDashboard.putNumber("P" + (id + 1) + " tics", tics); - // SmartDashboard.putNumber("P" + (id + 1) + " absTics", azimuthController.getSelectedSensorPosition()); - - // SmartDashboard.putNumber("P" + (id + 1) + " lastEncoderPos", this.lastEncoderPos); - - // SmartDashboard.putNumber("P" + (id + 1) + " lastEncoderPos", this.lastEncoderPos); - - //SmartDashboard.putNumber("P" + (id) + "getSelSenPos", azimuthController.getSelectedSensorPosition()); - - // SmartDashboard.putNumber("P"+this.id+".podThrust", podThrust); - //SmartDashboard.putNumber("actualVel", thrustController.getVoltage()); - - // if (this.id == 0){ - // SmartDashboard.putNumber("Pod3Distance", thrustController.getSelectedSensorPosition()); - // SmartDashboard.putNumber("velTicsPer100ms", velTicsPer100ms); - // SmartDashboard.putNumber("Tics Error", thrustController.getSelectedSensorVelocity()-velTicsPer100ms); - // SmartDashboard.putNumber("Tics Error2", thrustController.getSelectedSensorVelocity()-velTicsPer100ms); - // SmartDashboard.putNumber("thrustController Velocity", thrustController.getSelectedSensorVelocity()); - // } - - // SmartDashboard.putNumber("P" + (id + 1) + " velTicsPer100ms", velTicsPer100ms); - // SmartDashboard.putNumber("P" + (id + 1) + " encoderSetPos_end", encoderSetPos); - //} - //set(double podthrust, double podAzimuth) comments start - - - //private double optimizeAzimuthPos(double angle) comments start - // SmartDashboard.putNumber("P" + (id + 1) + " calcAzimuthPos_angle", angle); - - // SmartDashboard.putNumber("P" + (id + 1) + " kEncoderOffset", this.kEncoderOffset); - // SmartDashboard.putNumber("P" + (id + 1) + " getSelectedSensorPosition", azimuthController.getSelectedSensorPosition()); - // SmartDashboard.putNumber("P" + (id + 1) + " encoderPos_in_calcAzimuthPos",this.encoderPos); - - // SmartDashboard.putNumber("P" + (id + 1) + " radianPos", radianPos); - - // SmartDashboard.putNumber("P" + (id + 1) + " radianError", radianError); - - // SmartDashboard.putNumber("P" + (id + 1) + " encoderError", encoderError); - - // SmartDashboard.putNumber("P" + (id + 1) + "tics2radianThrustcommand", thrustCommand); - //private double optimizeAzimuthPos(double angle) comments end - - - //public void setDesiredState(SwerveModuleState desiredState) comments start - // SmartDashboard.putNumber("P"+this.id+".setDesiredState_desiredDegrees", desiredState.angle.getDegrees()); - // SmartDashboard.putNumber("P"+this.id+".setdesiredState_sensoredDegrees", rotation.getDegrees()); - - // SmartDashboard.putNumber("P"+this.id+".setDesiredState_TurnOutput",turnOutput); - // SmartDashboard.putNumber("P"+this.id+".setDesiredState_ThrustOutput", thrustOutput); - //public void setDesiredState(SwerveModuleState desiredState) comments ends - - - //public double getVelocity_metersPerSec() comments start - //SmartDashboard.putNumber("GetSensorVelocity", speed); - // SmartDashboard.putNumber("Velocity", metersPerSecond); - //public double getVelocity_metersPerSec() comments end - - } - - // public double getRate(){ - // } } diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIO.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIO.java new file mode 100644 index 0000000..b85ad4e --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIO.java @@ -0,0 +1,36 @@ +package team3176.robot.subsystems.drivetrain; + +import org.littletonrobotics.junction.AutoLog; + +public interface SwervePodIO { + @AutoLog + public static class SwervePodIOInputs { + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double[] driveCurrentAmpsStator = new double[] {}; + public double[] driveCurrentAmpsSupply = new double[] {}; + public double[] driveTempCelcius = new double[] {}; + + public double turnAbsolutePositionDegrees = 0.0; + public double turnVelocityRPM = 0.0; + public double turnAppliedVolts = 0.0; + public double[] turnCurrentAmps = new double[] {}; + public double[] turnTempCelcius = new double[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(SwervePodIOInputs inputs) {} + + /** Run the drive motor at the specified voltage. */ + public default void setDrive(double volts) {} + + /** Run the turn motor at the specified voltage. */ + public default void setTurn(double volts) {} + + /** Enable or disable brake mode on the drive motor. */ + public default void setDriveBrakeMode(boolean enable) {} + + /** Enable or disable brake mode on the turn motor. */ + public default void setTurnBrakeMode(boolean enable) {} +} diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOFalconSpark.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOFalconSpark.java new file mode 100644 index 0000000..a641159 --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOFalconSpark.java @@ -0,0 +1,76 @@ +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 team3176.robot.constants.DrivetrainConstants; +import team3176.robot.constants.SwervePodHardwareID; +import team3176.robot.util.God.Units3176; + +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 SwervePodIOFalconSpark(SwervePodHardwareID id,int sparkMaxID) { + turnSparkMax = new CANSparkMax(sparkMaxID, MotorType.kBrushless); + thrustFalcon = new TalonFX(id.THRUST_CID); + //reset the motor controllers + thrustFalcon.configFactoryDefault(); + turnSparkMax.restoreFactoryDefaults(); + + thrustFalcon.configClosedloopRamp(0.5); + thrustFalcon.setInverted(false); + thrustFalcon.config_kP(0, 0.03); + thrustFalcon.config_kI(0, 0.0); + thrustFalcon.config_kD(0, 0.0); + thrustFalcon.config_kF(0, 0.045); + + turnSparkMax.setOpenLoopRampRate(0.5); + turnSparkMax.setSmartCurrentLimit(20); + turnSparkMax.setInverted(true); + + azimuthEncoder = new CANCoder(id.CANCODER_CID); + azimuthEncoder.configAbsoluteSensorRange(AbsoluteSensorRange.Signed_PlusMinus180); + + azimuthEncoder.configMagnetOffset(id.OFFSET); + azimuthEncoder.configSensorDirection(true,100); + + } + 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; + inputs.driveAppliedVolts = thrustFalcon.getMotorOutputVoltage(); + inputs.driveCurrentAmpsStator = new double[] {thrustFalcon.getStatorCurrent()}; + inputs.driveCurrentAmpsSupply = new double[] {thrustFalcon.getSupplyCurrent()}; + inputs.driveTempCelcius = new double[] {thrustFalcon.getTemperature()}; + + inputs.turnAbsolutePositionDegrees = azimuthEncoder.getAbsolutePosition(); + + inputs.turnVelocityRPM = turnSparkMax.getEncoder().getVelocity(); + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + inputs.turnTempCelcius = new double[] {turnSparkMax.getMotorTemperature()}; + } + + @Override + public void setDrive(double velMetersPerSecond) { + double velTicsPer100ms = Units3176.mps2ums(velMetersPerSecond); + thrustFalcon.set(TalonFXControlMode.Velocity, velTicsPer100ms); + } + + /** Run the turn motor at the specified voltage. */ + public void setTurn(double volts) { + turnSparkMax.set(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/drivetrain/SwervePodIOSim.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java new file mode 100644 index 0000000..916018d --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java @@ -0,0 +1,95 @@ +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; +import edu.wpi.first.math.util.Units; +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 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() { + + + } + public void updateInputs(SwervePodIOInputs inputs) { + driveSim.update(Constants.loopPeriodSecs); + turnSim.update(Constants.loopPeriodSecs); + double angleDiffRad = Units.radiansToDegrees(turnSim.getAngularVelocityRadPerSec() * Constants.loopPeriodSecs); + turnRelativePositionRad += angleDiffRad; + turnAbsolutePositionRad += angleDiffRad; + while (turnAbsolutePositionRad < -180) { + turnAbsolutePositionRad += 360; + } + while (turnAbsolutePositionRad > 180) { + turnAbsolutePositionRad -= 360; + } + + inputs.drivePositionRad = + inputs.drivePositionRad + + (driveSim.getAngularVelocityRadPerSec() * Constants.loopPeriodSecs); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmpsStator = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; + inputs.driveTempCelcius = new double[] {}; + currentDriveSpeed = driveSim.getAngularVelocityRadPerSec(); + + inputs.turnAbsolutePositionDegrees = turnAbsolutePositionRad; + inputs.turnVelocityRPM = turnSim.getAngularVelocityRPM(); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; + inputs.turnTempCelcius = new double[] {}; + + } + + @Override + public void setDrive(double velMetersPerSecond) { + double freeSpeedRadPerSec = 142.0; + double driveSpeedError = velMetersPerSecond - (this.currentDriveSpeed / (2) * Units.inchesToMeters(DrivetrainConstants.WHEEL_DIAMETER_INCHES)); + double voltage = 12 * velMetersPerSecond * 1.0/(freeSpeedRadPerSec / (2*Math.PI) * Units.inchesToMeters(DrivetrainConstants.WHEEL_DIAMETER_INCHES) * Math.PI); + driveAppliedVolts = MathUtil.clamp(voltage + driveSpeedError*4,-12.0,12.0); + if(DriverStation.isEnabled()) { + driveSim.setInputVoltage(driveAppliedVolts); + }else { + driveSim.setInputVoltage(0.0); + } + + } + + /** Run the turn motor at the specified voltage. */ + public void setTurn(double volts) { + if(DriverStation.isEnabled()){ + turnAppliedVolts = MathUtil.clamp(volts * 12, -12.0, 12.0); + turnSim.setInputVoltage(turnAppliedVolts); + }else { + turnSim.setInput(0.0); + } + + } + + /** 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 4fad492..0fd2200 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Arm.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Arm.java @@ -14,17 +14,23 @@ 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; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; 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 team3176.robot.subsystems.superstructure.ArmIO.ArmIOInputs; import org.littletonrobotics.junction.Logger; public class Arm extends SubsystemBase { @@ -32,9 +38,8 @@ public class Arm extends SubsystemBase { private static final double MIN_ENCODER_ANGLE_VALUE = SuperStructureConstants.ARM_ZERO_POS; private static Arm instance; private final ArmIO io; - private final ArmIOInputs inputs = new ArmIOInputs(); - private CANSparkMax armController; - private CANCoder armEncoder; + private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); + private double armEncoderAbsPosition; private double lastEncoderPos; private final PIDController m_turningPIDController; @@ -42,12 +47,13 @@ public class Arm extends SubsystemBase { 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); + private MechanismRoot2d root = mech.getRoot("armRoot", 5, 5); + private MechanismLigament2d armSholder = root.append(new MechanismLigament2d("armLigament",2,90)); + private MechanismLigament2d simSholder = root.append(new MechanismLigament2d("simLigament",3,90,10,new Color8Bit(Color.kAqua))); + private MechanismLigament2d armElbow = armSholder.append(new MechanismLigament2d("armELigament",2,90)); private Arm(ArmIO io) { this.io = io; - armController = new CANSparkMax(Hardwaremap.arm_CID, MotorType.kBrushless); - armController.setSmartCurrentLimit(SuperStructureConstants.ARM_CURRENT_LIMIT_A); - armEncoder = new CANCoder(Hardwaremap.armEncoder_CID); this.m_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); @@ -55,17 +61,14 @@ private Arm(ArmIO io) { } public void setCoastMode() { - armController.setIdleMode(IdleMode.kCoast); + io.setCoastMode(true); } public void setBrakeMode() { - armController.setIdleMode(IdleMode.kBrake); + io.setCoastMode(false); } private void setArmPidPosMode() { - this.armEncoder.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360); - this.armEncoder.configMagnetOffset(SuperStructureConstants.ARM_ENCODER_OFFSET); - this.armEncoder.configSensorDirection(true,100); this.m_turningPIDController.setTolerance(SuperStructureConstants.ARM_TOLERANCE); //this.m_turningPIDController.enableContinuousInput() @@ -74,13 +77,17 @@ private void setArmPidPosMode() { this.m_turningPIDController.setI(SuperStructureConstants.ARM_kI); this.m_turningPIDController.setD(SuperStructureConstants.ARM_kD); //this.m_turningPIDController.enableContinuousInput(0, 360); - this.armController.setOpenLoopRampRate(0.5); - - this.armController.burnFlash(); } public static Arm getInstance() { - if (instance == null){instance = new Arm(new ArmIO() {});} + if (instance == null){ + if(Constants.getMode() != Mode.SIM) { + instance = new Arm(new ArmIOSpark() {}); + } else { + instance = new Arm(new ArmIOSim() {}); + } + + } return instance; } @@ -91,7 +98,7 @@ public static Arm getInstance() { private void setPIDPosition(double desiredAngle) { //need to double check these values - this.armEncoderAbsPosition = armEncoder.getAbsolutePosition(); + this.armEncoderAbsPosition = inputs.Position; 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 @@ -100,7 +107,7 @@ private void setPIDPosition(double desiredAngle) { // for example .4 output is generated by a 40 degree error double kp = SmartDashboard.getNumber("Arm_kp", SuperStructureConstants.ARM_kP); m_turningPIDController.setP(kp); - double feedForward = kg * physicsAngle/SuperStructureConstants.ARM_HIGH_POS; + double feedForward = 0.0;//kg * physicsAngle/SuperStructureConstants.ARM_HIGH_POS; if (this.armEncoderAbsPosition < SuperStructureConstants.ARM_MID_POS + 10){ feedForward =0.0; } else if (desiredAngle < SuperStructureConstants.ARM_ZERO_POS+5) { @@ -108,37 +115,34 @@ private void setPIDPosition(double desiredAngle) { } double turnOutput = m_turningPIDController.calculate(this.armEncoderAbsPosition, desiredAngle); turnOutput = MathUtil.clamp(turnOutput,-1,1); - armController.set(turnOutput + feedForward); + io.set(turnOutput + feedForward); SmartDashboard.putNumber("Arm_Output", turnOutput + feedForward); SmartDashboard.putNumber("Arm Feed Forward", feedForward); } public void armAnalogUp() { this.currentState = States.OPEN_LOOP; - armController.set(SuperStructureConstants.ARM_OUTPUT_POWER); + io.set(SuperStructureConstants.ARM_OUTPUT_POWER); } public void armAnalogDown() { this.currentState = States.OPEN_LOOP; - armController.set(-SuperStructureConstants.ARM_OUTPUT_POWER); + io.set(-SuperStructureConstants.ARM_OUTPUT_POWER); //System.out.println("Arm Analog Down"); //System.out.println("Arm_Abs_Position: " + armEncoder.getAbsolutePosition()); //System.out.println("Arm_Rel_Position: " + armEncoder.getPosition()); } public void idle() { - armController.set(0.0); + io.set(0.0); } public void fineTune(double delta) { this.currentState = States.CLOSED_LOOP; this.armSetpointAngleRaw -= delta * 0.5; - } - private void nothing() { - } public double getArmPosition() { - return armEncoder.getAbsolutePosition(); + return inputs.Position; } public boolean isArmAtPosition() { - return Math.abs(this.armEncoder.getAbsolutePosition() - this.armSetpointAngleRaw) < 7; + return Math.abs(this.m_turningPIDController.getPositionError()) < 7; } /** * to be used for trajectory following without disrupting other commands @@ -159,8 +163,8 @@ public Command armSetPositionBlocking(double angleInDegrees) { return new FunctionalCommand(() -> { this.currentState = States.CLOSED_LOOP; this.armSetpointAngleRaw = angleInDegrees;}, - ()-> this.nothing(), - (b) -> this.nothing(), + ()-> {}, + (b) -> {}, this::isArmAtPosition, this); } @@ -184,47 +188,16 @@ public Command armAnalogDownCommand() { public void periodic() { io.updateInputs(inputs); Logger.getInstance().processInputs("Arm", inputs); - Logger.getInstance().recordOutput("Arm/Velocity", getVelocity()); - Logger.getInstance().recordOutput("Arm/Position", getPosition()); - - SmartDashboard.putNumber("Arm_Position", armEncoder.getAbsolutePosition()); - SmartDashboard.putNumber("Arm_Position_Relative", armEncoder.getAbsolutePosition() - SuperStructureConstants.ARM_ZERO_POS); - - // if (counter == 50) { - // System.out.println("Arm_Position: " + armEncoder.getAbsolutePosition()); - // counter = 0; - // } else { - // counter = counter++; - // } + armSholder.setAngle(Rotation2d.fromDegrees(inputs.Position-SuperStructureConstants.ARM_ZERO_POS-190)); + armElbow.setAngle(Rotation2d.fromDegrees(20 + 100 * (SuperStructureConstants.ARM_HIGH_POS - inputs.Position)/(SuperStructureConstants.ARM_HIGH_POS-SuperStructureConstants.ARM_ZERO_POS))); + simSholder.setAngle(Rotation2d.fromDegrees(inputs.Position -90 - SuperStructureConstants.ARM_SIM_OFFSET)); + Logger.getInstance().recordOutput("Arm/mech2d", mech); + //SmartDashboard.putNumber("Arm_Position", armEncoder.getAbsolutePosition()); + //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); - SmartDashboard.putNumber("Arm_Error", armEncoder.getAbsolutePosition()-this.armSetpointAngleRaw); + Logger.getInstance().recordOutput("Arm/position_error", this.m_turningPIDController.getPositionError()); setPIDPosition(armSetpointAngleRaw); } } - - public double getVelocity() - { - return inputs.velocity; - } - - public double getPosition() - { - return inputs.position; - } - - public void runVoltage(double volts) - { - io.setVoltage(volts); - } - - public void setVelocity(double velocity) - { - io.setVelocity(velocity); - } - - public void setPosition(double position) - { - io.setPosition(position); - } } diff --git a/src/main/java/team3176/robot/subsystems/superstructure/ArmIO.java b/src/main/java/team3176/robot/subsystems/superstructure/ArmIO.java index 1f18984..e1b4d61 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/ArmIO.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/ArmIO.java @@ -7,53 +7,34 @@ 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{ /** Contains all of the input data received from hardware. */ - public static class ArmIOInputs implements LoggableInputs { - public double velocity = 0.0; - public double position = 0.0; - public double appliedVolts = 0.0; - public double[] currentAmps = new double[] {}; - public double[] tempCelcius = new double[] {}; - - - - public void toLog(LogTable table) { - table.put("VelocityOutputPercent", velocity); - table.put("Position", position); - table.put("AppliedVolts", appliedVolts); - table.put("CurrentAmps", currentAmps); - table.put("TempCelcius", tempCelcius); - } - - public void fromLog(LogTable table) { - velocity = table.getDouble("VelocityOutputPercent", velocity); - position = table.getDouble("Position", position); - appliedVolts = table.getDouble("AppliedVolts", appliedVolts); - currentAmps = table.getDoubleArray("CurrentAmps", currentAmps); - tempCelcius = table.getDoubleArray("TempCelcius", tempCelcius); - } + @AutoLog + public static class ArmIOInputs { + public double Position = 0.0; + public double VelocityRadPerSec = 0.0; + public double AppliedVolts = 0.0; + public double[] CurrentAmps = new double[] {}; + public double[] TempCelcius = new double[] {}; + + //constructor if needed for some inputs + ArmIOInputs() { + } } - - - + /** Updates the set of loggable inputs. */ public default void updateInputs(ArmIOInputs inputs) {} - - /** Run open loop at the specified voltage. */ - public default void setVoltage(double volts) {} - - /** - * Run closed loop at the specified velocity. - * - * @param velocityRadPerSec Velocity setpoint. - */ - public default void setVelocity(double velocityRadPerSec) {} - - public default void setPosition(double position) {} + public default void set(double percentOutput){} + public default void setCoastMode(boolean isCoastMode) {} + public default void reset() {} } diff --git a/src/main/java/team3176/robot/subsystems/superstructure/ArmIOSim.java b/src/main/java/team3176/robot/subsystems/superstructure/ArmIOSim.java new file mode 100644 index 0000000..59e6d7e --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/superstructure/ArmIOSim.java @@ -0,0 +1,63 @@ +// 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. +// 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.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; + +/** Template hardware interface for a closed loop subsystem. */ +public class ArmIOSim implements ArmIO{ + + private SingleJointedArmSim armSim; + private double appliedVolts; + 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. */ + public void updateInputs(ArmIOInputs inputs) { + armSim.update(Constants.loopPeriodSecs); + inputs.Position = Units.radiansToDegrees(armSim.getAngleRads()) + 90 + SuperStructureConstants.ARM_SIM_OFFSET; + inputs.VelocityRadPerSec = armSim.getVelocityRadPerSec(); + inputs.AppliedVolts = appliedVolts; + inputs.CurrentAmps = new double[] {armSim.getCurrentDrawAmps()}; + inputs.TempCelcius = new double[] {0.0}; + Logger.getInstance().recordOutput("Arm/SimPos",armSim.getAngleRads()); + } + public void set(double percentOuput) { + if(DriverStation.isEnabled()) { + appliedVolts = percentOuput * 12; + } else { + appliedVolts = 0.0; + } + 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 new file mode 100644 index 0000000..1b6b8bf --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/superstructure/ArmIOSpark.java @@ -0,0 +1,58 @@ +// 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. +// 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.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; +/** Template hardware interface for a closed loop subsystem. */ +public class ArmIOSpark implements ArmIO{ + + private CANSparkMax armController; + private CANCoder armEncoder; + public ArmIOSpark() { + armController = new CANSparkMax(Hardwaremap.arm_CID, MotorType.kBrushless); + armController.setSmartCurrentLimit(SuperStructureConstants.ARM_CURRENT_LIMIT_A); + armEncoder = new CANCoder(Hardwaremap.armEncoder_CID); + armController.setOpenLoopRampRate(0.5); + armEncoder.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360); + armEncoder.configMagnetOffset(SuperStructureConstants.ARM_ENCODER_OFFSET); + armEncoder.configSensorDirection(true,100); + } + /** Updates the set of loggable inputs. */ + public void updateInputs(ArmIOInputs inputs) { + inputs.Position = armEncoder.getAbsolutePosition(); + inputs.VelocityRadPerSec = Units.degreesToRadians(armEncoder.getVelocity()); + inputs.AppliedVolts = armController.getAppliedOutput() * armController.getBusVoltage(); + inputs.CurrentAmps = new double[] {armController.getOutputCurrent()}; + inputs.TempCelcius = new double[] {armController.getMotorTemperature()}; + } + public void set(double percentOuput) { + armController.set(percentOuput); + } + public void setCoastMode(boolean isCoastMode) { + if(isCoastMode) { + armController.setIdleMode(IdleMode.kCoast); + } else { + armController.setIdleMode(IdleMode.kBrake); + } + } + public void reset() {} +} + diff --git a/src/main/java/team3176/robot/subsystems/vision/VisionDual.java b/src/main/java/team3176/robot/subsystems/vision/VisionDual.java new file mode 100644 index 0000000..711fcbe --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/vision/VisionDual.java @@ -0,0 +1,94 @@ +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.VisionDualIO.VisionDualInputs; + +public class VisionDual extends SubsystemBase{ + private VisionDualIO io; + private VisionDualInputs inputs; + private Pose3d bestVisionPose3d; + private Pose2d bestVisionPose2d; + private enum blendModes {RIGHT,LEFT,BOTH}; + private blendModes mode; + private static VisionDual instance; + private VisionDual(VisionDualIO io) { + this.io = io; + this.inputs = new VisionDualInputs(); + mode = blendModes.LEFT; + } + public static VisionDual getInstance() { + if (instance == null) { + instance = new VisionDual(new VisionDualIOLime()); + } + return instance; + } + public Pose3d getPose3d() { + return bestVisionPose3d; + } + public Pose2d getPose2d() { + return bestVisionPose2d; + } + public boolean isValid() { + return inputs.lValid || inputs.rValid; + } + + public void periodic() { + Logger.getInstance().processInputs("Vision", inputs); + boolean isRed = DriverStation.getAlliance() == Alliance.Red; + Pose3d rPose = isRed ? inputs.rfovRed : inputs.rfovBlue; + Pose3d lPose = isRed ? inputs.lfovRed : inputs.lfovBlue; + if(inputs.lValid && inputs.rValid) { + //possible both good + if(inputs.lNumTags > inputs.rNumTags) { + mode = blendModes.LEFT; + } + else if (inputs.rNumTags > inputs.lNumTags) { + mode = blendModes.RIGHT; + } + else { + mode = blendModes.BOTH; + } + } + else if (inputs.lValid) { + mode = blendModes.LEFT; + } + else { + mode = blendModes.RIGHT; + } + + switch(mode){ + case BOTH: + bestVisionPose3d = lPose.interpolate(rPose, 0.5); + break; + case LEFT: + bestVisionPose3d = lPose; + break; + case RIGHT: + bestVisionPose3d = rPose; + break; + } + bestVisionPose2d = bestVisionPose3d.toPose2d(); + Logger.getInstance().recordOutput("Vision/bestPose",bestVisionPose3d); + + + // wildstang 111 code for refence + // if (rtv > 0.0 && ltv > 0.0){ + // if (lnumtargets > rnumtargets){ + // return (getLeftVertical()+offset*LC.OFFSET_VERTICAL) * LC.VERT_AUTOAIM_P; + // } else if (rnumtargets > lnumtargets){ + // return (getRightVertical()+offset*LC.OFFSET_VERTICAL) * LC.VERT_AUTOAIM_P; + // } + // return LC.VERT_AUTOAIM_P * (offset*LC.OFFSET_VERTICAL + (getLeftVertical() + getRightVertical())/2.0); + // } else if (ltv > 0.0){ + // return (getLeftVertical()+offset*LC.OFFSET_VERTICAL) * LC.VERT_AUTOAIM_P; + // } else { + // return (getRightVertical()+offset*LC.OFFSET_VERTICAL) * LC.VERT_AUTOAIM_P; + // } + } +} diff --git a/src/main/java/team3176/robot/subsystems/vision/VisionDualIO.java b/src/main/java/team3176/robot/subsystems/vision/VisionDualIO.java new file mode 100644 index 0000000..4f03e82 --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/vision/VisionDualIO.java @@ -0,0 +1,66 @@ +package team3176.robot.subsystems.vision; + +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; + +public interface VisionDualIO { + + public static class VisionDualInputs implements LoggableInputs { + public Pose3d rfovBlue = new Pose3d(); + public Pose3d rfovRed = new Pose3d(); + public Pose3d lfovBlue = new Pose3d(); + public Pose3d lfovRed = new Pose3d(); + public double rLatency = 0.0; + public double lLatency = 0.0; + public double rNumTags = 0; + public double lNumTags = 0; + public boolean rValid = false; + public boolean lValid = false; + private double[] blankPoseArray = {0.0,0.0,0.0,0.0,0.0,0.0}; + //constructor if needed for some inputs + VisionDualInputs() { + } + private double[] toLogPose3(Pose3d p) { + double[] poseArray = {p.getX(),p.getY(),p.getZ(),p.getRotation().getX(),p.getRotation().getY(),p.getRotation().getZ()}; + return poseArray; + } + private Pose3d fromLogPose3(double [] a) { + Rotation3d r = new Rotation3d(a[3], a[4], a[5]); + Pose3d pose = new Pose3d(a[0], a[1], a[2], r); + return pose; + } + @Override + public void toLog(LogTable table) { + table.put("rfovBlue", toLogPose3(rfovBlue)); + table.put("lfovBlue", toLogPose3(lfovBlue)); + table.put("rfovRed", toLogPose3(rfovRed)); + table.put("rfovRed", toLogPose3(lfovRed)); + table.put("RLatency", rLatency); + table.put("LLatency", lLatency); + table.put("rNumTags",rNumTags); + table.put("lNumTags",lNumTags); + table.put("RValid", rValid); + table.put("LValid", lValid); + + } + @Override + public void fromLog(LogTable table) { + rfovBlue=fromLogPose3(table.getDoubleArray("rfovBlue",blankPoseArray)); + lfovBlue=fromLogPose3(table.getDoubleArray("lfovBlue",blankPoseArray)); + rfovRed=fromLogPose3(table.getDoubleArray("rfovRed",blankPoseArray)); + lfovRed=fromLogPose3(table.getDoubleArray("lfovRed",blankPoseArray)); + rLatency = table.getDouble("RLatency", rLatency); + lLatency = table.getDouble("LLatency", lLatency); + rValid = table.getBoolean("RValid", rValid); + lValid = table.getBoolean("LValid", lValid); + rNumTags = table.getDouble("rNumTags", rNumTags); + lNumTags = table.getDouble("lNumTags", lNumTags); + } + } + /** Updates the set of loggable inputs. */ + public default void updateInputs(VisionDualInputs inputs) {} +} diff --git a/src/main/java/team3176/robot/subsystems/vision/VisionDualIOLime.java b/src/main/java/team3176/robot/subsystems/vision/VisionDualIOLime.java new file mode 100644 index 0000000..89b392e --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/vision/VisionDualIOLime.java @@ -0,0 +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"; + /** Updates the set of loggable inputs. */ + 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); + } +} diff --git a/src/main/java/team3176/robot/util/ExampleIO.java b/src/main/java/team3176/robot/util/ExampleIO.java new file mode 100644 index 0000000..8184ea3 --- /dev/null +++ b/src/main/java/team3176/robot/util/ExampleIO.java @@ -0,0 +1,26 @@ +package team3176.robot.util; + +import org.littletonrobotics.junction.AutoLog; + +public interface ExampleIO { + + /** Contains all of the input data received from hardware. */ + @AutoLog + public static class ExampleIOInputs { + public double PositionRad = 0.0; + public double VelocityRadPerSec = 0.0; + public double AppliedVolts = 0.0; + public double[] CurrentAmps = new double[] {}; + public double[] TempCelcius = new double[] {}; + + //constructor if needed for some inputs + ExampleIOInputs() { + } + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ExampleIOInputs inputs) {} + + public default void reset() {} + +} diff --git a/src/main/java/team3176/robot/util/God/Units3176.java b/src/main/java/team3176/robot/util/God/Units3176.java index 3299e6d..d63b42e 100644 --- a/src/main/java/team3176/robot/util/God/Units3176.java +++ b/src/main/java/team3176/robot/util/God/Units3176.java @@ -3,7 +3,7 @@ import edu.wpi.first.wpilibj.Timer; import team3176.robot.*; import team3176.robot.constants.DrivetrainConstants; -import team3176.robot.constants.SwervePodConstants2022; +import team3176.robot.constants.DrivetrainConstants; public class Units3176{ @@ -32,8 +32,8 @@ public double tics2Rads(double tics, double EncoderUnitsPerRevolution) { tics -= (EncoderUnitsPerRevolution / 2); return ((tics / EncoderUnitsPerRevolution) * (2 * Math.PI)); } - public static double conversion_feet_to_tics_per_100ms = 12.0 * (1.0/ (DrivetrainConstants.WHEEL_DIAMETER_INCHES * Math.PI)) * (1.0 /SwervePodConstants2022.THRUST_GEAR_RATIO) * SwervePodConstants2022.THRUST_ENCODER_UNITS_PER_REVOLUTION * .1; - public static double conversion_feet_to_tics = 12.0 * (1.0/ (DrivetrainConstants.WHEEL_DIAMETER_INCHES * Math.PI)) * (1.0 /SwervePodConstants2022.THRUST_GEAR_RATIO) * SwervePodConstants2022.THRUST_ENCODER_UNITS_PER_REVOLUTION; + public static double conversion_feet_to_tics_per_100ms = 12.0 * (1.0/ (DrivetrainConstants.WHEEL_DIAMETER_INCHES * Math.PI)) * (1.0 /DrivetrainConstants.THRUST_GEAR_RATIO) * DrivetrainConstants.THRUST_ENCODER_UNITS_PER_REVOLUTION * .1; + 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; /** * Given feet per second, returns Falcon500 units (ie tics per 100ms) * @param i feet per second diff --git a/src/main/java/team3176/robot/util/LoggedTunableNumber.java b/src/main/java/team3176/robot/util/LoggedTunableNumber.java new file mode 100644 index 0000000..9c4f716 --- /dev/null +++ b/src/main/java/team3176/robot/util/LoggedTunableNumber.java @@ -0,0 +1,93 @@ +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package team3176.robot.util; + +import java.util.HashMap; +import java.util.Map; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +/** + * 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 final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedDashboardNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + // if (Constants.tuningMode) { + dashboardNumber = new LoggedDashboardNumber(key, defaultValue); + // } + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() { + if (!hasDefault) { + return 0.0; + } else { + return true ? dashboardNumber.get() : defaultValue; + } + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) { + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; + } + + return false; + } +} \ No newline at end of file