From 2a965dedae7aaeaca7d246a9e724026f04c66d02 Mon Sep 17 00:00:00 2001 From: Abhi Sachi Date: Sat, 4 Feb 2023 13:48:05 -0700 Subject: [PATCH 01/22] added april tags --- simgui-ds.json | 92 +++++++++++++++++++ simgui.json | 55 +++++++++++ .../java/frc4388/robot/subsystems/Vision.java | 39 ++++++++ 3 files changed, 186 insertions(+) create mode 100644 simgui-ds.json create mode 100644 simgui.json create mode 100644 src/main/java/frc4388/robot/subsystems/Vision.java diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..c0596a7 --- /dev/null +++ b/simgui.json @@ -0,0 +1,55 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo" + } + }, + "NetworkTables": { + "transitory": { + "LiveWindow": { + ".status": { + "open": true + } + }, + "Shuffleboard": { + ".recording": { + "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "Clients": { + "open": true + }, + "NT3@192.168.1.132:40708": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + }, + "open": true + }, + "NT3@192.168.1.132:49546": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + }, + "open": true + }, + "NT3@192.168.1.132:52724": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + }, + "open": true + }, + "visible": true + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java new file mode 100644 index 0000000..b5fe03d --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -0,0 +1,39 @@ +package frc4388.robot.subsystems; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class Vision { + private final NetworkTableEntry m_isTags; + private final NetworkTableEntry m_xPoses; + private final NetworkTableEntry m_yPoses; + private final NetworkTableEntry m_zPoses; + + public Vision() { + final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); + + m_isTags = tagTable.getEntry("IsTag"); + m_xPoses = tagTable.getEntry("TagPosX"); + m_yPoses = tagTable.getEntry("TagPosY"); + m_zPoses = tagTable.getEntry("TagPosZ"); + } + + public AprilTag[] getAprilTags() { + if (!m_isTags.getBoolean(false)) return new AprilTag[0]; + + double xarr[] = m_xPoses.getDoubleArray(new double[] {}); + double yarr[] = m_yPoses.getDoubleArray(new double[] {}); + double zarr[] = m_zPoses.getDoubleArray(new double[] {}); + + AprilTag tags[] = new AprilTag[xarr.length]; + for (int i = 0; i < tags.length; i++) { + tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d())); + } + + return tags; + } +} From ab5cb82bc50bb6489263ea8fb28c53885e1a9bea Mon Sep 17 00:00:00 2001 From: Abhi Sachi Date: Sat, 4 Feb 2023 15:38:37 -0700 Subject: [PATCH 02/22] Get NetworkTables Working --- src/main/java/frc4388/robot/Robot.java | 9 ++++++++- src/main/java/frc4388/robot/subsystems/Vision.java | 2 +- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 8fc1ca3..16c67de 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,6 +7,11 @@ package frc4388.robot; + +import frc4388.robot.subsystems.Vision; + +import java.util.Arrays; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -24,7 +29,7 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - + private Vision Vision = new Vision(); /** * This function is run when the robot is first started up and should be @@ -35,6 +40,7 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + } /** @@ -53,6 +59,7 @@ public class Robot extends TimedRobot { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + System.out.println(Arrays.toString(Vision.getAprilTags())); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index b5fe03d..3731849 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -24,7 +24,7 @@ public class Vision { public AprilTag[] getAprilTags() { if (!m_isTags.getBoolean(false)) return new AprilTag[0]; - + double xarr[] = m_xPoses.getDoubleArray(new double[] {}); double yarr[] = m_yPoses.getDoubleArray(new double[] {}); double zarr[] = m_zPoses.getDoubleArray(new double[] {}); From 213c5b6e3dfa4214ef8842c27a0732c550b6e83a Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Mon, 6 Feb 2023 17:22:37 -0700 Subject: [PATCH 03/22] fefef --- simgui.json | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/simgui.json b/simgui.json index c0596a7..64da7ff 100644 --- a/simgui.json +++ b/simgui.json @@ -5,6 +5,11 @@ } }, "NetworkTables": { + "retained": { + "apriltag": { + "open": true + } + }, "transitory": { "LiveWindow": { ".status": { From a91d334e2c92238afd3519514d2e403e3589416a Mon Sep 17 00:00:00 2001 From: Astatin3 Date: Thu, 16 Feb 2023 18:46:43 -0700 Subject: [PATCH 04/22] Make code work Update basic functions for apriltags and outline for integration --- src/main/java/frc4388/robot/Constants.java | 24 ++- src/main/java/frc4388/robot/Robot.java | 16 +- .../java/frc4388/robot/RobotContainer.java | 74 ++++---- src/main/java/frc4388/robot/RobotMap.java | 7 - .../frc4388/robot/commands/AutoBalance.java | 5 +- .../robot/commands/DriveWithInput.java | 75 ++++++++ .../frc4388/robot/subsystems/Apriltags.java | 35 ++++ .../frc4388/robot/subsystems/Location.java | 44 +++++ .../frc4388/robot/subsystems/SwerveDrive.java | 172 +++++++----------- .../robot/subsystems/SwerveModule.java | 36 +--- 10 files changed, 282 insertions(+), 206 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/DriveWithInput.java create mode 100644 src/main/java/frc4388/robot/subsystems/Apriltags.java create mode 100644 src/main/java/frc4388/robot/subsystems/Location.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index efe227b..e0b2dee 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -24,7 +24,7 @@ import frc4388.utility.LEDPatterns; public final class Constants { public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = -0.7; + public static final double ROTATION_SPEED = 2.0; public static final class IDs { public static final int LEFT_FRONT_WHEEL_ID = 2; @@ -65,13 +65,13 @@ public final class Constants { public static final int CANCODER_TICKS_PER_ROTATION = 4096; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value - public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 1.0; // TODO: find the actual value + public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2.0; // TODO: find the actual value - public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; - public static final double MOTOR_REV_PER_STEER_REV = 12.8; + public static final double MOTOR_REV_PER_WHEEL_REV = 6.12; // TODO: find the actual value + public static final double MOTOR_REV_PER_STEER_REV = 12.8; // TODO: find the actual value public static final double TICKS_PER_MOTOR_REV = 2048; - public static final double WHEEL_DIAMETER_INCHES = 3.9; + public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: the actual value public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; @@ -88,6 +88,7 @@ public final class Constants { public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value + // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2022 SwerveDrive values // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2022 SwerveDrive values // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2022 SwerveDrive values @@ -105,11 +106,11 @@ public final class Constants { } - public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value + public static final double MAX_SPEED_FEET_PER_SECOND = 10; // TODO: find the actual value // dimensions - public static final double WIDTH = 18.5; - public static final double HEIGHT = 18.5; + public static final double WIDTH = 18.5; // TODO: find the actual value + public static final double HEIGHT = 18.5; // TODO: find the actual value public static final double HALF_WIDTH = WIDTH / 2.d; public static final double HALF_HEIGHT = HEIGHT / 2.d; @@ -137,10 +138,7 @@ public final class Constants { public static final class OIConstants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; - - public static final double LEFT_AXIS_DEADBAND = 0.1; - public static final double RIGHT_AXIS_DEADBAND = 0.6; - - public static final boolean SKEW_STICKS = true; + public static final double LEFT_AXIS_DEADBAND = 0.04; + public static final boolean SKEW_STICKS = false; } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 16c67de..ca33f4c 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,15 +7,15 @@ package frc4388.robot; - -import frc4388.robot.subsystems.Vision; - +import java.lang.System; +import java.lang.reflect.Array; import java.util.Arrays; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.RobotTime; +import frc4388.robot.subsystems.Vision; /** * The VM is configured to automatically run this class, and to call the @@ -29,7 +29,9 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - private Vision Vision = new Vision(); + + //private Vision Apriltag = new Vision(); + /** * This function is run when the robot is first started up and should be @@ -40,7 +42,6 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - } /** @@ -59,7 +60,10 @@ public class Robot extends TimedRobot { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - System.out.println(Arrays.toString(Vision.getAprilTags())); + + //final Object[] apriltagPos = Apriltag.getApriltagLocation(); + + //ystem.out.print(apriltagPos[0]); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a3690d9..031cd46 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -10,12 +10,11 @@ package frc4388.robot; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.AutoBalance; +import frc4388.robot.commands.DriveWithInput; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -31,16 +30,13 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro); + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro); // private final LED m_robotLED = new LED(m_robotMap.LEDController); /* Controllers */ - // private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); - // private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); - - private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); - private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); + private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -50,11 +46,15 @@ public class RobotContainer { /* Default Commands */ - m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> - m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(), true) - , m_robotSwerveDrive).withName("SwerveDrive DefaultCommand")); + m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive, + () -> getDriverController().getLeftXAxis(), + () -> getDriverController().getLeftYAxis(), + () -> getDriverController().getRightXAxis(), + false)); // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + + } @@ -67,20 +67,18 @@ public class RobotContainer { private void configureButtonBindings() { /* Driver Buttons */ - new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); - - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); - // // .onFalse() + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotMap.gyro.reset())); + // .onFalse() - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + /* Operator Buttons */ + + new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - - // /* Operator Buttons */ - // // interrupt button - // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand()); + + // interrupt button + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + .onTrue(new InstantCommand()); } /** @@ -96,36 +94,28 @@ public class RobotContainer { /** * Add your docs here. */ - // public IHandController getDriverController() { - // return m_driverXbox; - // } - - public DeadbandedXboxController getDeadbandedDriverController() { - return this.m_driverXbox; - } - - public DeadbandedXboxController getDeadbandedOperatorController() { - return this.m_operatorXbox; + public IHandController getDriverController() { + return m_driverXbox; } /** * Add your docs here. */ - // public IHandController getOperatorController() { - // return m_operatorXbox; - // } + public IHandController getOperatorController() { + return m_operatorXbox; + } /** * Add your docs here. */ - // public Joystick getOperatorJoystick() { - // return m_operatorXbox.getJoyStick(); - // } + public Joystick getOperatorJoystick() { + return m_operatorXbox.getJoyStick(); + } /** * Add your docs here. */ - // public Joystick getDriverJoystick() { - // return m_driverXbox.getJoyStick(); - // } + public Joystick getDriverJoystick() { + return m_driverXbox.getJoyStick(); + } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 9b5ae04..93a114d 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,7 +7,6 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_Pigeon2; @@ -114,12 +113,6 @@ public class RobotMap { rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // set neutral mode - leftFrontSteer.setNeutralMode(NeutralMode.Brake); - rightFrontSteer.setNeutralMode(NeutralMode.Brake); - leftBackSteer.setNeutralMode(NeutralMode.Brake); - rightBackSteer.setNeutralMode(NeutralMode.Brake); - // initialize SwerveModules this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469); this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234); diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index ffccee4..374de0a 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -5,7 +5,6 @@ package frc4388.robot.commands; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.RobotGyro; @@ -16,7 +15,7 @@ public class AutoBalance extends PelvicInflammatoryDisease { /** Creates a new AutoBalanceTF2. */ public AutoBalance(RobotGyro gyro, SwerveDrive drive) { - super(0.6, 0, 0, 0); + super(1.0, 0, 0, 0); this.gyro = gyro; this.drive = drive; @@ -35,7 +34,7 @@ public class AutoBalance extends PelvicInflammatoryDisease { public void runWithOutput(double output) { double out2 = MathUtil.clamp(output / 40, -.5, .5); if (Math.abs(gyro.getPitch()) < 3) out2 = 0; - drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false); + drive.drive(out2, 0, 0, false); } @Override diff --git a/src/main/java/frc4388/robot/commands/DriveWithInput.java b/src/main/java/frc4388/robot/commands/DriveWithInput.java new file mode 100644 index 0000000..3b4d638 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveWithInput.java @@ -0,0 +1,75 @@ +// 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 frc4388.robot.commands; + +import java.util.function.Supplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.subsystems.SwerveDrive; + +public class DriveWithInput extends CommandBase { + + private final SwerveDrive swerve; + + private final Supplier xSpeed; + private final Supplier ySpeed; + private final Supplier rot; + private final boolean fieldRelative; + + private final SlewRateLimiter xLimiter, yLimiter, rotLimiter; + + /** Creates a new DriveWithInput. */ + public DriveWithInput(SwerveDrive swerve, Supplier xSpeed, Supplier ySpeed, Supplier rot, boolean fieldRelative) { + // Use addRequirements() here to declare subsystem dependencies. + this.swerve = swerve; + this.xSpeed = xSpeed; + this.ySpeed = ySpeed; + this.rot = rot; + this.fieldRelative = fieldRelative; + + this.xLimiter = new SlewRateLimiter(3); + this.yLimiter = new SlewRateLimiter(3); + this.rotLimiter = new SlewRateLimiter(3); + + addRequirements(swerve); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double x = xSpeed.get(); + double y = ySpeed.get(); + double r = rot.get(); + + x = -xLimiter.calculate(MathUtil.applyDeadband(x * -0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + y = -yLimiter.calculate(MathUtil.applyDeadband(y * 0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + r = -rotLimiter.calculate(MathUtil.applyDeadband(r * 0.3, 0.02) * Units.feetToMeters(5*Math.PI)); + + swerve.drive(x, y, r, fieldRelative); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + System.out.println("----------------------------------------------------------------"); + System.out.println("DriveWithInput ended"); + System.out.println("Interrupted: " + interrupted); + System.out.println("----------------------------------------------------------------"); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java new file mode 100644 index 0000000..ccf1617 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Apriltags.java @@ -0,0 +1,35 @@ +package frc4388.robot.subsystems; + +//import edu.wpi.first.apriltag.AprilTag; +//import edu.wpi.first.math.geometry.Pose3d; +//import edu.wpi.first.math.geometry.Rotation3d; +//import edu.wpi.first.networktables.NetworkTable; +//import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class Apriltags { + public Object[] getApriltagPosition() { + final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); + + return new Object[] {true, + tagTable.getEntry("TagPosX"), + tagTable.getEntry("TagPosY"), + tagTable.getEntry("TagPosZ") + }; + } + + public Object[] getApriltagRotation() { + final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); + + return new Object[] {true, + tagTable.getEntry("TagRotY"), + tagTable.getEntry("TagRotP"), + tagTable.getEntry("TagRotR") + }; + } + + public boolean isAprilTag() { + final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); + return tagTable.getEntry("IsTag").getBoolean(false); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Location.java b/src/main/java/frc4388/robot/subsystems/Location.java new file mode 100644 index 0000000..b3e786a --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Location.java @@ -0,0 +1,44 @@ +package frc4388.robot.subsystems; + +import frc4388.robot.subsystems.Apriltags; + +public class Location { + final Apriltags Apriltag = new Apriltags(); + + public boolean isLimelight = false; + public boolean isApriltag = false; + + //Determines which source to get pos and rot from and also resets + public void reoderPrio(){ + isLimelight = false; //If limelight gets position and if within a certain range of poles + isApriltag = Apriltag.isAprilTag(); + } + + public Object[] getPosition() { + Object[] Position = {}; + + if(isLimelight){ + //Return Limelight Position + }else if(isApriltag){ + return Apriltag.getApriltagPosition(); + }else{ + //Return odometry Position, last resort + } + + return Position; + } + + public Object[] getRotation() { + Object[] Rotation = {}; + + if(isLimelight){ + //Return Limelight Rotation + }else if(isApriltag){ + return Apriltag.getApriltagRotation(); + }else{ + //Return odometry Rotation, last resort + } + + return Rotation; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 17bdd5b..d93f892 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,27 +4,20 @@ package frc4388.robot.subsystems; - -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.math.kinematics.ChassisSpeeds; 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.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.utility.RobotGyro; public class SwerveDrive extends SubsystemBase { - private SwerveModule leftFront; - private SwerveModule rightFront; - private SwerveModule leftBack; - private SwerveModule rightBack; + public SwerveModule leftFront; + public SwerveModule rightFront; + public SwerveModule leftBack; + public SwerveModule rightBack; private SwerveModule[] modules; @@ -33,64 +26,39 @@ public class SwerveDrive extends SubsystemBase { private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - private RobotGyro gyro; - - private SwerveDriveOdometry odometry; + // private SwerveDriveOdometry odometry = new SwerveDriveOdometry( + // kinematics, + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // } + // ); public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default - public Rotation2d rotTarget = new Rotation2d(); - public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { + public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack) {//, RobotGyro gyro) { this.leftFront = leftFront; this.rightFront = rightFront; this.leftBack = leftBack; this.rightBack = rightBack; - this.gyro = gyro; - - this.odometry = new SwerveDriveOdometry( - kinematics, - gyro.getRotation2d(), - new SwerveModulePosition[] { - leftFront.getPosition(), - rightFront.getPosition(), - leftBack.getPosition(), - rightBack.getPosition() - } - ); this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } - - public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - if (fieldRelative) { - - if (rightStick.getNorm() > 0.1) { - rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1)); - } - - double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); - - - // Use the left joystick to set speed. Apply a quadratic curve and the set max speed. - Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - - // if (rightStick.getNorm() < .1) { - // rot = 0; - // } - - // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); - - } else { - // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); - } - setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); + // WPILib swerve drive example + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { + // SwerveModuleState[] states = kinematics.toSwerveModuleStates( + // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d()) + // : new ChassisSpeeds(xSpeed, ySpeed, rot) + // ); + SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED)); + setModuleStates(states); } /** @@ -98,71 +66,70 @@ public class SwerveDrive extends SubsystemBase { * @param desiredStates Array of module states to set. */ public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); for (int i = 0; i < desiredStates.length; i++) { SwerveModule module = modules[i]; SwerveModuleState state = desiredStates[i]; - module.setDesiredState(state, false); + module.setDesiredState(state); } } - public double getGyroAngle() { - return gyro.getAngle(); - } - - public void resetGyro() { - gyro.reset(); - setOdometry(getOdometry()); - rotTarget = new Rotation2d(0); - } - /** * Updates the odometry of the SwerveDrive. */ - public void updateOdometry() { - odometry.update( - gyro.getRotation2d(), - new SwerveModulePosition[] { - leftFront.getPosition(), - rightFront.getPosition(), - leftBack.getPosition(), - rightBack.getPosition() - } - ); - } + // public void updateOdometry() { + // odometry.update( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // } + // ); + // } /** * Gets the odometry of the SwerveDrive. * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta). */ - public Pose2d getOdometry() { - return odometry.getPoseMeters(); - } + // public Pose2d getOdometry() { + // return odometry.getPoseMeters(); + // } /** * Sets the odometry of the SwerveDrive. * @param pose Pose to set the odometry to. */ - public void setOdometry(Pose2d pose) { - odometry.resetPosition( - gyro.getRotation2d(), - new SwerveModulePosition[] { - leftFront.getPosition(), - rightFront.getPosition(), - leftBack.getPosition(), - rightBack.getPosition() - }, - pose - ); - } + // public void setOdometry(Pose2d pose) { + // odometry.resetPosition( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // }, + // pose + // ); + // } /** * Resets the odometry of the SwerveDrive to 0. - * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions. + * *NOTE: If you reset your gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions. */ - public void resetOdometry() { - setOdometry(new Pose2d()); - } + // public void resetOdometry() { + // odometry.resetPosition( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // }, + // new Pose2d() + // ); + // } public SwerveDriveKinematics getKinematics() { return this.kinematics; @@ -171,14 +138,7 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run - updateOdometry(); - - SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX())); - SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY())); - SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees()); - - SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); - SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees()); + // updateOdometry(); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 2616b71..4bbbefb 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -20,8 +20,8 @@ import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; public class SwerveModule extends SubsystemBase { - private WPI_TalonFX driveMotor; - private WPI_TalonFX angleMotor; + public WPI_TalonFX driveMotor; + public WPI_TalonFX angleMotor; private CANCoder encoder; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; @@ -44,9 +44,6 @@ public class SwerveModule extends SubsystemBase { angleMotor.configAllSettings(angleConfig); encoder.configMagnetOffset(offset); - - driveMotor.setSelectedSensorPosition(0); - driveMotor.config_kP(0, 0.2); } /** @@ -82,18 +79,6 @@ public class SwerveModule extends SubsystemBase { return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); } - public double getAngularVel() { - return this.angleMotor.getSelectedSensorVelocity(); - } - - public double getDrivePos() { - return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; - } - - public double getDriveVel() { - return this.driveMotor.getSelectedSensorVelocity(0); - } - public void stop() { driveMotor.set(0); angleMotor.set(0); @@ -126,32 +111,25 @@ public class SwerveModule extends SubsystemBase { * Set the speed and rotation of the SwerveModule from a SwerveModuleState object * @param desiredState a SwerveModuleState representing the desired new state of the module */ - public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) { + public void setDesiredState(SwerveModuleState desiredState) { Rotation2d currentRotation = this.getAngle(); SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); // calculate the difference between our current rotational position and our new rotational position - Rotation2d rotationDelta = state.angle.minus(currentRotation); // ? might need to be negative + Rotation2d rotationDelta = state.angle.minus(currentRotation); // calculate the new absolute position of the SwerveModule based on the difference in rotation double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; // convert the CANCoder from its position reading to ticks - double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); // ? why feedback coefficient - - if (!ignoreAngle) { - angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); - } + double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); + angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - // double inchesPerSecond = Units.metersToFeet(state.speedMetersPerSecond) * 12; + driveMotor.set(-1 * angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); // driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME); - // driveMotor.set(0.1); - // double angleCorrection = getAngularVel() * 2.69; - driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME); } public void reset(double position) { From c12e657aeee7d361fd4707a9662fcd9f2ee182e6 Mon Sep 17 00:00:00 2001 From: Astatin3 Date: Thu, 16 Feb 2023 19:46:21 -0700 Subject: [PATCH 05/22] Update that does not currently work Work on shuffleboard integration --- simgui.json | 15 +++++++++++++++ src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/Robot.java | 15 ++++++++++++--- .../java/frc4388/robot/subsystems/Apriltags.java | 15 +++++++++++---- .../java/frc4388/robot/subsystems/Location.java | 6 +----- .../frc4388/robot/subsystems/SwerveDrive.java | 2 +- 6 files changed, 41 insertions(+), 14 deletions(-) diff --git a/simgui.json b/simgui.json index 64da7ff..59ccda3 100644 --- a/simgui.json +++ b/simgui.json @@ -5,6 +5,15 @@ } }, "NetworkTables": { + "Persistent Values": { + "open": false + }, + "Retained Values": { + "open": false + }, + "Transitory Values": { + "open": false + }, "retained": { "apriltag": { "open": true @@ -55,6 +64,12 @@ }, "open": true }, + "shuffleboard": { + "Subscribers": { + "open": true + }, + "open": true + }, "visible": true } } diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e0b2dee..57db05a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -139,6 +139,6 @@ public final class Constants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; public static final double LEFT_AXIS_DEADBAND = 0.04; - public static final boolean SKEW_STICKS = false; + public static final boolean SKEW_STICKS = false; // TODO: find the actual value } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index ca33f4c..d57fa12 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -15,7 +15,10 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.RobotTime; -import frc4388.robot.subsystems.Vision; + +import frc4388.robot.subsystems.Location; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * The VM is configured to automatically run this class, and to call the @@ -30,7 +33,7 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - //private Vision Apriltag = new Vision(); + private Location location = new Location(); /** @@ -61,7 +64,13 @@ public class Robot extends TimedRobot { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - //final Object[] apriltagPos = Apriltag.getApriltagLocation(); + // Call position and rotation reorder + location.reoderPrio(); + + final Object[] pos = location.getPosition(); + if (pos != null) { + SmartDashboard.putNumber("name", (Double) pos[0]); + } //ystem.out.print(apriltagPos[0]); } diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java index ccf1617..48b097c 100644 --- a/src/main/java/frc4388/robot/subsystems/Apriltags.java +++ b/src/main/java/frc4388/robot/subsystems/Apriltags.java @@ -11,10 +11,16 @@ public class Apriltags { public Object[] getApriltagPosition() { final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); + // return new Object[] {true, + // tagTable.getEntry("TagPosX"), + // tagTable.getEntry("TagPosY"), + // tagTable.getEntry("TagPosZ") + // }; + return new Object[] {true, - tagTable.getEntry("TagPosX"), - tagTable.getEntry("TagPosY"), - tagTable.getEntry("TagPosZ") + 1, + 2, + 3 }; } @@ -30,6 +36,7 @@ public class Apriltags { public boolean isAprilTag() { final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - return tagTable.getEntry("IsTag").getBoolean(false); + // return tagTable.getEntry("IsTag").getBoolean(false); + return true; } } diff --git a/src/main/java/frc4388/robot/subsystems/Location.java b/src/main/java/frc4388/robot/subsystems/Location.java index b3e786a..3683629 100644 --- a/src/main/java/frc4388/robot/subsystems/Location.java +++ b/src/main/java/frc4388/robot/subsystems/Location.java @@ -15,17 +15,13 @@ public class Location { } public Object[] getPosition() { - Object[] Position = {}; - if(isLimelight){ //Return Limelight Position }else if(isApriltag){ return Apriltag.getApriltagPosition(); - }else{ - //Return odometry Position, last resort } - return Position; + return null; } public Object[] getRotation() { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index d93f892..e9ef898 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -28,7 +28,7 @@ public class SwerveDrive extends SubsystemBase { private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - // private SwerveDriveOdometry odometry = new SwerveDriveOdometry( + // private SwerveDriveOdometry odometry = new SwerveDrive( // kinematics, // gyro.getRotation2d(), // new SwerveModulePosition[] { From 267d6cc8e9566b472013bae75ffb1cf1d1d449ed Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sat, 18 Feb 2023 15:26:38 -0700 Subject: [PATCH 06/22] Update Constants.java --- src/main/java/frc4388/robot/Constants.java | 38 +++++++--------------- 1 file changed, 11 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 57db05a..c18f200 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -24,7 +24,7 @@ import frc4388.utility.LEDPatterns; public final class Constants { public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 2.0; + public static final double ROTATION_SPEED = -0.7; public static final class IDs { public static final int LEFT_FRONT_WHEEL_ID = 2; @@ -64,14 +64,14 @@ public final class Constants { public static final class Conversions { public static final int CANCODER_TICKS_PER_ROTATION = 4096; - public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value - public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2.0; // TODO: find the actual value + public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 5.8; + public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8; - public static final double MOTOR_REV_PER_WHEEL_REV = 6.12; // TODO: find the actual value - public static final double MOTOR_REV_PER_STEER_REV = 12.8; // TODO: find the actual value + public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; + public static final double MOTOR_REV_PER_STEER_REV = 12.8; public static final double TICKS_PER_MOTOR_REV = 2048; - public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: the actual value + public static final double WHEEL_DIAMETER_INCHES = 3.9; public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; @@ -87,30 +87,13 @@ public final class Constants { public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value - - - // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2022 SwerveDrive values - // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2022 SwerveDrive values - // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2022 SwerveDrive values - // public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // * 2022 SwerveDrive values - - // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2023 translated values (don't work) - // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // * 2023 translated values (don't work) - // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2023 translated values (don't work) - // public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2023 translated values (don't work) - - // public static final double LEFT_FRONT_ENCODER_OFFSET = 0.0 + 90.0; // * 2023 experimentally-derived values (mostly work) - // public static final double RIGHT_FRONT_ENCODER_OFFSET = 0.0; // * 2023 experimentally-derived values (mostly work) - // public static final double LEFT_BACK_ENCODER_OFFSET = 0.0 + 24.0; // * 2023 experimentally-derived values (mostly work) - // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0 + 45.0 + 180.0; // * 2023 experimentally-derived values (mostly work) - } public static final double MAX_SPEED_FEET_PER_SECOND = 10; // TODO: find the actual value // dimensions - public static final double WIDTH = 18.5; // TODO: find the actual value - public static final double HEIGHT = 18.5; // TODO: find the actual value + public static final double WIDTH = 18.5; + public static final double HEIGHT = 18.5; public static final double HALF_WIDTH = WIDTH / 2.d; public static final double HALF_HEIGHT = HEIGHT / 2.d; @@ -138,7 +121,8 @@ public final class Constants { public static final class OIConstants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; - public static final double LEFT_AXIS_DEADBAND = 0.04; - public static final boolean SKEW_STICKS = false; // TODO: find the actual value + public static final double LEFT_AXIS_DEADBAND = 0.1; + public static final double RIGHT_AXIS_DEADBAND = 0.6; + public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing } } From 92243d094242f071039df241c89e98d88c6dbe0a Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sun, 19 Feb 2023 11:10:57 -0700 Subject: [PATCH 07/22] Added tag struct. Should integrate later --- src/main/java/frc4388/robot/subsystems/Apriltags.java | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java index 48b097c..bc24769 100644 --- a/src/main/java/frc4388/robot/subsystems/Apriltags.java +++ b/src/main/java/frc4388/robot/subsystems/Apriltags.java @@ -8,9 +8,16 @@ package frc4388.robot.subsystems; import edu.wpi.first.networktables.NetworkTableInstance; public class Apriltags { + public static class Tag { + boolean visible = true; + double x, y, z = 0; + } + public Object[] getApriltagPosition() { final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - + final var tag = new Tag(); + // integrate the tag system + // return new Object[] {true, // tagTable.getEntry("TagPosX"), // tagTable.getEntry("TagPosY"), From c546a2a294d26667fafbfbc498b66067364c87b6 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 27 Feb 2023 17:48:04 -0700 Subject: [PATCH 08/22] import RobotGyro --- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 9f29094..21a02f8 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.utility.RobotGyro; public class SwerveDrive extends SubsystemBase { From 73f3f1516f9b847057f8e5e992de7c9ed3f1e67e Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 27 Feb 2023 18:08:25 -0700 Subject: [PATCH 09/22] fix ignoreangles --- src/main/java/frc4388/robot/subsystems/SwerveModule.java | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 2ed5df8..64e8f89 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -125,9 +125,7 @@ public class SwerveModule extends SubsystemBase { // convert the CANCoder from its position reading to ticks double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); - if (!ignoreAngle) { - angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); - } + angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); @@ -146,4 +144,4 @@ public class SwerveModule extends SubsystemBase { public double getVoltage() { return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); } -} \ No newline at end of file +} From fc753cddde4c2f449c1618eebf65bee63b376618 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 27 Feb 2023 18:18:29 -0700 Subject: [PATCH 10/22] fix swervemoduleposition --- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 21a02f8..411551f 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.RobotGyro; From 2d4b4243e10c5acc7b8637d5141bdd0f5a31e19f Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 27 Feb 2023 18:20:24 -0700 Subject: [PATCH 11/22] fix rottarget and chassisspeeds --- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 411551f..4f7f888 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -40,6 +40,9 @@ public class SwerveDrive extends SubsystemBase { private SwerveDrivePoseEstimator poseEstimator; public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default + + public Rotation2d rotTarget = new Rotation2d(); + public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); /** Creates a new SwerveDrive. */ public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack) {//, RobotGyro gyro) { From 181654827c5388f30aeed1c229d73208439a0146 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 27 Feb 2023 18:24:07 -0700 Subject: [PATCH 12/22] fix swervedrive parameters (gyro) --- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 4f7f888..e68b65c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -45,7 +45,7 @@ public class SwerveDrive extends SubsystemBase { public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack) {//, RobotGyro gyro) { + public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { this.leftFront = leftFront; this.rightFront = rightFront; this.leftBack = leftBack; From 04ab0cf4b8d348cd493e89772334466a42464133 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 27 Feb 2023 18:25:14 -0700 Subject: [PATCH 13/22] Delete DriveWithInput.java --- .../robot/commands/DriveWithInput.java | 75 ------------------- 1 file changed, 75 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/DriveWithInput.java diff --git a/src/main/java/frc4388/robot/commands/DriveWithInput.java b/src/main/java/frc4388/robot/commands/DriveWithInput.java deleted file mode 100644 index 3b4d638..0000000 --- a/src/main/java/frc4388/robot/commands/DriveWithInput.java +++ /dev/null @@ -1,75 +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 frc4388.robot.commands; - -import java.util.function.Supplier; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.subsystems.SwerveDrive; - -public class DriveWithInput extends CommandBase { - - private final SwerveDrive swerve; - - private final Supplier xSpeed; - private final Supplier ySpeed; - private final Supplier rot; - private final boolean fieldRelative; - - private final SlewRateLimiter xLimiter, yLimiter, rotLimiter; - - /** Creates a new DriveWithInput. */ - public DriveWithInput(SwerveDrive swerve, Supplier xSpeed, Supplier ySpeed, Supplier rot, boolean fieldRelative) { - // Use addRequirements() here to declare subsystem dependencies. - this.swerve = swerve; - this.xSpeed = xSpeed; - this.ySpeed = ySpeed; - this.rot = rot; - this.fieldRelative = fieldRelative; - - this.xLimiter = new SlewRateLimiter(3); - this.yLimiter = new SlewRateLimiter(3); - this.rotLimiter = new SlewRateLimiter(3); - - addRequirements(swerve); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - double x = xSpeed.get(); - double y = ySpeed.get(); - double r = rot.get(); - - x = -xLimiter.calculate(MathUtil.applyDeadband(x * -0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - y = -yLimiter.calculate(MathUtil.applyDeadband(y * 0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - r = -rotLimiter.calculate(MathUtil.applyDeadband(r * 0.3, 0.02) * Units.feetToMeters(5*Math.PI)); - - swerve.drive(x, y, r, fieldRelative); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - System.out.println("----------------------------------------------------------------"); - System.out.println("DriveWithInput ended"); - System.out.println("Interrupted: " + interrupted); - System.out.println("----------------------------------------------------------------"); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} From ab9a6ec204c93d3e53d5db4834e4e6d62ceed375 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 27 Feb 2023 18:25:59 -0700 Subject: [PATCH 14/22] fix translation2d --- src/main/java/frc4388/robot/commands/AutoBalance.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index 58ee732..e8d398e 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -5,6 +5,7 @@ package frc4388.robot.commands; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.RobotGyro; From 58a0a91610a8bff3433c79bf7b3bfe65fdc3f194 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 27 Feb 2023 18:28:32 -0700 Subject: [PATCH 15/22] fix drivewithinput --- src/main/java/frc4388/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1d181cf..6df6a6a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -35,7 +35,6 @@ import frc4388.robot.Constants.*; import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants; import frc4388.robot.commands.AutoBalance; -import frc4388.robot.commands.DriveWithInput; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.controller.IHandController; import frc4388.robot.commands.JoystickPlayback; From d9040b21aa9234e128c96609581f34a389d01c4a Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 27 Feb 2023 18:30:38 -0700 Subject: [PATCH 16/22] fix getdriverjoystick --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6df6a6a..cf390e9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -128,7 +128,7 @@ public class RobotContainer { // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); // // .onFalse() - new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) From 4cd9ab5c30e5dfbcf6cf0adb20f90e17181288f5 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 27 Feb 2023 18:32:20 -0700 Subject: [PATCH 17/22] neutralmode re-implementation --- src/main/java/frc4388/robot/RobotMap.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 93a114d..373d0e8 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -8,6 +8,7 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_Pigeon2; @@ -112,6 +113,12 @@ public class RobotMap { rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + // set neutral mode + leftFrontSteer.setNeutralMode(NeutralMode.Brake); + rightFrontSteer.setNeutralMode(NeutralMode.Brake); + leftBackSteer.setNeutralMode(NeutralMode.Brake); + rightBackSteer.setNeutralMode(NeutralMode.Brake); // initialize SwerveModules this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469); From 9948035f6f90f4b94ede8ac4834ff059ff0920b0 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 27 Feb 2023 18:33:14 -0700 Subject: [PATCH 18/22] fix autobalance kP --- src/main/java/frc4388/robot/commands/AutoBalance.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index e8d398e..2da529c 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -16,7 +16,7 @@ public class AutoBalance extends PelvicInflammatoryDisease { /** Creates a new AutoBalanceTF2. */ public AutoBalance(RobotGyro gyro, SwerveDrive drive) { - super(1.0, 0, 0, 0); + super(0.6, 0, 0, 0); this.gyro = gyro; this.drive = drive; From 3249ff1d2f2cbc7020c665b1249eb41539f05bdb Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 27 Feb 2023 18:35:20 -0700 Subject: [PATCH 19/22] fix SwerveDrive.java --- .../java/frc4388/robot/subsystems/SwerveDrive.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index e68b65c..ac27c3b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -19,10 +19,10 @@ import frc4388.utility.RobotGyro; public class SwerveDrive extends SubsystemBase { - public SwerveModule leftFront; - public SwerveModule rightFront; - public SwerveModule leftBack; - public SwerveModule rightBack; + private SwerveModule leftFront; + private SwerveModule rightFront; + private SwerveModule leftBack; + private SwerveModule rightBack; private SwerveModule[] modules; @@ -106,7 +106,7 @@ public class SwerveDrive extends SubsystemBase { * @param desiredStates Array of module states to set. */ public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); for (int i = 0; i < desiredStates.length; i++) { SwerveModule module = modules[i]; SwerveModuleState state = desiredStates[i]; @@ -205,7 +205,7 @@ public class SwerveDrive extends SubsystemBase { /** * Resets the odometry of the SwerveDrive to 0. - * *NOTE: If you reset your gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions. + * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions. */ // public void resetOdometry() { // setOdometry(new Pose2d()); From 748cfe442839ee4c8e8d5d4e4558defca1f6c580 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 27 Feb 2023 18:37:59 -0700 Subject: [PATCH 20/22] fix SwerveModule.java --- .../robot/subsystems/SwerveModule.java | 22 +++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 64e8f89..1ddab51 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -20,8 +20,8 @@ import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; public class SwerveModule extends SubsystemBase { - public WPI_TalonFX driveMotor; - public WPI_TalonFX angleMotor; + private WPI_TalonFX driveMotor; + private WPI_TalonFX angleMotor; private CANCoder encoder; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; @@ -44,6 +44,9 @@ public class SwerveModule extends SubsystemBase { angleMotor.configAllSettings(angleConfig); encoder.configMagnetOffset(offset); + + driveMotor.setSelectedSensorPosition(0); + driveMotor.config_kP(0, 0.2); } /** @@ -78,6 +81,18 @@ public class SwerveModule extends SubsystemBase { // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); } + + public double getAngularVel() { + return this.angleMotor.getSelectedSensorVelocity(); + } + + public double getDrivePos() { + return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; + } + + public double getDriveVel() { + return this.driveMotor.getSelectedSensorVelocity(0); + } public void stop() { driveMotor.set(0); @@ -129,8 +144,7 @@ public class SwerveModule extends SubsystemBase { double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - driveMotor.set(-1 * angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); - // driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME); + driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); } public void reset(double position) { From 8132dde7c2fbe8b358fdbf8b97c7f778f817caa7 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 27 Feb 2023 18:42:27 -0700 Subject: [PATCH 21/22] fixed local var visibility issue --- src/main/java/frc4388/robot/subsystems/Location.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Location.java b/src/main/java/frc4388/robot/subsystems/Location.java index 3683629..e5d6959 100644 --- a/src/main/java/frc4388/robot/subsystems/Location.java +++ b/src/main/java/frc4388/robot/subsystems/Location.java @@ -5,8 +5,8 @@ import frc4388.robot.subsystems.Apriltags; public class Location { final Apriltags Apriltag = new Apriltags(); - public boolean isLimelight = false; - public boolean isApriltag = false; + private boolean isLimelight = false; + private boolean isApriltag = false; //Determines which source to get pos and rot from and also resets public void reoderPrio(){ From 73007e5458581f759270ae40386862e2475c7b33 Mon Sep 17 00:00:00 2001 From: Astatin3 Date: Tue, 28 Feb 2023 17:05:18 -0700 Subject: [PATCH 22/22] code review --- src/main/java/frc4388/robot/Robot.java | 9 ++-- .../frc4388/robot/subsystems/Apriltags.java | 43 +++++++------------ .../frc4388/robot/subsystems/Location.java | 33 +++++--------- 3 files changed, 28 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 54f68d3..ac78650 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -22,7 +22,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.RobotTime; import frc4388.robot.subsystems.Location; - +import frc4388.robot.subsystems.Apriltags.Tag; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** @@ -71,12 +71,9 @@ public class Robot extends TimedRobot { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - // Call position and rotation reorder - location.reoderPrio(); - - final Object[] pos = location.getPosition(); + final Tag pos = location.getPosRot(); if (pos != null) { - SmartDashboard.putNumber("name", (Double) pos[0]); + SmartDashboard.putNumber("x position", pos.x); } //ystem.out.print(apriltagPos[0]); diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java index bc24769..c6062e8 100644 --- a/src/main/java/frc4388/robot/subsystems/Apriltags.java +++ b/src/main/java/frc4388/robot/subsystems/Apriltags.java @@ -9,41 +9,28 @@ import edu.wpi.first.networktables.NetworkTableInstance; public class Apriltags { public static class Tag { - boolean visible = true; - double x, y, z = 0; - } - - public Object[] getApriltagPosition() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - final var tag = new Tag(); - // integrate the tag system - - // return new Object[] {true, - // tagTable.getEntry("TagPosX"), - // tagTable.getEntry("TagPosY"), - // tagTable.getEntry("TagPosZ") - // }; - - return new Object[] {true, - 1, - 2, - 3 - }; + public boolean visible = true; + public double x, y, z = 0; + public double ry, rp, rr = 0; } - public Object[] getApriltagRotation() { + public Tag getTagPosRot() { final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - return new Object[] {true, - tagTable.getEntry("TagRotY"), - tagTable.getEntry("TagRotP"), - tagTable.getEntry("TagRotR") - }; + final Tag tag = new Tag(); + tag.visible = isAprilTag(); + tag.x = tagTable.getEntry("TagPosX").getDouble(0); + tag.y = tagTable.getEntry("TagPosY").getDouble(0); + tag.z = tagTable.getEntry("TagPosZ").getDouble(0); + tag.ry = tagTable.getEntry("TagRotY").getDouble(0); + tag.rp = tagTable.getEntry("TagRotP").getDouble(0); + tag.rr = tagTable.getEntry("TagRotR").getDouble(0); + + return tag; } public boolean isAprilTag() { final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - // return tagTable.getEntry("IsTag").getBoolean(false); - return true; + return tagTable.getEntry("IsTag").getBoolean(false); } } diff --git a/src/main/java/frc4388/robot/subsystems/Location.java b/src/main/java/frc4388/robot/subsystems/Location.java index e5d6959..0bf5322 100644 --- a/src/main/java/frc4388/robot/subsystems/Location.java +++ b/src/main/java/frc4388/robot/subsystems/Location.java @@ -1,40 +1,27 @@ package frc4388.robot.subsystems; -import frc4388.robot.subsystems.Apriltags; +import frc4388.robot.subsystems.Apriltags.Tag; public class Location { - final Apriltags Apriltag = new Apriltags(); + final Apriltags apriltag = new Apriltags(); private boolean isLimelight = false; private boolean isApriltag = false; //Determines which source to get pos and rot from and also resets - public void reoderPrio(){ + private void reoderPrio(){ isLimelight = false; //If limelight gets position and if within a certain range of poles - isApriltag = Apriltag.isAprilTag(); + isApriltag = apriltag.isAprilTag(); } - public Object[] getPosition() { - if(isLimelight){ - //Return Limelight Position - }else if(isApriltag){ - return Apriltag.getApriltagPosition(); + public Tag getPosRot() { + reoderPrio(); + if(isApriltag){ + return apriltag.getTagPosRot(); + } else if (isLimelight) { + return null; } return null; } - - public Object[] getRotation() { - Object[] Rotation = {}; - - if(isLimelight){ - //Return Limelight Rotation - }else if(isApriltag){ - return Apriltag.getApriltagRotation(); - }else{ - //Return odometry Rotation, last resort - } - - return Rotation; - } }