diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java deleted file mode 100644 index 7695487..0000000 --- a/src/main/java/frc/robot/Constants.java +++ /dev/null @@ -1,69 +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 frc.robot; - -import java.util.HashMap; -import java.util.Hashtable; - -import com.revrobotics.CANSparkBase.IdleMode; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.util.Units; - -/** Add your docs here. */ -public final class Constants { - public static final int leftAzimuthID = 2; - public static final int rightAzimuthID = 3; - - public static final int leftDriveID = 4; - public static final int rightDriveID = 5; - - public static final int leftAbsoluteEncoderID = 3; - public static final int rightAbsoluteEncoderID = 0; - - public static final int pigeonID = 6; - - public static final double leftAbsoluteEncoderOffset = -1.304;//absolute encoder reading at position - public static final double rightAbsoluteEncoderOffset = -0.865;// gears facing inwards: fwd/bck TODO: less janky alignment - - public static final boolean leftAzimuthInvert = false; - public static final boolean rightAzimuthInvert = false; - public static final boolean leftDriveInvert = false; - public static final boolean rightDriveInvert = true; - - public static final double azimuthRadiansPerRotation = 2*Math.PI*15/33; - public static final double driveMetersPerRotation = Units.inchesToMeters(2) * Math.PI * 33 / 45 / 2; - public static final double absoluteEncoderRadiansPerRotation = 2*Math.PI; - - public static final int azimuthAmpLimit = 35; - public static final int driveAmpLimit = 25; - - public static final double podMaxSpeed = 1; - public static final double azimuthMaxOutput = 0.25; - public static double driveSpeedToPower = 0.25; - - public static final double azimuthkP = 0.4; - public static final double azimuthkI = 0.0; - public static final double azimuthkD = 0.003; - public static final double azimuthkIz = 0; - public static final double azimuthDriveSpeedMultiplier = 0.5; - - public static final IdleMode azimuthMode = IdleMode.kBrake; - public static final IdleMode driveMode = IdleMode.kCoast; - - public static final double gyroP = 2; - public static final double gyroI = 0.0; - public static final double gyroD = 0.00; - - public static final Translation2d robotCenter = new Translation2d(0, 0); // serves as "center of robot for calculations; robot will turn about this point - public static final Translation2d leftPodPosition = new Translation2d(-Units.inchesToMeters(5.5), -Units.inchesToMeters(5.5)); // units in meters - public static final Translation2d rightPodPosition = new Translation2d(Units.inchesToMeters(5.5), Units.inchesToMeters(5.5)); - public static final SwerveDriveKinematics drivetrainKinematics = new SwerveDriveKinematics(robotCenter.minus(leftPodPosition), robotCenter.minus(rightPodPosition)); - - public static final int driverPort = 0; - -} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 63bd2bd..66d39d9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,11 @@ package frc.robot; +import edu.wpi.first.util.datalog.BooleanLogEntry; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.util.datalog.StringLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -13,14 +18,25 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; + DoubleLogEntry rightAmps; + DoubleLogEntry leftAmps; + @Override public void robotInit() { m_robotContainer = new RobotContainer(); + + DataLogManager.start(); + DataLog log = DataLogManager.getLog(); + rightAmps = new DoubleLogEntry(log, "left drive amps"); + leftAmps = new DoubleLogEntry(log, "right drive amps"); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + leftAmps.append(RobotContainer.swerve.getDriveAmps()[0]); + rightAmps.append(RobotContainer.swerve.getDriveAmps()[1]); + } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index db22d17..d4b9633 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,31 +7,40 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.commands.ResetZeroes; +import frc.robot.commands.RevertZeroes; import frc.robot.commands.TurdDrive; -import frc.robot.subsystems.TurdPod; +import frc.robot.constants.Constants; import frc.robot.subsystems.TurdSwerve; public class RobotContainer { - public static final XboxController driver = new XboxController(Constants.driverPort); + public static final XboxController driverRaw = new XboxController(Constants.driverPort); + public static final CommandXboxController driverCommand = new CommandXboxController(Constants.driverPort); // public static final TurdPod leftPod = new TurdPod(Constants.leftAzimuthID, Constants.leftDriveID, Constants.leftAbsoluteEncoderID, Constants.leftAzimuthInvert,Constants.rightAzimuthInvert, Constants.leftAbsoluteEncoderOffset); public static final TurdSwerve swerve = new TurdSwerve(); + public RobotContainer() { + final var Odometry = Shuffleboard.getTab("Odometry"); configureBindings(); - Supplier driverRightJoystick = () -> new Translation2d(driver.getRightX(), driver.getRightY()); - Supplier driverLeftJoystick = () -> new Translation2d(driver.getLeftX(), driver.getLeftY()); - Supplier buttonStart = () -> driver.getStartButton(); - swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, buttonStart)); + Supplier driverRightJoystick = () -> new Translation2d(driverRaw.getRightX(), driverRaw.getRightY()); + Supplier driverLeftJoystick = () -> new Translation2d(driverRaw.getLeftX(), driverRaw.getLeftY()); + Supplier DPAD = () -> driverRaw.getPOV(); + swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, DPAD, driverRaw::getLeftBumper)); + swerve.addDashboardWidgets(Odometry); } private void configureBindings() { - // new JoystickButton(driver, 4).onTrue(swerve.resetPods()); + driverCommand.rightBumper().and(driverRaw::getYButton).onTrue(new ResetZeroes(swerve)); + driverCommand.rightBumper().and(driverRaw::getXButton).whileTrue(new RevertZeroes(swerve)); + driverCommand.start().whileTrue(new InstantCommand(swerve::resetPods, swerve)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/ResetZeroes.java b/src/main/java/frc/robot/commands/ResetZeroes.java new file mode 100644 index 0000000..d585a81 --- /dev/null +++ b/src/main/java/frc/robot/commands/ResetZeroes.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.TurdSwerve; + +public class ResetZeroes extends Command { + private TurdSwerve swerve; + public ResetZeroes(TurdSwerve swerve) { + this.swerve = swerve; + addRequirements(swerve); + // Use addRequirements() here to declare subsystem dependencies. + } + + // 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() { + swerve.resetZero(); + swerve.stop(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/RevertZeroes.java b/src/main/java/frc/robot/commands/RevertZeroes.java new file mode 100644 index 0000000..b4b724e --- /dev/null +++ b/src/main/java/frc/robot/commands/RevertZeroes.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.TurdSwerve; + +public class RevertZeroes extends Command { + private TurdSwerve swerve; + public RevertZeroes(TurdSwerve swerve) { + this.swerve = swerve; + addRequirements(swerve); + // Use addRequirements() here to declare subsystem dependencies. + } + + // 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() { + swerve.revertZero(); + swerve.stop(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/TurdDrive.java b/src/main/java/frc/robot/commands/TurdDrive.java index 5a9d244..2f8c42a 100644 --- a/src/main/java/frc/robot/commands/TurdDrive.java +++ b/src/main/java/frc/robot/commands/TurdDrive.java @@ -4,50 +4,65 @@ package frc.robot.commands; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; 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.SwerveModuleState; -import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.TurdPod; +import frc.robot.constants.Constants; +import frc.robot.constants.RobotMap; import frc.robot.subsystems.TurdSwerve; public class TurdDrive extends Command { TurdSwerve swerve; - Supplier joystickRight; - Supplier joystickLeft; - Supplier resetPods; + Supplier joystickRight, joystickLeft; + Supplier DPAD; + Supplier boost; Rotation2d rotation = new Rotation2d(); + double maxSpeed = Constants.robotMaxSpeed; - public TurdDrive(TurdSwerve swerve, Supplier joystickLeft, Supplier joystickRight, Supplier resetPods) { + public TurdDrive(TurdSwerve swerve, Supplier joystickLeft, Supplier joystickRight, Supplier DPAD, Supplier boost) { this.swerve = swerve; this.joystickRight = joystickRight; this.joystickLeft = joystickLeft; - this.resetPods = resetPods; + this.DPAD = DPAD; + this.boost = boost; addRequirements(swerve); } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + swerve.resetGyro(); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (resetPods.get()) { - swerve.resetPods(); + + if (DPAD.get() != -1) { + swerve.targetAngle = -Units.degreesToRadians(DPAD.get()); + } + + if (boost.get()) { + swerve.setAmpLimit(Constants.driveTopAmpLimit); + maxSpeed = 1; + } else { + swerve.setAmpLimit(Constants.driveAmpLimit); + maxSpeed = Constants.robotMaxSpeed; } - boolean deadband = Math.abs(joystickRight.get().getX()) + Math.abs(joystickRight.get().getY()) < 0.1; - double speedX = deadband ? 0 : -joystickRight.get().getX(); - double speedY = deadband ? 0 : joystickRight.get().getY(); - double speedOmega = -joystickLeft.get().getX() * Math.abs(joystickLeft.get().getX()); + boolean deadband = Math.abs(joystickRight.get().getX()) + Math.abs(joystickRight.get().getY()) < 0.05; + double speedX = deadband ? 0 : -joystickRight.get().getX() * maxSpeed; + double speedY = deadband ? 0 : joystickRight.get().getY() * maxSpeed; + // double speedX = deadband ? 0 : 3.0 * Math.abs(joystickRight.get().getX()) * -joystickRight.get().getX(); + // double speedY = deadband ? 0 : 3.0 * Math.abs(joystickRight.get().getY()) * joystickRight.get().getY(); + double speedOmega = Math.abs(joystickLeft.get().getX()) > 0.07 ? -joystickLeft.get().getX() * Math.abs(joystickLeft.get().getX())*0.3 : 0; ChassisSpeeds speeds = new ChassisSpeeds(speedX, speedY, speedOmega); swerve.setRobotSpeeds(speeds); + } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java new file mode 100644 index 0000000..23e542a --- /dev/null +++ b/src/main/java/frc/robot/constants/Constants.java @@ -0,0 +1,45 @@ +// 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 frc.robot.constants; + +import com.revrobotics.CANSparkBase.IdleMode; + + +/** Add your docs here. */ +public final class Constants { + public static final int driverPort = 0; + + public static final double robotMaxSpeed = 0.25; + + public static final double gyroP = 2; + public static final double gyroI = 0.0; + public static final double gyroD = 0.00; + + + // Azimuth Settings + public static final IdleMode azimuthMode = IdleMode.kBrake; + + public static final int azimuthAmpLimit = 55; + public static final double azimuthMaxOutput = 0.25; + + public static final double azimuthkP = 0.4; + public static final double azimuthkI = 0.0; + public static final double azimuthkD = 0.003; + public static final double azimuthkIz = 0; + + public static final double azimuthDriveSpeedMultiplier = 0;//0.5; + + + // Drive Settings + public static final IdleMode driveMode = IdleMode.kCoast; + + public static final int driveAmpLimit = 25; + public static final int driveTopAmpLimit = 50; + public static final double driveSpeedToPower = 1.0; + public static final double driveMotorRampRate = 0.2; + + public static final double podMaxSpeed = 1; + +} diff --git a/src/main/java/frc/robot/constants/RobotMap.java b/src/main/java/frc/robot/constants/RobotMap.java new file mode 100644 index 0000000..81fb6a1 --- /dev/null +++ b/src/main/java/frc/robot/constants/RobotMap.java @@ -0,0 +1,44 @@ +// 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 frc.robot.constants; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.util.Units; + +/** CAN ID, Invert, Pod Positions, Offsets, Conversion Rates */ +public final class RobotMap { + public static final int leftAzimuthID = 18;//BL + public static final int rightAzimuthID = 14;//FR + + public static final int leftDriveID = 17;//BL + public static final int rightDriveID = 13;//FR + + public static final int leftAbsoluteEncoderID = 3; + public static final int rightAbsoluteEncoderID = 0; + + public static final int pigeonID = 25; + + public static final boolean leftAzimuthInvert = false; + public static final boolean rightAzimuthInvert = false; + public static final boolean leftDriveInvert = false; + public static final boolean rightDriveInvert = true; + + public static final Translation2d robotCenter = new Translation2d(0, 0); // serves as "center of robot for calculations; robot will turn about this point + public static final Translation2d leftPodPosition = new Translation2d(-Units.inchesToMeters(5.5), -Units.inchesToMeters(5.5)); // units in meters + public static final Translation2d rightPodPosition = new Translation2d(Units.inchesToMeters(5.5), Units.inchesToMeters(5.5)); + public static final SwerveDriveKinematics drivetrainKinematics = new SwerveDriveKinematics(robotCenter.minus(leftPodPosition), robotCenter.minus(rightPodPosition)); + + public static final double leftAbsoluteEncoderOffset = 1.006118384272561;//absolute encoder reading at position + public static final double rightAbsoluteEncoderOffset = 1.056659935134538;// gears facing inwards: fwd/bck TODO: less janky alignment + + public static final double azimuthRadiansPerMotorRotation = 2*Math.PI*15/33; + public static final double driveMetersPerMotorRotation = Units.inchesToMeters(2) * Math.PI * 33 / 45 / 15/13; + public static final double absoluteRadiansPerEncoderRotation = 2*Math.PI; + + + + +} diff --git a/src/main/java/frc/robot/subsystems/TurdPod.java b/src/main/java/frc/robot/subsystems/TurdPod.java index 31f1195..778f29b 100644 --- a/src/main/java/frc/robot/subsystems/TurdPod.java +++ b/src/main/java/frc/robot/subsystems/TurdPod.java @@ -4,27 +4,20 @@ package frc.robot.subsystems; -import com.revrobotics.AnalogInput; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkMaxPIDController; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.AnalogEncoder; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.PWM; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.constants.Constants; +import frc.robot.constants.RobotMap; public class TurdPod extends SubsystemBase { @@ -34,11 +27,12 @@ public class TurdPod extends SubsystemBase { private final RelativeEncoder azimuthEncoder; private final RelativeEncoder driveEncoder; - private final SparkMaxPIDController azimuthPID; + private final SparkPIDController azimuthPID; private double azimuthDriveSpeedMultiplier; private double speed = 0; private double absoluteEncoderOffset; + private double driveSpeedToPower = Constants.driveSpeedToPower; public TurdPod(int azimuthID, int driveID, int absoluteEncoderID, boolean azimuthInvert, boolean driveInvert, double absoluteEncoderOffset) { @@ -52,9 +46,9 @@ public TurdPod(int azimuthID, int driveID, int absoluteEncoderID, boolean azimut azimuth.setInverted(azimuthInvert); drive.setInverted(driveInvert); - driveEncoder.setPositionConversionFactor(Constants.driveMetersPerRotation); - azimuthEncoder.setPositionConversionFactor(Constants.azimuthRadiansPerRotation); - absoluteEncoder.setDistancePerRotation(Constants.absoluteEncoderRadiansPerRotation); + driveEncoder.setPositionConversionFactor(RobotMap.driveMetersPerMotorRotation); + azimuthEncoder.setPositionConversionFactor(RobotMap.azimuthRadiansPerMotorRotation); + absoluteEncoder.setDistancePerRotation(RobotMap.absoluteRadiansPerEncoderRotation); // absoluteEncoder.setPositionOffset(absoluteEncoderOffset); this.absoluteEncoderOffset = absoluteEncoderOffset; @@ -65,15 +59,47 @@ public TurdPod(int azimuthID, int driveID, int absoluteEncoderID, boolean azimut azimuth.setIdleMode(Constants.azimuthMode); drive.setIdleMode(Constants.driveMode); + drive.setOpenLoopRampRate(Constants.driveMotorRampRate); + azimuthPID = azimuth.getPIDController(); resetPod(); } + + public void setAmpLimit(int ampLimit) { + drive.setSmartCurrentLimit(ampLimit); + } + + // public void setDriveSpeedtoPower(double driveSpeedToPower) { + // this.driveSpeedToPower = driveSpeedToPower; + // } public void resetPod() { driveEncoder.setPosition(0); azimuthEncoder.setPosition(getAbsoluteEncoder()); } + + public String getPod() { + return azimuth.getDeviceId() == RobotMap.leftAzimuthID ? "Left" : "Right"; + } + + public void resetZero() { + absoluteEncoderOffset = (absoluteEncoder.getAbsolutePosition() * 2*Math.PI); + SmartDashboard.putNumber((getPod() + " Encoder Offset"), absoluteEncoderOffset); + resetPod(); + } + + public void revertZero() { + absoluteEncoderOffset = azimuth.getDeviceId() == RobotMap.leftAzimuthID ? RobotMap.leftAbsoluteEncoderOffset : RobotMap.rightAbsoluteEncoderOffset; + SmartDashboard.putNumber((getPod() + " Encoder Offset"), absoluteEncoderOffset); + resetPod(); + } + + public void stop() { + azimuth.set(0); + drive.set(0); + } + public void setPID(double P, double I, double D, double IZone, double outputRange, double ADMult) { if (P != azimuthPID.getP()) {azimuthPID.setP(P);} if (I != azimuthPID.getI()) {azimuthPID.setI(I);} @@ -83,8 +109,8 @@ public void setPID(double P, double I, double D, double IZone, double outputRang azimuthPID.setPositionPIDWrappingMaxInput(Math.PI); azimuthPID.setPositionPIDWrappingMinInput(-Math.PI); azimuthPID.setPositionPIDWrappingEnabled(true); - azimuthPID.setSmartMotionAllowedClosedLoopError(0, 0); - azimuth.setClosedLoopRampRate(.1); + // azimuthPID.setSmartMotionAllowedClosedLoopError(0, 0); + azimuth.setClosedLoopRampRate(0.35); azimuthDriveSpeedMultiplier = ADMult; } @@ -94,8 +120,8 @@ public SwerveModulePosition getPodPosition() { public void setPodState(SwerveModuleState state) { state = SwerveModuleState.optimize(state, new Rotation2d(azimuthEncoder.getPosition())); // does not account for rotations between 180 and 360? - azimuthPID.setReference(state.angle.getRadians(), CANSparkMax.ControlType.kPosition); - speed = Math.abs(state.speedMetersPerSecond) < .01 ? 0 : state.speedMetersPerSecond * Constants.driveSpeedToPower; + azimuthPID.setReference(state.angle.getRadians(), ControlType.kPosition); + speed = Math.abs(state.speedMetersPerSecond) < .01 ? 0 : state.speedMetersPerSecond * driveSpeedToPower; SmartDashboard.putNumber("state.angle.getRadians()", state.angle.getRadians()); double error = (state.angle.getRadians() - azimuthEncoder.getPosition()) % (2*Math.PI); @@ -109,12 +135,18 @@ public double getAbsoluteEncoder() { return (absoluteEncoder.getAbsolutePosition() * 2*Math.PI) - absoluteEncoderOffset; } + public double getDriveAmp() { + return drive.getOutputCurrent(); + } + @Override public void periodic() { SmartDashboard.putNumber("getabsoluteEncoder() " + absoluteEncoder.getChannel(), getAbsoluteEncoder()); drive.set(speed + (azimuth.getAppliedOutput() * azimuthDriveSpeedMultiplier)); SmartDashboard.putNumber("azimuthEncoder.getPosition() " + azimuth.getDeviceId(), azimuthEncoder.getPosition()); SmartDashboard.putNumber("drive pos " + drive.getDeviceId(), driveEncoder.getPosition()); - SmartDashboard.putNumber("azimuth.getAppliedOutput()" + azimuth.getDeviceId(), azimuth.getAppliedOutput()); //getAppliedOutput()); + SmartDashboard.putNumber("azimuth.getoutputcurrent()" + azimuth.getDeviceId(), azimuth.getOutputCurrent()); + SmartDashboard.putNumber("drive.getoutputcurrent()" + drive.getDeviceId(), drive.getOutputCurrent()); } + } diff --git a/src/main/java/frc/robot/subsystems/TurdSwerve.java b/src/main/java/frc/robot/subsystems/TurdSwerve.java index b190607..d9f0da0 100644 --- a/src/main/java/frc/robot/subsystems/TurdSwerve.java +++ b/src/main/java/frc/robot/subsystems/TurdSwerve.java @@ -6,25 +6,34 @@ import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.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.networktables.GenericEntry; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.constants.Constants; +import frc.robot.constants.RobotMap; public class TurdSwerve extends SubsystemBase { - private final Pigeon2 gyro = new Pigeon2(Constants.pigeonID); - private final TurdPod leftPod = new TurdPod(Constants.leftAzimuthID, Constants.leftDriveID, Constants.leftAbsoluteEncoderID, Constants.leftAzimuthInvert, Constants.leftDriveInvert, Constants.leftAbsoluteEncoderOffset); - private final TurdPod rightPod = new TurdPod(Constants.rightAzimuthID, Constants.rightDriveID, Constants.rightAbsoluteEncoderID, Constants.rightAzimuthInvert, Constants.rightDriveInvert, Constants.rightAbsoluteEncoderOffset); - private final SwerveDriveOdometry odometer = new SwerveDriveOdometry(Constants.drivetrainKinematics, + private final SlewRateLimiter xLimiter = new SlewRateLimiter(0.75); + private final SlewRateLimiter yLimiter = new SlewRateLimiter(0.75); + private final Pigeon2 gyro = new Pigeon2(RobotMap.pigeonID); + private final TurdPod leftPod = new TurdPod(RobotMap.leftAzimuthID, RobotMap.leftDriveID, RobotMap.leftAbsoluteEncoderID, RobotMap.leftAzimuthInvert, RobotMap.leftDriveInvert, RobotMap.leftAbsoluteEncoderOffset); + private final TurdPod rightPod = new TurdPod(RobotMap.rightAzimuthID, RobotMap.rightDriveID, RobotMap.rightAbsoluteEncoderID, RobotMap.rightAzimuthInvert, RobotMap.rightDriveInvert, RobotMap.rightAbsoluteEncoderOffset); + private final SwerveDriveOdometry odometer = new SwerveDriveOdometry(RobotMap.drivetrainKinematics, new Rotation2d(0), new SwerveModulePosition[] { leftPod.getPodPosition(), rightPod.getPodPosition() @@ -38,15 +47,32 @@ public class TurdSwerve extends SubsystemBase { private GenericEntry ADMult = tab.add("azimuth-drive speed multiplier", Constants.azimuthDriveSpeedMultiplier).getEntry(); private PIDController GyroPID = new PIDController(Constants.gyroP, Constants.gyroI, Constants.gyroD); - private double targetAngle = 0; + public double targetAngle = 0; + private double odoAngleOffset = Math.PI * 0.0; private Rotation2d gyroResetAngle = new Rotation2d(); + + + private final Field2d field2d = new Field2d(); + public TurdSwerve() { + GyroPID.enableContinuousInput(0.0, 2*Math.PI); // gyro.configAllSettings(new Pigeon2Configuration()); } + public void setAmpLimit(int ampLimit) { + leftPod.setAmpLimit(ampLimit); + rightPod.setAmpLimit(ampLimit); + } + + // public void setDriveSpeedtoPower(double driveSpeedToPower) { + // leftPod.setDriveSpeedtoPower(driveSpeedToPower); + // rightPod.setDriveSpeedtoPower(driveSpeedToPower); + // } + public void resetOdometry(Pose2d pose) { - odometer.resetPosition(getGyro(), new SwerveModulePosition[] {leftPod.getPodPosition(), rightPod.getPodPosition()}, pose); + odoAngleOffset = DriverStation.getAlliance().get() == Alliance.Red ? Math.PI * 0.5 : Math.PI * 1.5; + odometer.resetPosition(new Rotation2d(odoAngleOffset), new SwerveModulePosition[] {leftPod.getPodPosition(), rightPod.getPodPosition()}, pose); } public void resetPods() { @@ -55,6 +81,22 @@ public void resetPods() { rightPod.resetPod(); leftPod.setPID(azimuthP.getDouble(Constants.azimuthkP), azimuthI.getDouble(Constants.azimuthkI), azimuthD.getDouble(Constants.azimuthkD), azimuthIzone.getDouble(Constants.azimuthkIz), Constants.azimuthMaxOutput, ADMult.getDouble(Constants.azimuthDriveSpeedMultiplier)); rightPod.setPID(azimuthP.getDouble(Constants.azimuthkP), azimuthI.getDouble(Constants.azimuthkI), azimuthD.getDouble(Constants.azimuthkD), azimuthIzone.getDouble(Constants.azimuthkIz), Constants.azimuthMaxOutput, ADMult.getDouble(Constants.azimuthDriveSpeedMultiplier)); + resetOdometry(new Pose2d(new Translation2d(8.0, 4.2), new Rotation2d())); + } + + public void resetZero() { + leftPod.resetZero(); + rightPod.resetZero(); + } + + public void revertZero() { + leftPod.revertZero(); + rightPod.revertZero(); + } + + public void stop() { + leftPod.stop(); + rightPod.stop(); } public Rotation2d getGyro() { @@ -66,15 +108,19 @@ public void resetGyro() { targetAngle = 0; } - public void setLeftPod(SwerveModuleState state) { - leftPod.setPodState(state); - } + // public void setLeftPod(SwerveModuleState state) { + // leftPod.setPodState(state); + // } public void setRobotSpeeds(ChassisSpeeds chassisSpeeds) { - targetAngle += chassisSpeeds.omegaRadiansPerSecond / 10; - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond * 3 /*GyroPID.calculate(getGyro().getRadians(), targetAngle)*/, getGyro()); - SwerveModuleState[] states = Constants.drivetrainKinematics.toSwerveModuleStates(chassisSpeeds); + boolean manualTurn = true;//Math.abs(chassisSpeeds.omegaRadiansPerSecond) > 0.1; + // chassisSpeeds = chassisSpeeds.times(robotMaxSpeed); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xLimiter.calculate(chassisSpeeds.vxMetersPerSecond), yLimiter.calculate(chassisSpeeds.vyMetersPerSecond), manualTurn ? chassisSpeeds.omegaRadiansPerSecond * 3.0 : GyroPID.calculate(getGyro().getRadians(), targetAngle), getGyro()); + SwerveModuleState[] states = RobotMap.drivetrainKinematics.toSwerveModuleStates(chassisSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(states, Constants.podMaxSpeed); + if (manualTurn) { + targetAngle = getGyro().getRadians() + (chassisSpeeds.omegaRadiansPerSecond / 2.0); + } leftPod.setPodState(states[0]); rightPod.setPodState(states[1]); @@ -84,5 +130,22 @@ public void setRobotSpeeds(ChassisSpeeds chassisSpeeds) { public void periodic() { odometer.update(getGyro(), new SwerveModulePosition[] {leftPod.getPodPosition(), rightPod.getPodPosition()}); SmartDashboard.putNumber("pigeon", getGyro().getDegrees()); + field2d.setRobotPose(odometer.getPoseMeters().transformBy(new Transform2d(new Translation2d(), new Rotation2d(odoAngleOffset + Math.PI)))); + } + + private String getFomattedPose() { + var pose = odometer.getPoseMeters(); + return String.format( + "(%.3f, %.3f) %.2f degrees", + pose.getX(), pose.getY(), pose.getRotation().plus(new Rotation2d(odoAngleOffset)).getDegrees()); + } + + public void addDashboardWidgets(ShuffleboardTab tab) { + tab.add("Field", field2d).withPosition(0, 0).withSize(6, 4); + tab.addString("Pose", this::getFomattedPose).withPosition(6, 2).withSize(2, 1); + } + + public double[] getDriveAmps() { + return new double[] {leftPod.getDriveAmp(), rightPod.getDriveAmp()}; } }