diff --git a/simgui.json b/simgui.json index 449f4b1..59ccda3 100644 --- a/simgui.json +++ b/simgui.json @@ -3,5 +3,73 @@ "types": { "/FMSInfo": "FMSInfo" } + }, + "NetworkTables": { + "Persistent Values": { + "open": false + }, + "Retained Values": { + "open": false + }, + "Transitory Values": { + "open": false + }, + "retained": { + "apriltag": { + "open": true + } + }, + "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 + }, + "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 6aad04d..29a2125 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -63,8 +63,8 @@ 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 = 1.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 = 5.12; public static final double MOTOR_REV_PER_STEER_REV = 12.8; @@ -86,22 +86,6 @@ 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 = 5; // TODO: find the actual value @@ -137,10 +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.1; public static final double RIGHT_AXIS_DEADBAND = 0.6; - - public static final boolean SKEW_STICKS = true; + public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 04dbb0b..ac78650 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,6 +7,9 @@ package frc4388.robot; +import java.lang.System; +import java.lang.reflect.Array; +import java.util.Arrays; import java.io.File; import java.io.IOException; import java.io.PrintWriter; @@ -18,6 +21,10 @@ import edu.wpi.first.wpilibj2.command.Command; 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; + /** * The VM is configured to automatically run this class, and to call the * functions corresponding to each mode, as described in the TimedRobot @@ -31,6 +38,8 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; + private Location location = new Location(); + /** * This function is run when the robot is first started up and should be @@ -61,6 +70,13 @@ 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(); + + final Tag pos = location.getPosRot(); + if (pos != null) { + SmartDashboard.putNumber("x position", pos.x); + } + + //ystem.out.print(apriltagPos[0]); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0267c8b..cf390e9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -35,6 +35,8 @@ import frc4388.robot.Constants.*; import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants; import frc4388.robot.commands.AutoBalance; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.controller.IHandController; import frc4388.robot.commands.JoystickPlayback; import frc4388.robot.commands.JoystickRecorder; import frc4388.robot.commands.PlaybackChooser; diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 9b5ae04..373d0e8 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,8 +7,8 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.NeutralMode; 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; @@ -113,7 +113,7 @@ 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); diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index a1c5910..2da529c 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -34,6 +34,7 @@ public class AutoBalance extends PelvicInflammatoryDisease { @Override public void runWithOutput(double output) { double out2 = MathUtil.clamp(output / 40, -.5, .5); + if (Math.abs(getError()) < 3) out2 = 0; drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), 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..c6062e8 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Apriltags.java @@ -0,0 +1,36 @@ +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 static class Tag { + public boolean visible = true; + public double x, y, z = 0; + public double ry, rp, rr = 0; + } + + public Tag getTagPosRot() { + final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); + + 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); + } +} 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..0bf5322 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Location.java @@ -0,0 +1,27 @@ +package frc4388.robot.subsystems; + +import frc4388.robot.subsystems.Apriltags.Tag; + +public class Location { + 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 + private void reoderPrio(){ + isLimelight = false; //If limelight gets position and if within a certain range of poles + isApriltag = apriltag.isAprilTag(); + } + + public Tag getPosRot() { + reoderPrio(); + if(isApriltag){ + return apriltag.getTagPosRot(); + } else if (isLimelight) { + return null; + } + + return null; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 0b13b6c..ac27c3b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,19 +4,16 @@ package frc4388.robot.subsystems; - import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; 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 edu.wpi.first.math.kinematics.SwerveModulePosition; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.RobotGyro; @@ -43,6 +40,7 @@ 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(); @@ -52,6 +50,7 @@ public class SwerveDrive extends SubsystemBase { this.rightFront = rightFront; this.leftBack = leftBack; this.rightBack = rightBack; + this.gyro = gyro; // this.odometry = new SwerveDriveOdometry( @@ -81,7 +80,6 @@ public class SwerveDrive extends SubsystemBase { 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) { @@ -112,7 +110,7 @@ public class SwerveDrive extends SubsystemBase { for (int i = 0; i < desiredStates.length; i++) { SwerveModule module = modules[i]; SwerveModuleState state = desiredStates[i]; - module.setDesiredState(state, false); + module.setDesiredState(state); } } @@ -230,7 +228,6 @@ public class SwerveDrive extends SubsystemBase { // SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); // SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees()); - } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index a88fca7..1ddab51 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -81,7 +81,7 @@ 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(); } @@ -126,7 +126,7 @@ 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); @@ -140,18 +140,11 @@ 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); - // double inchesPerSecond = Units.metersToFeet(state.speedMetersPerSecond) * 12; - // 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) { @@ -165,4 +158,4 @@ public class SwerveModule extends SubsystemBase { public double getVoltage() { return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); } -} \ No newline at end of file +} 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..3731849 --- /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; + } +}