From a91d334e2c92238afd3519514d2e403e3589416a Mon Sep 17 00:00:00 2001 From: Astatin3 Date: Thu, 16 Feb 2023 18:46:43 -0700 Subject: [PATCH] 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) {