From 8ca97daa73a8fae4fa70d1f559a8d879631f3aa5 Mon Sep 17 00:00:00 2001 From: Ian Ruiz Date: Thu, 14 Sep 2023 19:02:06 -0400 Subject: [PATCH 01/20] Just adding final draft --- src/main/java/frc/robot/Constants.java | 158 ++++++++++++++++-- src/main/java/frc/robot/RobotContainer.java | 72 +++----- .../frc/robot/subsystems/SwerveModule.java | 121 +++++++------- .../frc/robot/subsystems/SwerveSubsystem.java | 94 +++++++++-- 4 files changed, 307 insertions(+), 138 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c50ba05..0b428bc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,19 +1,153 @@ -// 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; -/** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared - * globally (i.e. public static). Do not put anything functional in this class. - * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. - */ +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; + +public final class Constants { + public static final class ModuleConstants { + public static final double kWheelDiameterMeters = Units.inchesToMeters(4); + public static final double kDriveMotorGearRatio = 1/6.75; + public static final double kTurningMotorGearRatio = 1/21.0; + public static final double kDriveEncoderRot2Meter = kDriveMotorGearRatio * Math.PI * kWheelDiameterMeters; + public static final double kTurningEncoderRot2Rad = kTurningMotorGearRatio * 2 * Math.PI; + public static final double kDriveEncoderRPM2MeterPerSec = kDriveEncoderRot2Meter / 60; + public static final double kTurningEncoderRPM2RadPerSec = kTurningEncoderRot2Rad / 60; + + + // We may need to tune this for the PID turning + public static final double kPTurning = 0.5; + } + + public static final class DriveConstants { + + // we need to update this + public static final double kTrackWidth = 0.5969; + public static final double kWheelBase = 0.5969; + + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), //front left + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2), //front right + new Translation2d(kWheelBase / 2, kTrackWidth / 2), //back left + new Translation2d(kWheelBase / 2, -kTrackWidth / 2)); //backright + + //We need to update these motors + // frontLeft Module + public static final int kFrontLeftDriveMotorPort = 4; + public static final int kFrontLeftTurningMotorPort = 3; + public static final boolean kFrontLeftDriveMotorReversed = false; + public static final boolean kFrontLeftTurningMotorReversed = true; + public static final int kFrontLeftDriveAbsoluteEncoderPort = 3; + public static final boolean kFrontLeftDriveAbsoluteEncoderReversed = true; + public static final double kFrontLeftDriveAbsoluteEncoderOffsetRad = -106.5; + + // frontRight Module + public static final int kFrontRightDriveMotorPort = 2; + public static final int kFrontRightTurningMotorPort = 1; + public static final boolean kFrontRightDriveMotorReversed = false; + public static final boolean kFrontRightTurningMotorReversed = true; + public static final int kFrontRightDriveAbsoluteEncoderPort = 1; + public static final boolean kFrontRightDriveAbsoluteEncoderReversed = true; + public static final double kFrontRightDriveAbsoluteEncoderOffsetRad = -128; + + // backLeft Module + public static final int kBackLeftDriveMotorPort = 8; + public static final int kBackLeftTurningMotorPort = 7; + public static final boolean kBackLeftDriveMotorReversed = true; + public static final boolean kBackLeftTurningMotorReversed = true; + public static final int kBackLeftDriveAbsoluteEncoderPort = 7; + public static final boolean kBackLeftDriveAbsoluteEncoderReversed = true; + public static final double kBackLeftDriveAbsoluteEncoderOffsetRad = -3; + + // backRight Module + public static final int kBackRightDriveMotorPort = 6; + public static final int kBackRightTurningMotorPort = 5; + public static final boolean kBackRightDriveMotorReversed = true; + public static final boolean kBackRightTurningMotorReversed = true; + public static final int kBackRightDriveAbsoluteEncoderPort = 5; + public static final boolean kBackRightDriveAbsoluteEncoderReversed = true; + public static final double kBackRightDriveAbsoluteEncoderOffsetRad = -64; + + + + public static final double kPhysicalMaxSpeedMetersPerSecond = 6380.0 / 60.0 * (ModuleConstants.kDriveMotorGearRatio) * ModuleConstants.kWheelDiameterMeters * Math.PI * 2; + public static final double kPhysicalMaxAngularSpeedRadiansPerSecond = kPhysicalMaxSpeedMetersPerSecond / Math.hypot(DriveConstants.kTrackWidth / 2.0, DriveConstants.kWheelBase / 2.0 * 3); + + public static final double kTeleDriveMaxSpeedMetersPerSecond = kPhysicalMaxSpeedMetersPerSecond / 4; + public static final double kTeleDriveMaxAngularSpeedRadiansPerSecond = kPhysicalMaxAngularSpeedRadiansPerSecond / 4; + + public static final double kTeleDriveMaxAccelerationUnitsPerSecond = 3; + public static final double kTeleDriveMaxAngularAccelerationUnitsPerSecond = 3; + } + + public static final class AutoConstants { + public static final double kMaxSpeedMetersPerSecond = DriveConstants.kPhysicalMaxSpeedMetersPerSecond / 4; + public static final double kMaxAngularSpeedRadiansPerSecond = DriveConstants.kPhysicalMaxAngularSpeedRadiansPerSecond / 10; + + public static final double kMaxAccelerationMetersPerSecondSquared = 3; + public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI / 4; + public static final double kPXController = 1.5; + public static final double kPYController = 1.5; + public static final double kPThetaController = 3; + + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = + new TrapezoidProfile.Constraints( + kMaxAngularSpeedRadiansPerSecond, + kMaxAngularAccelerationRadiansPerSecondSquared); + } + + public static final class OIConstants { + public static final double kDeadband = 0.05; + } +} + + /** public final class Constants { public static class OperatorConstants { public static final int kDriverControllerPort = 0; + + //these numbers may be incorrect } + + public static final int kDriverControllerPort = 1; + + public static final double kTrackWidth = Units.inchesToMeters(21); + // Distance between right and left wheels + public static final double kWheelBase = Units.inchesToMeters(25.5); + // Distance between front and back wheels + + + //please update these with the accurate dimensions for robot + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2)); + + + + public static final double kDriveEncoderRot2Meter = kDriveMotorGearRatio * Math.PI * kWheelDiameterMeters; + public static final double kTurningEncoderRot2Rad = kTurningMotorGearRatio * 2 * Math.PI; + public static final double kDriveEncoderRPM2MeterPerSec = kDriveEncoderRot2Meter / 60; + public static final double kTurningEncoderRPM2RadPerSec = kTurningEncoderRot2Rad / 60; + public static final double kPturning = 0.5; + public static final double kMaxSpeed = 5; + public static final double kTeleDriveMaxAccelerationUnitsPerSecond = 5; + public static final double kTeleDriveMaxAngularAccelerationUnitsPerSecond = 5; + + public static final double kPhysicalMaxSpeedMetersPerSecond = 5; + public static final double kPhysicalMaxAngularSpeedRadiansPerSecond = 2 * 2 * Math.PI; + + public static final int kDriverYAxis = 1; + public static final int kDriverXAxis = 0; + public static final int kDriverRotAxis = 4; + public static final int kDriverFieldOrientedButtonIdx = 1; + public static final double kDeadband = 0.05; + public static final double kTeleDriveMaxSpeedMetersPerSecond = kPhysicalMaxSpeedMetersPerSecond / 4; + public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI / 4; + public static final double kTeleDriveMaxAngularSpeedRadiansPerSecond = // + kPhysicalMaxAngularSpeedRadiansPerSecond / 4; } +*/ + \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a33249e..36cea3c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,63 +1,29 @@ -// 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 frc.robot.Constants.OperatorConstants; -import frc.robot.commands.Autos; -import frc.robot.commands.ExampleCommand; -import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.SwerveCommand; +import frc.robot.subsystems.SwerveSubsystem; -/** - * This class is where the bulk of the robot should be declared. Since Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} - * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including - * subsystems, commands, and trigger mappings) should be declared here. - */ public class RobotContainer { - // The robot's subsystems and commands are defined here... - private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); - // Replace with CommandPS4Controller or CommandJoystick if needed - private final CommandXboxController m_driverController = - new CommandXboxController(OperatorConstants.kDriverControllerPort); + private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem();; + private final XboxController driverOne = new XboxController(0); - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { - // Configure the trigger bindings - configureBindings(); - } + public RobotContainer() { + swerveSubsystem.setDefaultCommand(new SwerveCommand(swerveSubsystem, driverOne)); + configureButtonBindings(); + } - /** - * Use this method to define your trigger->command mappings. Triggers can be created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary - * predicate, or via the named factories in {@link - * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link - * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller - * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight - * joysticks}. - */ - private void configureBindings() { - // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - new Trigger(m_exampleSubsystem::exampleCondition) - .onTrue(new ExampleCommand(m_exampleSubsystem)); + private void configureButtonBindings() { + + } - // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // cancelling on release. - m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); - } + public SwerveSubsystem getSwerve() { + return swerveSubsystem; + } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // An example command will be run in autonomous - return Autos.exampleAuto(m_exampleSubsystem); - } -} + public Command getAutonomousCommand() { + return null; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 271cda1..36f7e40 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -1,101 +1,110 @@ package frc.robot.subsystems; -import com.revrobotics.CANSparkMax; + import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.robot.Constants; +import frc.robot.Constants.ModuleConstants; + +import com.ctre.phoenix.sensors.AbsoluteSensorRange; +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.CANCoderConfiguration; public class SwerveModule { - private final CANSparkMax driveMotor; - private final CANSparkMax turningMotor; + public final CANSparkMax driveMotor; + public final CANSparkMax turningMotor; private final RelativeEncoder driveEncoder; private final RelativeEncoder turningEncoder; - private final PIDController turningPIDController; + private final PIDController turningPidController; - private final AnalogInput absoluteEncoder; - private final boolean absoluteEncoderReversed; - private final double absoluteEncoderOffSetRad; + private final CANCoder absoluteEncoder; + private final double absoluteEncoderOffset; - public SwerveModule(int driveMotorID, int turningMotorID, boolean driveMotorReversed, boolean turningMotorReversed, - int absoluteEncoderID, double absoluteEncoderOffset, boolean absoluteEncoderReversed) { - - this.absoluteEncoderOffSetRad = absoluteEncoderOffset; - this.absoluteEncoderReversed = absoluteEncoderReversed; - absoluteEncoder = new AnalogInput(absoluteEncoderID); + private final String ModuleName; - driveMotor = new CANSparkMax(driveMotorID, MotorType.kBrushless); - turningMotor = new CANSparkMax(turningMotorID, MotorType.kBrushless); + public SwerveModule(String ModuleName, int driveMotorId, int turningMotorId, boolean driveMotorReversed, boolean turningMotorReversed, + int absoluteEncoderId, double absoluteEncoderOffset, boolean absoluteEncoderReversed) { - driveMotor.setInverted(driveMotorReversed); - turningMotor.setInverted(turningMotorReversed); + this.ModuleName = ModuleName; + absoluteEncoder = new CANCoder(absoluteEncoderId); + CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); + canCoderConfiguration.magnetOffsetDegrees = absoluteEncoderOffset; + canCoderConfiguration.sensorDirection = absoluteEncoderReversed; + canCoderConfiguration.absoluteSensorRange = AbsoluteSensorRange.Signed_PlusMinus180; + absoluteEncoder.configAllSettings(canCoderConfiguration); - driveEncoder = driveMotor.getEncoder(); - turningEncoder = turningMotor.getEncoder(); + this.absoluteEncoderOffset = absoluteEncoderOffset; - driveEncoder.setPositionConversionFactor(Constants.kDriveEncoderRot2Meter); - driveEncoder.setVelocityConversionFactor(Constants.kDriveEncoderRPM2MeterPerSec); - turningEncoder.setPositionConversionFactor(Constants.kTurningEncoderRot2Rad); - turningEncoder.setVelocityConversionFactor(Constants.kTurningEncoderRPM2RadPerSec); + driveMotor = new CANSparkMax(driveMotorId, MotorType.kBrushless); + configureMotor(driveMotor, driveMotorReversed); + turningMotor = new CANSparkMax(turningMotorId, MotorType.kBrushless); + configureMotor(turningMotor, turningMotorReversed); - turningPIDController = new PIDController(Constants.kPturning,0,0); - turningPIDController.enableContinuousInput(-Math.PI, Math.PI); + driveEncoder = driveMotor.getEncoder(); + turningEncoder = turningMotor.getEncoder(); - resetEncoders(); + driveEncoder.setPositionConversionFactor(ModuleConstants.kDriveEncoderRot2Meter); + driveEncoder.setVelocityConversionFactor(ModuleConstants.kDriveEncoderRPM2MeterPerSec); + turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderRot2Rad); + turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderRPM2RadPerSec); + + turningPidController = new PIDController(ModuleConstants.kPTurning, 0, 0); + turningPidController.enableContinuousInput(-180, 180); - } + resetEncoders(); + } - public double getDrivePosition() { - return 0.0; //driveEncoder.getPosition(); + private void configureMotor(CANSparkMax motor, Boolean inverted) { + motor.restoreFactoryDefaults(); + motor.setIdleMode(IdleMode.kBrake); + motor.setInverted(inverted); + motor.setSmartCurrentLimit(40); + motor.burnFlash(); } - public double getTurningPosition(){ - return turningEncoder.getPosition(); + + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(driveEncoder.getPosition(), Rotation2d.fromDegrees(getAbsoluteEncoderRad())); } - public double getDriveVelocity(){ + + public double getDriveVelocity() { return driveEncoder.getVelocity(); } - public double getTurningVelocity(){ - return turningEncoder.getVelocity(); - } - public double getAbsoluteEncoderRad(){ - double angle = absoluteEncoder.getVoltage() / RobotController.getCurrent5V(); - angle *= 2 * Math.PI; - angle -= absoluteEncoderOffSetRad; - return angle * (absoluteEncoderReversed ? -1.0 : 1.0); + + public double getAbsoluteEncoderRad() { + return absoluteEncoder.getAbsolutePosition(); } public void resetEncoders() { driveEncoder.setPosition(0); - turningEncoder.setPosition(getAbsoluteEncoderRad()); + turningEncoder.setPosition(0); } - + public SwerveModuleState getState() { - return new SwerveModuleState(getDriveVelocity(), new Rotation2d(getTurningPosition())); + return new SwerveModuleState(getDriveVelocity(), Rotation2d.fromDegrees(getAbsoluteEncoderRad())); } public void setDesiredState(SwerveModuleState state) { if (Math.abs(state.speedMetersPerSecond) < 0.001) { stop(); + return; } - state = SwerveModuleState.optimize(state, getState().angle); - driveMotor.set(state.speedMetersPerSecond / Constants.kMaxSpeed); - turningMotor.set(turningPIDController.calculate(getTurningPosition(), state.angle.getRadians())); - SmartDashboard.putString("Swerve[" + absoluteEncoder.getChannel() + "] state", state.toString()); + double driveSpeed = state.speedMetersPerSecond*0.2; + driveMotor.set(driveSpeed); + double turnSpeed = (turningPidController.calculate(getAbsoluteEncoderRad(), state.angle.getDegrees()))/20; + turningMotor.set(turnSpeed); + //System.out.println(ModuleName + "- DriveMotorCommand: " + driveSpeed + " - True Angle: " + getAbsoluteEncoderRad() + " AngleSetPoint: " + state.angle.getDegrees() + " AngleMotorCommand: " + turnSpeed); } - public void stop(){ + public void stop() { driveMotor.set(0); turningMotor.set(0); } - - -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index a622b11..18778f5 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -1,32 +1,80 @@ package frc.robot.subsystems; import com.kauailabs.navx.frc.AHRS; - +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; 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.wpilibj.SPI; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.Constants.DriveConstants; public class SwerveSubsystem extends SubsystemBase { - private final SwerveModule frontRight = null; - private final SwerveModule frontLeft = null; - private final SwerveModule backRight = null; - private final SwerveModule backLeft = null; + public static final double MAX_VOLTAGE = 12.0; + public final SwerveModule frontLeft; + public final SwerveModule frontRight; + public final SwerveModule backLeft; + public final SwerveModule backRight; + + private final AHRS gyro = new AHRS(SPI.Port.kMXP); - private AHRS gyro = new AHRS(SPI.Port.kMXP); + private final SwerveDriveOdometry odometer; public SwerveSubsystem() { + frontLeft = new SwerveModule( + "Front Left", + DriveConstants.kFrontLeftDriveMotorPort, + DriveConstants.kFrontLeftTurningMotorPort, + DriveConstants.kFrontLeftDriveMotorReversed, + DriveConstants.kFrontLeftTurningMotorReversed, + DriveConstants.kFrontLeftDriveAbsoluteEncoderPort, + DriveConstants.kFrontLeftDriveAbsoluteEncoderOffsetRad, + DriveConstants.kFrontLeftDriveAbsoluteEncoderReversed); + + frontRight = new SwerveModule( + "Front Right", + DriveConstants.kFrontRightDriveMotorPort, + DriveConstants.kFrontRightTurningMotorPort, + DriveConstants.kFrontRightDriveMotorReversed, + DriveConstants.kFrontRightTurningMotorReversed, + DriveConstants.kFrontRightDriveAbsoluteEncoderPort, + DriveConstants.kFrontRightDriveAbsoluteEncoderOffsetRad, + DriveConstants.kFrontRightDriveAbsoluteEncoderReversed); + + backLeft = new SwerveModule( + "Back Left", + DriveConstants.kBackLeftDriveMotorPort, + DriveConstants.kBackLeftTurningMotorPort, + DriveConstants.kBackLeftDriveMotorReversed, + DriveConstants.kBackLeftTurningMotorReversed, + DriveConstants.kBackLeftDriveAbsoluteEncoderPort, + DriveConstants.kBackLeftDriveAbsoluteEncoderOffsetRad, + DriveConstants.kBackLeftDriveAbsoluteEncoderReversed); + + backRight = new SwerveModule( + "Back Right", + DriveConstants.kBackRightDriveMotorPort, + DriveConstants.kBackRightTurningMotorPort, + DriveConstants.kBackRightDriveMotorReversed, + DriveConstants.kBackRightTurningMotorReversed, + DriveConstants.kBackRightDriveAbsoluteEncoderPort, + DriveConstants.kBackRightDriveAbsoluteEncoderOffsetRad, + DriveConstants.kBackRightDriveAbsoluteEncoderReversed); + + odometer = new SwerveDriveOdometry(DriveConstants.kDriveKinematics, getRotation2d(), new SwerveModulePosition[] { + frontLeft.getPosition(), frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()}); + new Thread(() -> { try { - zeroHeading(); Thread.sleep(1000); - } catch(Exception e) { + zeroHeading(); } + catch (Exception e) { + } }).start(); - } public void zeroHeading() { @@ -34,16 +82,29 @@ public void zeroHeading() { } public double getHeading() { - return Math.IEEEremainder(gyro.getAngle(),360); + return Math.IEEEremainder(gyro.getAngle(), 360); } - public Rotation2d getRotation2d(){ + public Rotation2d getRotation2d() { return Rotation2d.fromDegrees(getHeading()); } + public Pose2d getPose() { + return odometer.getPoseMeters(); + } + @Override - public void periodic(){ + public void periodic() { + odometer.update(getRotation2d(), new SwerveModulePosition[] { + frontLeft.getPosition(), frontRight.getPosition(), + backLeft.getPosition(), backRight.getPosition() + }); SmartDashboard.putNumber("Robot Heading", getHeading()); + SmartDashboard.putNumber("Front Left Wheel Angle", frontLeft.getAbsoluteEncoderRad()); + SmartDashboard.putNumber("Front Right Wheel Angle", frontRight.getAbsoluteEncoderRad()); + SmartDashboard.putNumber("Back Left Wheel Angle", backLeft.getAbsoluteEncoderRad()); + SmartDashboard.putNumber("Back Right Wheel Angle", backRight.getAbsoluteEncoderRad()); + SmartDashboard.putString("Robot Location", getPose().getTranslation().toString()); } public void stopModules() { @@ -53,12 +114,11 @@ public void stopModules() { backRight.stop(); } - //Note: the paramater SwerveModuleState must be of length 4 public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.kMaxSpeed); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kPhysicalMaxSpeedMetersPerSecond); frontLeft.setDesiredState(desiredStates[0]); frontRight.setDesiredState(desiredStates[1]); backLeft.setDesiredState(desiredStates[2]); backRight.setDesiredState(desiredStates[3]); } -} +} \ No newline at end of file From a677228e88f8d8a6d51adacdffccf4ce7d0c0394 Mon Sep 17 00:00:00 2001 From: Ian Ruiz Date: Thu, 21 Sep 2023 16:25:26 -0400 Subject: [PATCH 02/20] added comments on what to change and other things --- src/main/java/frc/robot/Constants.java | 15 ++++- .../frc/robot/commands/SwerveCommand.java | 67 ++++++------------- 2 files changed, 34 insertions(+), 48 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0b428bc..855458d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,6 +7,7 @@ public final class Constants { public static final class ModuleConstants { + // these constants should be correct for the current 4ki module we are using public static final double kWheelDiameterMeters = Units.inchesToMeters(4); public static final double kDriveMotorGearRatio = 1/6.75; public static final double kTurningMotorGearRatio = 1/21.0; @@ -17,15 +18,18 @@ public static final class ModuleConstants { // We may need to tune this for the PID turning + // we may need to tune it so this is not set in stone public static final double kPTurning = 0.5; } public static final class DriveConstants { // we need to update this + //thse seem to be based off of the base dimensions public static final double kTrackWidth = 0.5969; public static final double kWheelBase = 0.5969; + //This should be relative to the center, but still check documentation about how the grid is set up for swerve kinetics public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( new Translation2d(-kWheelBase / 2, kTrackWidth / 2), //front left new Translation2d(-kWheelBase / 2, -kTrackWidth / 2), //front right @@ -42,6 +46,7 @@ public static final class DriveConstants { public static final boolean kFrontLeftDriveAbsoluteEncoderReversed = true; public static final double kFrontLeftDriveAbsoluteEncoderOffsetRad = -106.5; + //we need to updats these motors // frontRight Module public static final int kFrontRightDriveMotorPort = 2; public static final int kFrontRightTurningMotorPort = 1; @@ -51,6 +56,7 @@ public static final class DriveConstants { public static final boolean kFrontRightDriveAbsoluteEncoderReversed = true; public static final double kFrontRightDriveAbsoluteEncoderOffsetRad = -128; + //we need to update these motors // backLeft Module public static final int kBackLeftDriveMotorPort = 8; public static final int kBackLeftTurningMotorPort = 7; @@ -60,6 +66,7 @@ public static final class DriveConstants { public static final boolean kBackLeftDriveAbsoluteEncoderReversed = true; public static final double kBackLeftDriveAbsoluteEncoderOffsetRad = -3; + //we need to update these motors // backRight Module public static final int kBackRightDriveMotorPort = 6; public static final int kBackRightTurningMotorPort = 5; @@ -70,10 +77,14 @@ public static final class DriveConstants { public static final double kBackRightDriveAbsoluteEncoderOffsetRad = -64; - + //NOTE: these are not used in actual code they are just used to define max based on physical contraints + //If you want to ignore these then change the limit on the max speeds manually + //these seem to be mostly fine but we may need change some things + // also we need to change the physical dimensions of our base if we are going to use this public static final double kPhysicalMaxSpeedMetersPerSecond = 6380.0 / 60.0 * (ModuleConstants.kDriveMotorGearRatio) * ModuleConstants.kWheelDiameterMeters * Math.PI * 2; public static final double kPhysicalMaxAngularSpeedRadiansPerSecond = kPhysicalMaxSpeedMetersPerSecond / Math.hypot(DriveConstants.kTrackWidth / 2.0, DriveConstants.kWheelBase / 2.0 * 3); + //These are the variables that determine the max speeds of our swerve drive public static final double kTeleDriveMaxSpeedMetersPerSecond = kPhysicalMaxSpeedMetersPerSecond / 4; public static final double kTeleDriveMaxAngularSpeedRadiansPerSecond = kPhysicalMaxAngularSpeedRadiansPerSecond / 4; @@ -81,6 +92,7 @@ public static final class DriveConstants { public static final double kTeleDriveMaxAngularAccelerationUnitsPerSecond = 3; } + //these we can ignore for now since they are only for autonmous public static final class AutoConstants { public static final double kMaxSpeedMetersPerSecond = DriveConstants.kPhysicalMaxSpeedMetersPerSecond / 4; public static final double kMaxAngularSpeedRadiansPerSecond = DriveConstants.kPhysicalMaxAngularSpeedRadiansPerSecond / 10; @@ -97,6 +109,7 @@ public static final class AutoConstants { kMaxAngularAccelerationRadiansPerSecondSquared); } + //this is just to set a deadzone so we don't need to move the stick public static final class OIConstants { public static final double kDeadband = 0.05; } diff --git a/src/main/java/frc/robot/commands/SwerveCommand.java b/src/main/java/frc/robot/commands/SwerveCommand.java index 1ebcb95..9327af8 100644 --- a/src/main/java/frc/robot/commands/SwerveCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommand.java @@ -1,30 +1,24 @@ package frc.robot.commands; -import java.util.function.Supplier; +import com.ctre.phoenix.sensors.CANCoder; +import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.Constants; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.OIConstants; import frc.robot.subsystems.SwerveSubsystem; public class SwerveCommand extends CommandBase { private final SwerveSubsystem swerveSubsystem; - private final Supplier xSpdFunction, ySpdFunction, turningSpdFunction; - private final Supplier fieldOrientedFunction; - private final SlewRateLimiter xLimiter, yLimiter, turningLimiter; + private final XboxController controller; - public SwerveCommand(SwerveSubsystem swerveSubsystem, Supplier xSpdFunction, Supplier ySpdFunction, Supplier turningSpdFunction, Supplier fieldOrientedFunction) { + public SwerveCommand(SwerveSubsystem swerveSubsystem, XboxController controller) { this.swerveSubsystem = swerveSubsystem; - this.xSpdFunction = xSpdFunction; - this.ySpdFunction = ySpdFunction; - this.turningSpdFunction = turningSpdFunction; - this.fieldOrientedFunction = fieldOrientedFunction; - this.xLimiter = new SlewRateLimiter(Constants.kTeleDriveMaxAccelerationUnitsPerSecond); - this.yLimiter = new SlewRateLimiter(Constants.kTeleDriveMaxAccelerationUnitsPerSecond); - this.turningLimiter = new SlewRateLimiter(Constants.kTeleDriveMaxAngularAccelerationUnitsPerSecond); + this.controller = controller; addRequirements(swerveSubsystem); } @@ -34,38 +28,17 @@ public void initialize() { @Override public void execute() { - // 1. Get real-time joystick inputs - double xSpeed = xSpdFunction.get(); - double ySpeed = ySpdFunction.get(); - double turningSpeed = turningSpdFunction.get(); - - // 2. Apply deadband - xSpeed = Math.abs(xSpeed) > Constants.kDeadband ? xSpeed : 0.0; - ySpeed = Math.abs(ySpeed) > Constants.kDeadband ? ySpeed : 0.0; - turningSpeed = Math.abs(turningSpeed) > Constants.kDeadband ? turningSpeed : 0.0; - - // 3. Make the driving smoother - xSpeed = xLimiter.calculate(xSpeed) * Constants.kTeleDriveMaxSpeedMetersPerSecond; - ySpeed = yLimiter.calculate(ySpeed) * Constants.kTeleDriveMaxSpeedMetersPerSecond; - turningSpeed = turningLimiter.calculate(turningSpeed) - * Constants.kTeleDriveMaxAngularSpeedRadiansPerSecond; - - // 4. Construct desired chassis speeds - ChassisSpeeds chassisSpeeds; - if (fieldOrientedFunction.get()) { - // Relative to field - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, turningSpeed, swerveSubsystem.getRotation2d()); - } else { - // Relative to robot - chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, turningSpeed); - } - - // 5. Convert chassis speeds to individual module states - SwerveModuleState[] moduleStates = Constants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - - // 6. Output each module states to wheels - swerveSubsystem.setModuleStates(moduleStates); + double xSpeed = -controller.getLeftY() * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; + xSpeed = Math.abs(xSpeed) > OIConstants.kDeadband ? xSpeed : 0.0; + + double ySpeed = controller.getLeftX() * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; + ySpeed = Math.abs(ySpeed) > OIConstants.kDeadband ? ySpeed : 0.0; + + double turningSpeed = controller.getRightX() * DriveConstants.kTeleDriveMaxAngularSpeedRadiansPerSecond; + turningSpeed = Math.abs(turningSpeed) > OIConstants.kDeadband ? turningSpeed : 0.0; + + ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeed, Rotation2d.fromDegrees(swerveSubsystem.getHeading())); + swerveSubsystem.setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds)); } @Override From 73a43306b115701565a5cc3f3120dca33f77f50e Mon Sep 17 00:00:00 2001 From: JingerJesus Date: Thu, 28 Sep 2023 23:57:02 -0400 Subject: [PATCH 03/20] Got a large amount of constants updated. Need to lock the wheels and calibrate the encoders. Co-authored-by: Goldrn --- src/main/java/frc/robot/Constants.java | 61 +++++++++++++------------- 1 file changed, 31 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 855458d..715ff44 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,60 +24,61 @@ public static final class ModuleConstants { public static final class DriveConstants { - // we need to update this + // we need to update this // no longer needs to be updated: I measured from center of axle to center of axle //thse seem to be based off of the base dimensions - public static final double kTrackWidth = 0.5969; - public static final double kWheelBase = 0.5969; + public static final double kTrackWidth = Units.inchesToMeters(21); + public static final double kWheelBase = Units.inchesToMeters(21); //This should be relative to the center, but still check documentation about how the grid is set up for swerve kinetics + //Ive changed it. It looked wrong and I fixed it based on the coordinate system at https://hhs-team670.github.io/MustangLib/frc/team670/robot/utils/math/Translation2d.html public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( new Translation2d(-kWheelBase / 2, kTrackWidth / 2), //front left - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2), //front right - new Translation2d(kWheelBase / 2, kTrackWidth / 2), //back left + new Translation2d(kWheelBase / 2, kTrackWidth / 2), //front right + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2), //back left new Translation2d(kWheelBase / 2, -kTrackWidth / 2)); //backright //We need to update these motors // frontLeft Module - public static final int kFrontLeftDriveMotorPort = 4; - public static final int kFrontLeftTurningMotorPort = 3; - public static final boolean kFrontLeftDriveMotorReversed = false; - public static final boolean kFrontLeftTurningMotorReversed = true; - public static final int kFrontLeftDriveAbsoluteEncoderPort = 3; - public static final boolean kFrontLeftDriveAbsoluteEncoderReversed = true; + public static final int kFrontLeftDriveMotorPort = 57; //motors updated + public static final int kFrontLeftTurningMotorPort = 56; //motors updated + public static final boolean kFrontLeftDriveMotorReversed = false; //updated + public static final boolean kFrontLeftTurningMotorReversed = true; //updated + public static final int kFrontLeftDriveAbsoluteEncoderPort = 30; //updated + public static final boolean kFrontLeftDriveAbsoluteEncoderReversed = false; //updated public static final double kFrontLeftDriveAbsoluteEncoderOffsetRad = -106.5; //we need to updats these motors // frontRight Module - public static final int kFrontRightDriveMotorPort = 2; - public static final int kFrontRightTurningMotorPort = 1; - public static final boolean kFrontRightDriveMotorReversed = false; - public static final boolean kFrontRightTurningMotorReversed = true; - public static final int kFrontRightDriveAbsoluteEncoderPort = 1; - public static final boolean kFrontRightDriveAbsoluteEncoderReversed = true; + public static final int kFrontRightDriveMotorPort = 51; //motors updated + public static final int kFrontRightTurningMotorPort = 50; //motors updated + public static final boolean kFrontRightDriveMotorReversed = true; //updated + public static final boolean kFrontRightTurningMotorReversed = true; //updated + public static final int kFrontRightDriveAbsoluteEncoderPort = 31; //updated + public static final boolean kFrontRightDriveAbsoluteEncoderReversed = false; //updated public static final double kFrontRightDriveAbsoluteEncoderOffsetRad = -128; //we need to update these motors // backLeft Module - public static final int kBackLeftDriveMotorPort = 8; - public static final int kBackLeftTurningMotorPort = 7; - public static final boolean kBackLeftDriveMotorReversed = true; - public static final boolean kBackLeftTurningMotorReversed = true; - public static final int kBackLeftDriveAbsoluteEncoderPort = 7; - public static final boolean kBackLeftDriveAbsoluteEncoderReversed = true; + public static final int kBackLeftDriveMotorPort = 55; //motors updated + public static final int kBackLeftTurningMotorPort = 54; //motors updated + public static final boolean kBackLeftDriveMotorReversed = false; //updated + public static final boolean kBackLeftTurningMotorReversed = true; //updated + public static final int kBackLeftDriveAbsoluteEncoderPort = 33; //updated + public static final boolean kBackLeftDriveAbsoluteEncoderReversed = false; //updated public static final double kBackLeftDriveAbsoluteEncoderOffsetRad = -3; //we need to update these motors // backRight Module - public static final int kBackRightDriveMotorPort = 6; - public static final int kBackRightTurningMotorPort = 5; - public static final boolean kBackRightDriveMotorReversed = true; - public static final boolean kBackRightTurningMotorReversed = true; - public static final int kBackRightDriveAbsoluteEncoderPort = 5; - public static final boolean kBackRightDriveAbsoluteEncoderReversed = true; + public static final int kBackRightDriveMotorPort = 52; //motors updated + public static final int kBackRightTurningMotorPort = 53; //motors updated + public static final boolean kBackRightDriveMotorReversed = true; //updated + public static final boolean kBackRightTurningMotorReversed = true; //updated + public static final int kBackRightDriveAbsoluteEncoderPort = 32; //updated + public static final boolean kBackRightDriveAbsoluteEncoderReversed = false; //updated public static final double kBackRightDriveAbsoluteEncoderOffsetRad = -64; - //NOTE: these are not used in actual code they are just used to define max based on physical contraints + //NOTE: these are not used in actual code they are just used to define max based on physical contraints //If you want to ignore these then change the limit on the max speeds manually //these seem to be mostly fine but we may need change some things // also we need to change the physical dimensions of our base if we are going to use this From a47e4dcbf939755c9f68dc38fa97e73705389f9b Mon Sep 17 00:00:00 2001 From: JingerJesus Date: Thu, 5 Oct 2023 23:08:22 -0400 Subject: [PATCH 04/20] bllaaaaargh Co-authored-by: Goldrn --- src/main/java/frc/robot/Constants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 715ff44..d3d0ab0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -45,7 +45,7 @@ public static final class DriveConstants { public static final boolean kFrontLeftTurningMotorReversed = true; //updated public static final int kFrontLeftDriveAbsoluteEncoderPort = 30; //updated public static final boolean kFrontLeftDriveAbsoluteEncoderReversed = false; //updated - public static final double kFrontLeftDriveAbsoluteEncoderOffsetRad = -106.5; + public static final double kFrontLeftDriveAbsoluteEncoderOffsetRad = -2.0 * Math.PI * 0.134033; //updated, in radians // they want this to be the negative of the reported values? //we need to updats these motors // frontRight Module @@ -55,7 +55,7 @@ public static final class DriveConstants { public static final boolean kFrontRightTurningMotorReversed = true; //updated public static final int kFrontRightDriveAbsoluteEncoderPort = 31; //updated public static final boolean kFrontRightDriveAbsoluteEncoderReversed = false; //updated - public static final double kFrontRightDriveAbsoluteEncoderOffsetRad = -128; + public static final double kFrontRightDriveAbsoluteEncoderOffsetRad = -2.0 * Math.PI * 0.279297; //updated, in radians //we need to update these motors // backLeft Module @@ -65,7 +65,7 @@ public static final class DriveConstants { public static final boolean kBackLeftTurningMotorReversed = true; //updated public static final int kBackLeftDriveAbsoluteEncoderPort = 33; //updated public static final boolean kBackLeftDriveAbsoluteEncoderReversed = false; //updated - public static final double kBackLeftDriveAbsoluteEncoderOffsetRad = -3; + public static final double kBackLeftDriveAbsoluteEncoderOffsetRad = -2.0 * Math.PI * 0.029297; //updated, in radians //we need to update these motors // backRight Module @@ -75,7 +75,7 @@ public static final class DriveConstants { public static final boolean kBackRightTurningMotorReversed = true; //updated public static final int kBackRightDriveAbsoluteEncoderPort = 32; //updated public static final boolean kBackRightDriveAbsoluteEncoderReversed = false; //updated - public static final double kBackRightDriveAbsoluteEncoderOffsetRad = -64; + public static final double kBackRightDriveAbsoluteEncoderOffsetRad = -2.0 * Math.PI * 0.607910; //updated, in radians //NOTE: these are not used in actual code they are just used to define max based on physical contraints From fb87c101de29f60739776fe7eb15b1ffaff749d6 Mon Sep 17 00:00:00 2001 From: JingerJesus Date: Fri, 6 Oct 2023 00:03:59 -0400 Subject: [PATCH 05/20] :3 Co-authored-by: Goldrn Co-authored-by: Bionical-X --- src/main/java/frc/robot/Constants.java | 27 +++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d3d0ab0..d8b1b87 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,12 +12,13 @@ public static final class ModuleConstants { public static final double kDriveMotorGearRatio = 1/6.75; public static final double kTurningMotorGearRatio = 1/21.0; public static final double kDriveEncoderRot2Meter = kDriveMotorGearRatio * Math.PI * kWheelDiameterMeters; - public static final double kTurningEncoderRot2Rad = kTurningMotorGearRatio * 2 * Math.PI; + public static final double kTurningEncoderRot2Deg = kTurningMotorGearRatio * 360; public static final double kDriveEncoderRPM2MeterPerSec = kDriveEncoderRot2Meter / 60; - public static final double kTurningEncoderRPM2RadPerSec = kTurningEncoderRot2Rad / 60; + public static final double kTurningEncoderRPM2DegPerSec = kTurningEncoderRot2Deg / 60; // We may need to tune this for the PID turning + // we may need to tune it so this is not set in stone public static final double kPTurning = 0.5; } @@ -45,7 +46,7 @@ public static final class DriveConstants { public static final boolean kFrontLeftTurningMotorReversed = true; //updated public static final int kFrontLeftDriveAbsoluteEncoderPort = 30; //updated public static final boolean kFrontLeftDriveAbsoluteEncoderReversed = false; //updated - public static final double kFrontLeftDriveAbsoluteEncoderOffsetRad = -2.0 * Math.PI * 0.134033; //updated, in radians // they want this to be the negative of the reported values? + public static final double kFrontLeftDriveAbsoluteEncoderOffsetDeg = -360.0 * 0.134033; //updated, in degrees // they want this to be the negative of the reported values? //we need to updats these motors // frontRight Module @@ -55,7 +56,7 @@ public static final class DriveConstants { public static final boolean kFrontRightTurningMotorReversed = true; //updated public static final int kFrontRightDriveAbsoluteEncoderPort = 31; //updated public static final boolean kFrontRightDriveAbsoluteEncoderReversed = false; //updated - public static final double kFrontRightDriveAbsoluteEncoderOffsetRad = -2.0 * Math.PI * 0.279297; //updated, in radians + public static final double kFrontRightDriveAbsoluteEncoderOffsetDeg = -360.0 * 0.279297; //updated, in degrees //we need to update these motors // backLeft Module @@ -65,7 +66,7 @@ public static final class DriveConstants { public static final boolean kBackLeftTurningMotorReversed = true; //updated public static final int kBackLeftDriveAbsoluteEncoderPort = 33; //updated public static final boolean kBackLeftDriveAbsoluteEncoderReversed = false; //updated - public static final double kBackLeftDriveAbsoluteEncoderOffsetRad = -2.0 * Math.PI * 0.029297; //updated, in radians + public static final double kBackLeftDriveAbsoluteEncoderOffsetDeg = -360.0 * 0.029297; //updated, in degrees //we need to update these motors // backRight Module @@ -75,19 +76,19 @@ public static final class DriveConstants { public static final boolean kBackRightTurningMotorReversed = true; //updated public static final int kBackRightDriveAbsoluteEncoderPort = 32; //updated public static final boolean kBackRightDriveAbsoluteEncoderReversed = false; //updated - public static final double kBackRightDriveAbsoluteEncoderOffsetRad = -2.0 * Math.PI * 0.607910; //updated, in radians + public static final double kBackRightDriveAbsoluteEncoderOffsetDeg = -360.0 * 0.607910; //updated, in degrees //NOTE: these are not used in actual code they are just used to define max based on physical contraints //If you want to ignore these then change the limit on the max speeds manually //these seem to be mostly fine but we may need change some things // also we need to change the physical dimensions of our base if we are going to use this - public static final double kPhysicalMaxSpeedMetersPerSecond = 6380.0 / 60.0 * (ModuleConstants.kDriveMotorGearRatio) * ModuleConstants.kWheelDiameterMeters * Math.PI * 2; - public static final double kPhysicalMaxAngularSpeedRadiansPerSecond = kPhysicalMaxSpeedMetersPerSecond / Math.hypot(DriveConstants.kTrackWidth / 2.0, DriveConstants.kWheelBase / 2.0 * 3); + public static final double kPhysicalMaxSpeedMetersPerSecond = 6380.0 / 60.0 * (ModuleConstants.kDriveMotorGearRatio) * ModuleConstants.kWheelDiameterMeters * 360; + public static final double kPhysicalMaxAngularSpeedDegreesPerSecond = kPhysicalMaxSpeedMetersPerSecond / Math.hypot(DriveConstants.kTrackWidth / 2.0, DriveConstants.kWheelBase / 2.0 * 3); //These are the variables that determine the max speeds of our swerve drive public static final double kTeleDriveMaxSpeedMetersPerSecond = kPhysicalMaxSpeedMetersPerSecond / 4; - public static final double kTeleDriveMaxAngularSpeedRadiansPerSecond = kPhysicalMaxAngularSpeedRadiansPerSecond / 4; + public static final double kTeleDriveMaxAngularSpeedDegreesPerSecond = kPhysicalMaxAngularSpeedDegreesPerSecond / 4; public static final double kTeleDriveMaxAccelerationUnitsPerSecond = 3; public static final double kTeleDriveMaxAngularAccelerationUnitsPerSecond = 3; @@ -96,18 +97,18 @@ public static final class DriveConstants { //these we can ignore for now since they are only for autonmous public static final class AutoConstants { public static final double kMaxSpeedMetersPerSecond = DriveConstants.kPhysicalMaxSpeedMetersPerSecond / 4; - public static final double kMaxAngularSpeedRadiansPerSecond = DriveConstants.kPhysicalMaxAngularSpeedRadiansPerSecond / 10; + public static final double kMaxAngularSpeedDegreesPerSecond = DriveConstants.kPhysicalMaxAngularSpeedDegreesPerSecond / 10; public static final double kMaxAccelerationMetersPerSecondSquared = 3; - public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI / 4; + public static final double kMaxAngularAccelerationDegreesPerSecondSquared = 45.0; public static final double kPXController = 1.5; public static final double kPYController = 1.5; public static final double kPThetaController = 3; public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( - kMaxAngularSpeedRadiansPerSecond, - kMaxAngularAccelerationRadiansPerSecondSquared); + kMaxAngularSpeedDegreesPerSecond, + kMaxAngularAccelerationDegreesPerSecondSquared); } //this is just to set a deadzone so we don't need to move the stick From b1078225fe5dd6f9aeec88dfda701b39eafb970b Mon Sep 17 00:00:00 2001 From: JingerJesus Date: Fri, 6 Oct 2023 00:04:01 -0400 Subject: [PATCH 06/20] colon 3 --- src/main/java/frc/robot/RobotContainer.java | 3 +++ .../java/frc/robot/commands/SwerveCommand.java | 2 +- .../java/frc/robot/subsystems/SwerveModule.java | 14 +++++++------- .../frc/robot/subsystems/SwerveSubsystem.java | 16 ++++++++-------- 4 files changed, 19 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 36cea3c..3fe1f5e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,8 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.commands.SwerveCommand; import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.Constants; +import frc.robot.Constants.DriveConstants; public class RobotContainer { @@ -13,6 +15,7 @@ public class RobotContainer { public RobotContainer() { swerveSubsystem.setDefaultCommand(new SwerveCommand(swerveSubsystem, driverOne)); configureButtonBindings(); + // System.out.println(DriveConstants.kTeleDriveMaxSpeedMetersPerSecond); } private void configureButtonBindings() { diff --git a/src/main/java/frc/robot/commands/SwerveCommand.java b/src/main/java/frc/robot/commands/SwerveCommand.java index 9327af8..c8fb96a 100644 --- a/src/main/java/frc/robot/commands/SwerveCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommand.java @@ -34,7 +34,7 @@ public void execute() { double ySpeed = controller.getLeftX() * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; ySpeed = Math.abs(ySpeed) > OIConstants.kDeadband ? ySpeed : 0.0; - double turningSpeed = controller.getRightX() * DriveConstants.kTeleDriveMaxAngularSpeedRadiansPerSecond; + double turningSpeed = controller.getRightX() * DriveConstants.kTeleDriveMaxAngularSpeedDegreesPerSecond; turningSpeed = Math.abs(turningSpeed) > OIConstants.kDeadband ? turningSpeed : 0.0; ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeed, Rotation2d.fromDegrees(swerveSubsystem.getHeading())); diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 36f7e40..354d8ed 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -52,8 +52,8 @@ public SwerveModule(String ModuleName, int driveMotorId, int turningMotorId, boo driveEncoder.setPositionConversionFactor(ModuleConstants.kDriveEncoderRot2Meter); driveEncoder.setVelocityConversionFactor(ModuleConstants.kDriveEncoderRPM2MeterPerSec); - turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderRot2Rad); - turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderRPM2RadPerSec); + turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderRot2Deg); + turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderRPM2DegPerSec); turningPidController = new PIDController(ModuleConstants.kPTurning, 0, 0); turningPidController.enableContinuousInput(-180, 180); @@ -65,19 +65,19 @@ private void configureMotor(CANSparkMax motor, Boolean inverted) { motor.restoreFactoryDefaults(); motor.setIdleMode(IdleMode.kBrake); motor.setInverted(inverted); - motor.setSmartCurrentLimit(40); + motor.setSmartCurrentLimit(30); motor.burnFlash(); } public SwerveModulePosition getPosition() { - return new SwerveModulePosition(driveEncoder.getPosition(), Rotation2d.fromDegrees(getAbsoluteEncoderRad())); + return new SwerveModulePosition(driveEncoder.getPosition(), Rotation2d.fromDegrees(getAbsoluteEncoderDeg())); } public double getDriveVelocity() { return driveEncoder.getVelocity(); } - public double getAbsoluteEncoderRad() { + public double getAbsoluteEncoderDeg() { return absoluteEncoder.getAbsolutePosition(); } @@ -87,7 +87,7 @@ public void resetEncoders() { } public SwerveModuleState getState() { - return new SwerveModuleState(getDriveVelocity(), Rotation2d.fromDegrees(getAbsoluteEncoderRad())); + return new SwerveModuleState(getDriveVelocity(), Rotation2d.fromDegrees(getAbsoluteEncoderDeg())); } public void setDesiredState(SwerveModuleState state) { @@ -98,7 +98,7 @@ public void setDesiredState(SwerveModuleState state) { state = SwerveModuleState.optimize(state, getState().angle); double driveSpeed = state.speedMetersPerSecond*0.2; driveMotor.set(driveSpeed); - double turnSpeed = (turningPidController.calculate(getAbsoluteEncoderRad(), state.angle.getDegrees()))/20; + double turnSpeed = (turningPidController.calculate(getAbsoluteEncoderDeg(), state.angle.getDegrees()))/20; turningMotor.set(turnSpeed); //System.out.println(ModuleName + "- DriveMotorCommand: " + driveSpeed + " - True Angle: " + getAbsoluteEncoderRad() + " AngleSetPoint: " + state.angle.getDegrees() + " AngleMotorCommand: " + turnSpeed); } diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 18778f5..fcb97ba 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -31,7 +31,7 @@ public SwerveSubsystem() { DriveConstants.kFrontLeftDriveMotorReversed, DriveConstants.kFrontLeftTurningMotorReversed, DriveConstants.kFrontLeftDriveAbsoluteEncoderPort, - DriveConstants.kFrontLeftDriveAbsoluteEncoderOffsetRad, + DriveConstants.kFrontLeftDriveAbsoluteEncoderOffsetDeg, DriveConstants.kFrontLeftDriveAbsoluteEncoderReversed); frontRight = new SwerveModule( @@ -41,7 +41,7 @@ public SwerveSubsystem() { DriveConstants.kFrontRightDriveMotorReversed, DriveConstants.kFrontRightTurningMotorReversed, DriveConstants.kFrontRightDriveAbsoluteEncoderPort, - DriveConstants.kFrontRightDriveAbsoluteEncoderOffsetRad, + DriveConstants.kFrontRightDriveAbsoluteEncoderOffsetDeg, DriveConstants.kFrontRightDriveAbsoluteEncoderReversed); backLeft = new SwerveModule( @@ -51,7 +51,7 @@ public SwerveSubsystem() { DriveConstants.kBackLeftDriveMotorReversed, DriveConstants.kBackLeftTurningMotorReversed, DriveConstants.kBackLeftDriveAbsoluteEncoderPort, - DriveConstants.kBackLeftDriveAbsoluteEncoderOffsetRad, + DriveConstants.kBackLeftDriveAbsoluteEncoderOffsetDeg, DriveConstants.kBackLeftDriveAbsoluteEncoderReversed); backRight = new SwerveModule( @@ -61,7 +61,7 @@ public SwerveSubsystem() { DriveConstants.kBackRightDriveMotorReversed, DriveConstants.kBackRightTurningMotorReversed, DriveConstants.kBackRightDriveAbsoluteEncoderPort, - DriveConstants.kBackRightDriveAbsoluteEncoderOffsetRad, + DriveConstants.kBackRightDriveAbsoluteEncoderOffsetDeg, DriveConstants.kBackRightDriveAbsoluteEncoderReversed); odometer = new SwerveDriveOdometry(DriveConstants.kDriveKinematics, getRotation2d(), new SwerveModulePosition[] { @@ -100,10 +100,10 @@ public void periodic() { backLeft.getPosition(), backRight.getPosition() }); SmartDashboard.putNumber("Robot Heading", getHeading()); - SmartDashboard.putNumber("Front Left Wheel Angle", frontLeft.getAbsoluteEncoderRad()); - SmartDashboard.putNumber("Front Right Wheel Angle", frontRight.getAbsoluteEncoderRad()); - SmartDashboard.putNumber("Back Left Wheel Angle", backLeft.getAbsoluteEncoderRad()); - SmartDashboard.putNumber("Back Right Wheel Angle", backRight.getAbsoluteEncoderRad()); + SmartDashboard.putNumber("Front Left Wheel Angle", frontLeft.getAbsoluteEncoderDeg()); + SmartDashboard.putNumber("Front Right Wheel Angle", frontRight.getAbsoluteEncoderDeg()); + SmartDashboard.putNumber("Back Left Wheel Angle", backLeft.getAbsoluteEncoderDeg()); + SmartDashboard.putNumber("Back Right Wheel Angle", backRight.getAbsoluteEncoderDeg()); SmartDashboard.putString("Robot Location", getPose().getTranslation().toString()); } From 7a3fc1ffd25956dc2ad77e52f361fa58b0e3d5b7 Mon Sep 17 00:00:00 2001 From: goldrn Date: Mon, 16 Oct 2023 20:32:10 -0400 Subject: [PATCH 07/20] this should be the latest code --- .../java/frc/robot/commands/SwerveCommand.java | 17 +++++++++-------- .../frc/robot/subsystems/SwerveSubsystem.java | 18 +++++++++--------- 2 files changed, 18 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/commands/SwerveCommand.java b/src/main/java/frc/robot/commands/SwerveCommand.java index c8fb96a..d598c8c 100644 --- a/src/main/java/frc/robot/commands/SwerveCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommand.java @@ -28,17 +28,18 @@ public void initialize() { @Override public void execute() { - double xSpeed = -controller.getLeftY() * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; - xSpeed = Math.abs(xSpeed) > OIConstants.kDeadband ? xSpeed : 0.0; + //this.swerveSubsystem.getpo + // double xSpeed = -controller.getLeftY() * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; + // xSpeed = Math.abs(xSpeed) > OIConstants.kDeadband ? xSpeed : 0.0; - double ySpeed = controller.getLeftX() * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; - ySpeed = Math.abs(ySpeed) > OIConstants.kDeadband ? ySpeed : 0.0; + // double ySpeed = controller.getLeftX() * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; + // ySpeed = Math.abs(ySpeed) > OIConstants.kDeadband ? ySpeed : 0.0; - double turningSpeed = controller.getRightX() * DriveConstants.kTeleDriveMaxAngularSpeedDegreesPerSecond; - turningSpeed = Math.abs(turningSpeed) > OIConstants.kDeadband ? turningSpeed : 0.0; + // double turningSpeed = controller.getRightX() * DriveConstants.kTeleDriveMaxAngularSpeedDegreesPerSecond; + // turningSpeed = Math.abs(turningSpeed) > OIConstants.kDeadband ? turningSpeed : 0.0; - ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeed, Rotation2d.fromDegrees(swerveSubsystem.getHeading())); - swerveSubsystem.setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds)); + // ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeed, Rotation2d.fromDegrees(swerveSubsystem.getHeading())); + // swerveSubsystem.setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds)); } @Override diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index fcb97ba..b5c1610 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -95,16 +95,16 @@ public Pose2d getPose() { @Override public void periodic() { - odometer.update(getRotation2d(), new SwerveModulePosition[] { - frontLeft.getPosition(), frontRight.getPosition(), - backLeft.getPosition(), backRight.getPosition() - }); - SmartDashboard.putNumber("Robot Heading", getHeading()); + // odometer.update(getRotation2d(), new SwerveModulePosition[] { + // frontLeft.getPosition(), frontRight.getPosition(), + // backLeft.getPosition(), backRight.getPosition() + // }); + //SmartDashboard.putNumber("Robot Heading", getHeading()); SmartDashboard.putNumber("Front Left Wheel Angle", frontLeft.getAbsoluteEncoderDeg()); - SmartDashboard.putNumber("Front Right Wheel Angle", frontRight.getAbsoluteEncoderDeg()); - SmartDashboard.putNumber("Back Left Wheel Angle", backLeft.getAbsoluteEncoderDeg()); - SmartDashboard.putNumber("Back Right Wheel Angle", backRight.getAbsoluteEncoderDeg()); - SmartDashboard.putString("Robot Location", getPose().getTranslation().toString()); + // SmartDashboard.putNumber("Front Right Wheel Angle", frontRight.getAbsoluteEncoderDeg()); + // SmartDashboard.putNumber("Back Left Wheel Angle", backLeft.getAbsoluteEncoderDeg()); + // SmartDashboard.putNumber("Back Right Wheel Angle", backRight.getAbsoluteEncoderDeg()); + //SmartDashboard.putString("Robot Location", getPose().getTranslation().toString()); } public void stopModules() { From 9b4328c8070aa452f19bc14557672b1807a65734 Mon Sep 17 00:00:00 2001 From: goldrn Date: Fri, 20 Oct 2023 00:42:06 -0400 Subject: [PATCH 08/20] Moved over to new can coder library Co-authored-by: Eugene, Bringer of Pestilence, Devourer of the Weak --- src/main/java/frc/robot/Constants.java | 6 + .../frc/robot/subsystems/SwerveModule.java | 25 +- .../frc/robot/subsystems/SwerveSubsystem.java | 8 +- vendordeps/PhoenixProAnd5.json | 458 ++++++++++++++++++ 4 files changed, 483 insertions(+), 14 deletions(-) create mode 100644 vendordeps/PhoenixProAnd5.json diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d8b1b87..59eb86f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,5 +1,7 @@ package frc.robot; +import com.ctre.phoenixpro.signals.SensorDirectionValue; + import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -47,6 +49,7 @@ public static final class DriveConstants { public static final int kFrontLeftDriveAbsoluteEncoderPort = 30; //updated public static final boolean kFrontLeftDriveAbsoluteEncoderReversed = false; //updated public static final double kFrontLeftDriveAbsoluteEncoderOffsetDeg = -360.0 * 0.134033; //updated, in degrees // they want this to be the negative of the reported values? + public static final SensorDirectionValue kFrontLeftTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive; //we need to updats these motors // frontRight Module @@ -57,6 +60,7 @@ public static final class DriveConstants { public static final int kFrontRightDriveAbsoluteEncoderPort = 31; //updated public static final boolean kFrontRightDriveAbsoluteEncoderReversed = false; //updated public static final double kFrontRightDriveAbsoluteEncoderOffsetDeg = -360.0 * 0.279297; //updated, in degrees + public static final SensorDirectionValue kFrontRightTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive; //we need to update these motors // backLeft Module @@ -67,6 +71,7 @@ public static final class DriveConstants { public static final int kBackLeftDriveAbsoluteEncoderPort = 33; //updated public static final boolean kBackLeftDriveAbsoluteEncoderReversed = false; //updated public static final double kBackLeftDriveAbsoluteEncoderOffsetDeg = -360.0 * 0.029297; //updated, in degrees + public static final SensorDirectionValue kBackLeftTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive; //we need to update these motors // backRight Module @@ -77,6 +82,7 @@ public static final class DriveConstants { public static final int kBackRightDriveAbsoluteEncoderPort = 32; //updated public static final boolean kBackRightDriveAbsoluteEncoderReversed = false; //updated public static final double kBackRightDriveAbsoluteEncoderOffsetDeg = -360.0 * 0.607910; //updated, in degrees + public static final SensorDirectionValue kBackRightTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive; //NOTE: these are not used in actual code they are just used to define max based on physical contraints diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 354d8ed..fc4c53b 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -12,8 +12,12 @@ import frc.robot.Constants.ModuleConstants; import com.ctre.phoenix.sensors.AbsoluteSensorRange; -import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.CANCoder; //BAD CRINGE import com.ctre.phoenix.sensors.CANCoderConfiguration; +import com.ctre.phoenixpro.configs.CANcoderConfiguration; +import com.ctre.phoenixpro.hardware.CANcoder; +import com.ctre.phoenixpro.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenixpro.signals.SensorDirectionValue; public class SwerveModule { public final CANSparkMax driveMotor; @@ -24,21 +28,22 @@ public class SwerveModule { private final PIDController turningPidController; - private final CANCoder absoluteEncoder; + private final CANcoder absoluteEncoder; private final double absoluteEncoderOffset; private final String ModuleName; public SwerveModule(String ModuleName, int driveMotorId, int turningMotorId, boolean driveMotorReversed, boolean turningMotorReversed, - int absoluteEncoderId, double absoluteEncoderOffset, boolean absoluteEncoderReversed) { + int absoluteEncoderId, double absoluteEncoderOffset, SensorDirectionValue positiveDirection) { this.ModuleName = ModuleName; - absoluteEncoder = new CANCoder(absoluteEncoderId); - CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); - canCoderConfiguration.magnetOffsetDegrees = absoluteEncoderOffset; - canCoderConfiguration.sensorDirection = absoluteEncoderReversed; - canCoderConfiguration.absoluteSensorRange = AbsoluteSensorRange.Signed_PlusMinus180; - absoluteEncoder.configAllSettings(canCoderConfiguration); + absoluteEncoder = new CANcoder(absoluteEncoderId); + CANcoderConfiguration canCoderConfiguration = new CANcoderConfiguration(); + canCoderConfiguration.MagnetSensor.MagnetOffset = absoluteEncoderOffset; + canCoderConfiguration.MagnetSensor.SensorDirection = positiveDirection; + canCoderConfiguration.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + //absoluteEncoder.configAllSettings(canCoderConfiguration); + absoluteEncoder.getConfigurator().apply(canCoderConfiguration); this.absoluteEncoderOffset = absoluteEncoderOffset; @@ -78,7 +83,7 @@ public double getDriveVelocity() { } public double getAbsoluteEncoderDeg() { - return absoluteEncoder.getAbsolutePosition(); + return absoluteEncoder.getAbsolutePosition().getValue(); } public void resetEncoders() { diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index b5c1610..df51533 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -32,7 +32,7 @@ public SwerveSubsystem() { DriveConstants.kFrontLeftTurningMotorReversed, DriveConstants.kFrontLeftDriveAbsoluteEncoderPort, DriveConstants.kFrontLeftDriveAbsoluteEncoderOffsetDeg, - DriveConstants.kFrontLeftDriveAbsoluteEncoderReversed); + DriveConstants.kFrontLeftTurningForwardDirection); frontRight = new SwerveModule( "Front Right", @@ -42,7 +42,7 @@ public SwerveSubsystem() { DriveConstants.kFrontRightTurningMotorReversed, DriveConstants.kFrontRightDriveAbsoluteEncoderPort, DriveConstants.kFrontRightDriveAbsoluteEncoderOffsetDeg, - DriveConstants.kFrontRightDriveAbsoluteEncoderReversed); + DriveConstants.kFrontRightTurningForwardDirection); backLeft = new SwerveModule( "Back Left", @@ -52,7 +52,7 @@ public SwerveSubsystem() { DriveConstants.kBackLeftTurningMotorReversed, DriveConstants.kBackLeftDriveAbsoluteEncoderPort, DriveConstants.kBackLeftDriveAbsoluteEncoderOffsetDeg, - DriveConstants.kBackLeftDriveAbsoluteEncoderReversed); + DriveConstants.kBackLeftTurningForwardDirection); backRight = new SwerveModule( "Back Right", @@ -62,7 +62,7 @@ public SwerveSubsystem() { DriveConstants.kBackRightTurningMotorReversed, DriveConstants.kBackRightDriveAbsoluteEncoderPort, DriveConstants.kBackRightDriveAbsoluteEncoderOffsetDeg, - DriveConstants.kBackRightDriveAbsoluteEncoderReversed); + DriveConstants.kBackRightTurningForwardDirection); odometer = new SwerveDriveOdometry(DriveConstants.kDriveKinematics, getRotation2d(), new SwerveModulePosition[] { frontLeft.getPosition(), frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()}); diff --git a/vendordeps/PhoenixProAnd5.json b/vendordeps/PhoenixProAnd5.json new file mode 100644 index 0000000..21206b0 --- /dev/null +++ b/vendordeps/PhoenixProAnd5.json @@ -0,0 +1,458 @@ +{ + "fileName": "PhoenixProAnd5.json", + "name": "CTRE-Phoenix (Pro And v5)", + "version": "23.0.12", + "frcYear": 2023, + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/PhoenixProAnd5-frc2023-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.30.4" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.30.4" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "wpiapi-java", + "version": "23.0.12" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.30.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.30.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "tools", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "tools-sim", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonSRX", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonFX", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simVictorSPX", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simPigeonIMU", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simCANCoder", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProTalonFX", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProCANcoder", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProPigeon2", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.30.4", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.30.4", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.30.4", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "tools", + "version": "23.0.12", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.30.4", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.30.4", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.30.4", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "tools-sim", + "version": "23.0.12", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonSRX", + "version": "23.0.12", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonFX", + "version": "23.0.12", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simVictorSPX", + "version": "23.0.12", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simPigeonIMU", + "version": "23.0.12", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simCANCoder", + "version": "23.0.12", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProTalonFX", + "version": "23.0.12", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProCANcoder", + "version": "23.0.12", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProPigeon2", + "version": "23.0.12", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "wpiapi-cpp", + "version": "23.0.12", + "libName": "CTRE_PhoenixPro_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "23.0.12", + "libName": "CTRE_PhoenixPro_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file From d17e0153bb338df2c00bcebac5f9ac472a8da06b Mon Sep 17 00:00:00 2001 From: goldrn Date: Thu, 26 Oct 2023 20:39:39 -0400 Subject: [PATCH 09/20] they be spinnin so good Co-authored-by: Eugene, Bringer of Pestilence, Devourer of the Weak Co-authored-by: EvanWoodfin --- networktables.json | 1 + src/main/java/frc/robot/commands/SwerveCommand.java | 6 ++++++ src/main/java/frc/robot/subsystems/SwerveSubsystem.java | 4 ++++ 3 files changed, 11 insertions(+) create mode 100644 networktables.json diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/src/main/java/frc/robot/commands/SwerveCommand.java b/src/main/java/frc/robot/commands/SwerveCommand.java index d598c8c..71dbdc0 100644 --- a/src/main/java/frc/robot/commands/SwerveCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommand.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants.DriveConstants; @@ -40,6 +41,11 @@ public void execute() { // ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeed, Rotation2d.fromDegrees(swerveSubsystem.getHeading())); // swerveSubsystem.setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds)); + this.swerveSubsystem.setModuleStates(new SwerveModuleState[] {new SwerveModuleState(.5, new Rotation2d(.1)), new SwerveModuleState(.5, new Rotation2d(.1)), new SwerveModuleState(.5, new Rotation2d(.1)), new SwerveModuleState(.5, new Rotation2d(.1))}); + for (int i = 0; i < this.swerveSubsystem.getModuleStates().length; i ++) { + System.out.print(this.swerveSubsystem.getModuleStates()[i] + ", " ); + } + System.out.print("\n"); } @Override diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index df51533..c8d7da4 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -121,4 +121,8 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { backLeft.setDesiredState(desiredStates[2]); backRight.setDesiredState(desiredStates[3]); } + + public SwerveModuleState[] getModuleStates() { + return new SwerveModuleState[] {frontLeft.getState(), frontLeft.getState(), backLeft.getState(), backRight.getState()}; + } } \ No newline at end of file From 2d2950cc0292a439b033edda0f93e761774d19ed Mon Sep 17 00:00:00 2001 From: goldrn Date: Fri, 3 Nov 2023 00:41:19 -0400 Subject: [PATCH 10/20] Front left is correct, the others are wrong and cringe and bad Co-authored-by: Eugene, Bringer of Pestilence, Devourer of the Weak Co-authored-by: Bionical-X Co-authored-by: Evan W Co-authored-by: Wide Glorg, Devourer of the Inhumane, Herald of Absorption Co-authored-by: monkey13blue Co-authored-by: Wevan --- src/main/java/frc/robot/Constants.java | 12 ++++++--- .../frc/robot/commands/SwerveCommand.java | 27 +++++++++++++++---- .../frc/robot/subsystems/SwerveModule.java | 8 +++++- 3 files changed, 37 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 59eb86f..f65322d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -42,46 +42,50 @@ public static final class DriveConstants { //We need to update these motors // frontLeft Module + // offsets should be drawn from no offest vaule on x tuner public static final int kFrontLeftDriveMotorPort = 57; //motors updated public static final int kFrontLeftTurningMotorPort = 56; //motors updated public static final boolean kFrontLeftDriveMotorReversed = false; //updated public static final boolean kFrontLeftTurningMotorReversed = true; //updated public static final int kFrontLeftDriveAbsoluteEncoderPort = 30; //updated public static final boolean kFrontLeftDriveAbsoluteEncoderReversed = false; //updated - public static final double kFrontLeftDriveAbsoluteEncoderOffsetDeg = -360.0 * 0.134033; //updated, in degrees // they want this to be the negative of the reported values? + public static final double kFrontLeftDriveAbsoluteEncoderOffsetDeg = -0.134521; //updated, in degrees // they want this to be the negative of the reported values? public static final SensorDirectionValue kFrontLeftTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive; //we need to updats these motors // frontRight Module + // offset -0.266357 public static final int kFrontRightDriveMotorPort = 51; //motors updated public static final int kFrontRightTurningMotorPort = 50; //motors updated public static final boolean kFrontRightDriveMotorReversed = true; //updated public static final boolean kFrontRightTurningMotorReversed = true; //updated public static final int kFrontRightDriveAbsoluteEncoderPort = 31; //updated public static final boolean kFrontRightDriveAbsoluteEncoderReversed = false; //updated - public static final double kFrontRightDriveAbsoluteEncoderOffsetDeg = -360.0 * 0.279297; //updated, in degrees + public static final double kFrontRightDriveAbsoluteEncoderOffsetDeg = 360.0 * -0.266357; //updated, in degrees public static final SensorDirectionValue kFrontRightTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive; //we need to update these motors // backLeft Module + // offset -0.480713 public static final int kBackLeftDriveMotorPort = 55; //motors updated public static final int kBackLeftTurningMotorPort = 54; //motors updated public static final boolean kBackLeftDriveMotorReversed = false; //updated public static final boolean kBackLeftTurningMotorReversed = true; //updated public static final int kBackLeftDriveAbsoluteEncoderPort = 33; //updated public static final boolean kBackLeftDriveAbsoluteEncoderReversed = false; //updated - public static final double kBackLeftDriveAbsoluteEncoderOffsetDeg = -360.0 * 0.029297; //updated, in degrees + public static final double kBackLeftDriveAbsoluteEncoderOffsetDeg = 360.0 * -0.480713; //updated, in degrees public static final SensorDirectionValue kBackLeftTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive; //we need to update these motors // backRight Module + //0.480957 public static final int kBackRightDriveMotorPort = 52; //motors updated public static final int kBackRightTurningMotorPort = 53; //motors updated public static final boolean kBackRightDriveMotorReversed = true; //updated public static final boolean kBackRightTurningMotorReversed = true; //updated public static final int kBackRightDriveAbsoluteEncoderPort = 32; //updated public static final boolean kBackRightDriveAbsoluteEncoderReversed = false; //updated - public static final double kBackRightDriveAbsoluteEncoderOffsetDeg = -360.0 * 0.607910; //updated, in degrees + public static final double kBackRightDriveAbsoluteEncoderOffsetDeg = 360.0 * 0.480957; //updated, in degrees public static final SensorDirectionValue kBackRightTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive; diff --git a/src/main/java/frc/robot/commands/SwerveCommand.java b/src/main/java/frc/robot/commands/SwerveCommand.java index 71dbdc0..4d27485 100644 --- a/src/main/java/frc/robot/commands/SwerveCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommand.java @@ -1,7 +1,9 @@ package frc.robot.commands; import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenixpro.hardware.CANcoder; import com.kauailabs.navx.frc.AHRS; +import com.revrobotics.CANSparkMax; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -16,11 +18,15 @@ public class SwerveCommand extends CommandBase { private final SwerveSubsystem swerveSubsystem; private final XboxController controller; + private CANcoder FrontLeftCoder; + private CANSparkMax FrontLeftTurnSpark; public SwerveCommand(SwerveSubsystem swerveSubsystem, XboxController controller) { this.swerveSubsystem = swerveSubsystem; this.controller = controller; addRequirements(swerveSubsystem); + this.FrontLeftCoder = swerveSubsystem.frontLeft.getCANcoder(); + this.FrontLeftTurnSpark = swerveSubsystem.frontLeft.turningMotor; } @Override @@ -29,6 +35,15 @@ public void initialize() { @Override public void execute() { + + // :3 + + // + System.out.println("ANGLE = " + swerveSubsystem.frontLeft.getAbsoluteEncoderDeg() + ", SPEED = " + FrontLeftTurnSpark.get()); + + // \:3 + + //this.swerveSubsystem.getpo // double xSpeed = -controller.getLeftY() * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; // xSpeed = Math.abs(xSpeed) > OIConstants.kDeadband ? xSpeed : 0.0; @@ -41,11 +56,13 @@ public void execute() { // ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeed, Rotation2d.fromDegrees(swerveSubsystem.getHeading())); // swerveSubsystem.setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds)); - this.swerveSubsystem.setModuleStates(new SwerveModuleState[] {new SwerveModuleState(.5, new Rotation2d(.1)), new SwerveModuleState(.5, new Rotation2d(.1)), new SwerveModuleState(.5, new Rotation2d(.1)), new SwerveModuleState(.5, new Rotation2d(.1))}); - for (int i = 0; i < this.swerveSubsystem.getModuleStates().length; i ++) { - System.out.print(this.swerveSubsystem.getModuleStates()[i] + ", " ); - } - System.out.print("\n"); + // this.swerveSubsystem.setModuleStates(new SwerveModuleState[] {new SwerveModuleState(0.5, new Rotation2d(0.1)), new SwerveModuleState(0.5, new Rotation2d(0.1)), new SwerveModuleState(0.5, new Rotation2d(0.1)), new SwerveModuleState(.5, new Rotation2d( 0.1))}); + //for (int i = 0; i < this.swerveSubsystem.getModuleStates().length; i ++) { + // System.out.print(this.swerveSubsystem.getModuleStates()[i] + ", " ); + // } + // System.out.print("\n"); + // + } @Override diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index fc4c53b..3fd4882 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -83,7 +83,9 @@ public double getDriveVelocity() { } public double getAbsoluteEncoderDeg() { - return absoluteEncoder.getAbsolutePosition().getValue(); + return (absoluteEncoder.getAbsolutePosition().getValue() * 360);// - absoluteEncoderOffset; + + //should be in degrees??? } public void resetEncoders() { @@ -112,4 +114,8 @@ public void stop() { driveMotor.set(0); turningMotor.set(0); } + + public CANcoder getCANcoder() { + return this.absoluteEncoder; + } } \ No newline at end of file From 73fdaefb51e77178c485daba3f9d231721452604 Mon Sep 17 00:00:00 2001 From: Peter Edwards Date: Thu, 16 Nov 2023 14:01:49 -0500 Subject: [PATCH 11/20] adding pid to drive speed --- src/main/java/frc/robot/Constants.java | 6 +- .../frc/robot/commands/SwerveCommand.java | 31 ++++---- .../java/frc/robot/helpers/Crashboard.java | 75 +++++++++++++++++++ .../frc/robot/subsystems/SwerveModule.java | 37 ++++++--- .../frc/robot/subsystems/SwerveSubsystem.java | 6 +- 5 files changed, 125 insertions(+), 30 deletions(-) create mode 100644 src/main/java/frc/robot/helpers/Crashboard.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f65322d..e4b761f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,7 +22,11 @@ public static final class ModuleConstants { // We may need to tune this for the PID turning // we may need to tune it so this is not set in stone - public static final double kPTurning = 0.5; + public static final double kPTurning = .01; + public static final double kDTurning = 0; + + public static final double kPModuleDriveController = .01; + public static final double kDModuleDriveController = 0; } public static final class DriveConstants { diff --git a/src/main/java/frc/robot/commands/SwerveCommand.java b/src/main/java/frc/robot/commands/SwerveCommand.java index 4d27485..9bb89c0 100644 --- a/src/main/java/frc/robot/commands/SwerveCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommand.java @@ -42,26 +42,27 @@ public void execute() { System.out.println("ANGLE = " + swerveSubsystem.frontLeft.getAbsoluteEncoderDeg() + ", SPEED = " + FrontLeftTurnSpark.get()); // \:3 - - //this.swerveSubsystem.getpo - // double xSpeed = -controller.getLeftY() * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; - // xSpeed = Math.abs(xSpeed) > OIConstants.kDeadband ? xSpeed : 0.0; - - // double ySpeed = controller.getLeftX() * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; - // ySpeed = Math.abs(ySpeed) > OIConstants.kDeadband ? ySpeed : 0.0; + double xSpeed = Math.abs(controller.getLeftY()) > OIConstants.kDeadband ? controller.getLeftY() : 0.0; + xSpeed = xSpeed * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; - // double turningSpeed = controller.getRightX() * DriveConstants.kTeleDriveMaxAngularSpeedDegreesPerSecond; - // turningSpeed = Math.abs(turningSpeed) > OIConstants.kDeadband ? turningSpeed : 0.0; + double ySpeed = Math.abs(controller.getLeftX()) > OIConstants.kDeadband ? controller.getLeftX() : 0.0; + ySpeed = ySpeed * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; - // ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeed, Rotation2d.fromDegrees(swerveSubsystem.getHeading())); - // swerveSubsystem.setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds)); - // this.swerveSubsystem.setModuleStates(new SwerveModuleState[] {new SwerveModuleState(0.5, new Rotation2d(0.1)), new SwerveModuleState(0.5, new Rotation2d(0.1)), new SwerveModuleState(0.5, new Rotation2d(0.1)), new SwerveModuleState(.5, new Rotation2d( 0.1))}); + double turningSpeed = controller.getRightX() * DriveConstants.kTeleDriveMaxAngularSpeedDegreesPerSecond; + turningSpeed = Math.abs(turningSpeed) > OIConstants.kDeadband ? turningSpeed : 0.0; + turningSpeed = 0; + System.out.println("xSpeed: " + xSpeed + " ySpeed: " + ySpeed + " turningSpeed: " + turningSpeed); + + + ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeed, Rotation2d.fromDegrees(swerveSubsystem.getHeading())); + swerveSubsystem.setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds)); + // this.swerveSubsystem.setModuleStates(new SwerveModuleState[] {new SwerveModuleState(0.5, new Rotation2d(0.1)), new SwerveModuleState(0.5, new Rotation2d(0.1)), new SwerveModuleState(0.5, new Rotation2d(0.1)), new SwerveModuleState(.5, new Rotation2d( 0.1))}); //for (int i = 0; i < this.swerveSubsystem.getModuleStates().length; i ++) { // System.out.print(this.swerveSubsystem.getModuleStates()[i] + ", " ); - // } - // System.out.print("\n"); - // + // } + // System.out.print("\n"); + // } diff --git a/src/main/java/frc/robot/helpers/Crashboard.java b/src/main/java/frc/robot/helpers/Crashboard.java new file mode 100644 index 0000000..2848357 --- /dev/null +++ b/src/main/java/frc/robot/helpers/Crashboard.java @@ -0,0 +1,75 @@ +package frc.robot.helpers; + +import java.util.Dictionary; +import java.util.Hashtable; +import java.util.Map; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; + +public class Crashboard { + + private static Dictionary DashboardEntries = new Hashtable<>(); + + public static GenericEntry toDashboard(String identifier, double value, String tab) { + return toDashboardGeneric(identifier, value, tab); + } + + private static GenericEntry toDashboardGeneric(String identifier, Object value, String tab) { + + GenericEntry genericEntry = DashboardEntries.get(getKey(identifier, tab)); + if (genericEntry == null) { + genericEntry = Shuffleboard.getTab(tab).add(identifier, value).getEntry(); + DashboardEntries.put(getKey(identifier, tab), genericEntry); + } else { + genericEntry.setValue(value); + } + return genericEntry; + + } + + public static ComplexWidget AddChooser(String identifier, SendableChooser value, String tab, BuiltInWidgets widget) { + return Shuffleboard.getTab(tab).add(identifier, value).withWidget(widget); + } + + public static GenericEntry AddSlider(String identifier, double value, String tab, double minValue, double maxValue) { + GenericEntry genericEntry = DashboardEntries.get(getKey(identifier, tab)); + if (genericEntry == null) { + genericEntry = Shuffleboard.getTab(tab) + .add(getKey(identifier, tab), value) + .withWidget(BuiltInWidgets.kNumberSlider) + .withProperties(Map.of("min", minValue, "max", maxValue)) // specify widget properties here + .getEntry(); + } + else + { + genericEntry.setValue(value); + } + return genericEntry; + } + + private static String getKey(String identifier, String tab) { + return identifier + "-" + tab; + } + + public static GenericEntry toDashboard(String identifier, boolean value, String tab) { + return toDashboardGeneric(identifier, value, tab); + } + + public static GenericEntry toDashboard(String identifier, Sendable value, String tab) { + return toDashboardGeneric(identifier, value, tab); + } + + + public static double clamp(double in, double min, double max) { + if (in < min) + in = min; + if (in > max) + in = max; + return in; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 3fd4882..b683e04 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import frc.robot.Constants.ModuleConstants; +import frc.robot.helpers.Crashboard; import com.ctre.phoenix.sensors.AbsoluteSensorRange; import com.ctre.phoenix.sensors.CANCoder; //BAD CRINGE @@ -24,12 +25,13 @@ public class SwerveModule { public final CANSparkMax turningMotor; private final RelativeEncoder driveEncoder; - private final RelativeEncoder turningEncoder; + //private final RelativeEncoder turningEncoder; private final PIDController turningPidController; + private final PIDController drivePidController; + private final CANcoder absoluteEncoder; - private final double absoluteEncoderOffset; private final String ModuleName; @@ -45,24 +47,24 @@ public SwerveModule(String ModuleName, int driveMotorId, int turningMotorId, boo //absoluteEncoder.configAllSettings(canCoderConfiguration); absoluteEncoder.getConfigurator().apply(canCoderConfiguration); - this.absoluteEncoderOffset = absoluteEncoderOffset; - driveMotor = new CANSparkMax(driveMotorId, MotorType.kBrushless); configureMotor(driveMotor, driveMotorReversed); turningMotor = new CANSparkMax(turningMotorId, MotorType.kBrushless); configureMotor(turningMotor, turningMotorReversed); driveEncoder = driveMotor.getEncoder(); - turningEncoder = turningMotor.getEncoder(); + //turningEncoder = turningMotor.getEncoder(); driveEncoder.setPositionConversionFactor(ModuleConstants.kDriveEncoderRot2Meter); driveEncoder.setVelocityConversionFactor(ModuleConstants.kDriveEncoderRPM2MeterPerSec); - turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderRot2Deg); - turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderRPM2DegPerSec); + //turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderRot2Deg); + //turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderRPM2DegPerSec); - turningPidController = new PIDController(ModuleConstants.kPTurning, 0, 0); + turningPidController = new PIDController(ModuleConstants.kPTurning, 0, ModuleConstants.kDTurning); turningPidController.enableContinuousInput(-180, 180); + this.drivePidController = new PIDController(ModuleConstants.kPModuleDriveController, 0, ModuleConstants.kDModuleDriveController); + resetEncoders(); } @@ -90,7 +92,7 @@ public double getAbsoluteEncoderDeg() { public void resetEncoders() { driveEncoder.setPosition(0); - turningEncoder.setPosition(0); + //turningEncoder.setPosition(0); } public SwerveModuleState getState() { @@ -98,14 +100,27 @@ public SwerveModuleState getState() { } public void setDesiredState(SwerveModuleState state) { + if (Math.abs(state.speedMetersPerSecond) < 0.001) { stop(); return; } state = SwerveModuleState.optimize(state, getState().angle); - double driveSpeed = state.speedMetersPerSecond*0.2; + + // Calculate the drive output from the drive PID controller. + double driveSpeed = this.drivePidController.calculate(getDriveVelocity(), state.speedMetersPerSecond); driveMotor.set(driveSpeed); - double turnSpeed = (turningPidController.calculate(getAbsoluteEncoderDeg(), state.angle.getDegrees()))/20; + + double turnSpeed = (turningPidController.calculate(getAbsoluteEncoderDeg(), state.angle.getDegrees())); + System.out.println("Turn Speed Calculated " + this.ModuleName + ": " + turnSpeed); + if (turnSpeed > 0) + turnSpeed = Math.min(turnSpeed, .2); + else + turnSpeed = Math.max(turnSpeed, -.2); + + System.out.println("Turn Speed Final " + this.ModuleName + ": " + turnSpeed); + Crashboard.toDashboard(ModuleName + "Turn Speed Final", turnSpeed, "Swerve"); + turningMotor.set(turnSpeed); //System.out.println(ModuleName + "- DriveMotorCommand: " + driveSpeed + " - True Angle: " + getAbsoluteEncoderRad() + " AngleSetPoint: " + state.angle.getDegrees() + " AngleMotorCommand: " + turnSpeed); } diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index c8d7da4..2e1bc29 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -117,9 +117,9 @@ public void stopModules() { public void setModuleStates(SwerveModuleState[] desiredStates) { SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kPhysicalMaxSpeedMetersPerSecond); frontLeft.setDesiredState(desiredStates[0]); - frontRight.setDesiredState(desiredStates[1]); - backLeft.setDesiredState(desiredStates[2]); - backRight.setDesiredState(desiredStates[3]); + // frontRight.setDesiredState(desiredStates[1]); + // backLeft.setDesiredState(desiredStates[2]); + // backRight.setDesiredState(desiredStates[3]); } public SwerveModuleState[] getModuleStates() { From 9e893966e3abbdb59abd2d36e8c95a1508ed88d8 Mon Sep 17 00:00:00 2001 From: goldrn Date: Thu, 16 Nov 2023 23:54:08 -0500 Subject: [PATCH 12/20] changes --- src/main/java/frc/robot/subsystems/SwerveModule.java | 9 ++++++++- src/main/java/frc/robot/subsystems/SwerveSubsystem.java | 2 +- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index b683e04..a08643c 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -70,7 +70,7 @@ public SwerveModule(String ModuleName, int driveMotorId, int turningMotorId, boo private void configureMotor(CANSparkMax motor, Boolean inverted) { motor.restoreFactoryDefaults(); - motor.setIdleMode(IdleMode.kBrake); + motor.setIdleMode(IdleMode.kCoast); motor.setInverted(inverted); motor.setSmartCurrentLimit(30); motor.burnFlash(); @@ -109,7 +109,14 @@ public void setDesiredState(SwerveModuleState state) { // Calculate the drive output from the drive PID controller. double driveSpeed = this.drivePidController.calculate(getDriveVelocity(), state.speedMetersPerSecond); + if (driveSpeed > .5) { + driveSpeed = .5; + } + else if (driveSpeed < -.5) { + driveSpeed = -.5; + } driveMotor.set(driveSpeed); + Crashboard.toDashboard("driveSpeed", driveSpeed, "Swerve"); double turnSpeed = (turningPidController.calculate(getAbsoluteEncoderDeg(), state.angle.getDegrees())); System.out.println("Turn Speed Calculated " + this.ModuleName + ": " + turnSpeed); diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 2e1bc29..127cbf8 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -15,7 +15,7 @@ public class SwerveSubsystem extends SubsystemBase { public static final double MAX_VOLTAGE = 12.0; public final SwerveModule frontLeft; - public final SwerveModule frontRight; + public final SwerveModule frontRight; public final SwerveModule backLeft; public final SwerveModule backRight; From 7b5b97edc36abe43e391255f02c11a6cdf459fea Mon Sep 17 00:00:00 2001 From: JingerJesus Date: Fri, 17 Nov 2023 01:54:56 -0500 Subject: [PATCH 13/20] YYYEYYEAEAAAAAAHHHHH Co-authored-by: Goldrn Co-authored-by: Wevan Co-authored-by: Bionical-X --- src/main/java/frc/robot/Constants.java | 14 +++++++------- .../frc/robot/subsystems/SwerveModule.java | 18 +++++++++++------- .../frc/robot/subsystems/SwerveSubsystem.java | 11 +++++++---- 3 files changed, 25 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e4b761f..65ac837 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -49,7 +49,7 @@ public static final class DriveConstants { // offsets should be drawn from no offest vaule on x tuner public static final int kFrontLeftDriveMotorPort = 57; //motors updated public static final int kFrontLeftTurningMotorPort = 56; //motors updated - public static final boolean kFrontLeftDriveMotorReversed = false; //updated + public static final boolean kFrontLeftDriveMotorReversed = true; //re updated //updated public static final boolean kFrontLeftTurningMotorReversed = true; //updated public static final int kFrontLeftDriveAbsoluteEncoderPort = 30; //updated public static final boolean kFrontLeftDriveAbsoluteEncoderReversed = false; //updated @@ -61,11 +61,11 @@ public static final class DriveConstants { // offset -0.266357 public static final int kFrontRightDriveMotorPort = 51; //motors updated public static final int kFrontRightTurningMotorPort = 50; //motors updated - public static final boolean kFrontRightDriveMotorReversed = true; //updated + public static final boolean kFrontRightDriveMotorReversed = false; //updated public static final boolean kFrontRightTurningMotorReversed = true; //updated public static final int kFrontRightDriveAbsoluteEncoderPort = 31; //updated public static final boolean kFrontRightDriveAbsoluteEncoderReversed = false; //updated - public static final double kFrontRightDriveAbsoluteEncoderOffsetDeg = 360.0 * -0.266357; //updated, in degrees + public static final double kFrontRightDriveAbsoluteEncoderOffsetDeg = -0.281494; //updated, in degrees public static final SensorDirectionValue kFrontRightTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive; //we need to update these motors @@ -73,11 +73,11 @@ public static final class DriveConstants { // offset -0.480713 public static final int kBackLeftDriveMotorPort = 55; //motors updated public static final int kBackLeftTurningMotorPort = 54; //motors updated - public static final boolean kBackLeftDriveMotorReversed = false; //updated + public static final boolean kBackLeftDriveMotorReversed = true; //updated public static final boolean kBackLeftTurningMotorReversed = true; //updated public static final int kBackLeftDriveAbsoluteEncoderPort = 33; //updated public static final boolean kBackLeftDriveAbsoluteEncoderReversed = false; //updated - public static final double kBackLeftDriveAbsoluteEncoderOffsetDeg = 360.0 * -0.480713; //updated, in degrees + public static final double kBackLeftDriveAbsoluteEncoderOffsetDeg = -0.027832; //updated, in degrees public static final SensorDirectionValue kBackLeftTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive; //we need to update these motors @@ -85,11 +85,11 @@ public static final class DriveConstants { //0.480957 public static final int kBackRightDriveMotorPort = 52; //motors updated public static final int kBackRightTurningMotorPort = 53; //motors updated - public static final boolean kBackRightDriveMotorReversed = true; //updated + public static final boolean kBackRightDriveMotorReversed = false; //updated public static final boolean kBackRightTurningMotorReversed = true; //updated public static final int kBackRightDriveAbsoluteEncoderPort = 32; //updated public static final boolean kBackRightDriveAbsoluteEncoderReversed = false; //updated - public static final double kBackRightDriveAbsoluteEncoderOffsetDeg = 360.0 * 0.480957; //updated, in degrees + public static final double kBackRightDriveAbsoluteEncoderOffsetDeg = 0.389160; //updated, in degrees public static final SensorDirectionValue kBackRightTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive; diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index a08643c..9bc2635 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -5,7 +5,10 @@ import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -108,15 +111,12 @@ public void setDesiredState(SwerveModuleState state) { state = SwerveModuleState.optimize(state, getState().angle); // Calculate the drive output from the drive PID controller. - double driveSpeed = this.drivePidController.calculate(getDriveVelocity(), state.speedMetersPerSecond); - if (driveSpeed > .5) { - driveSpeed = .5; - } - else if (driveSpeed < -.5) { - driveSpeed = -.5; - } + double driveSpeed = MathUtil.clamp(state.speedMetersPerSecond / 360, -.5 ,.5); + driveMotor.set(driveSpeed); Crashboard.toDashboard("driveSpeed", driveSpeed, "Swerve"); + Crashboard.toDashboard("speed in m/s", state.speedMetersPerSecond, "Swerve"); + double turnSpeed = (turningPidController.calculate(getAbsoluteEncoderDeg(), state.angle.getDegrees())); System.out.println("Turn Speed Calculated " + this.ModuleName + ": " + turnSpeed); @@ -140,4 +140,8 @@ public void stop() { public CANcoder getCANcoder() { return this.absoluteEncoder; } + + public void logIt() { + Crashboard.toDashboard(ModuleName + " Wheel Angle", this.getAbsoluteEncoderDeg(), "swerve"); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 127cbf8..689500c 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -100,7 +100,10 @@ public void periodic() { // backLeft.getPosition(), backRight.getPosition() // }); //SmartDashboard.putNumber("Robot Heading", getHeading()); - SmartDashboard.putNumber("Front Left Wheel Angle", frontLeft.getAbsoluteEncoderDeg()); + frontLeft.logIt(); + frontRight.logIt(); + backLeft.logIt(); + backRight.logIt(); // SmartDashboard.putNumber("Front Right Wheel Angle", frontRight.getAbsoluteEncoderDeg()); // SmartDashboard.putNumber("Back Left Wheel Angle", backLeft.getAbsoluteEncoderDeg()); // SmartDashboard.putNumber("Back Right Wheel Angle", backRight.getAbsoluteEncoderDeg()); @@ -117,9 +120,9 @@ public void stopModules() { public void setModuleStates(SwerveModuleState[] desiredStates) { SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kPhysicalMaxSpeedMetersPerSecond); frontLeft.setDesiredState(desiredStates[0]); - // frontRight.setDesiredState(desiredStates[1]); - // backLeft.setDesiredState(desiredStates[2]); - // backRight.setDesiredState(desiredStates[3]); + frontRight.setDesiredState(desiredStates[1]); + backLeft.setDesiredState(desiredStates[2]); + backRight.setDesiredState(desiredStates[3]); } public SwerveModuleState[] getModuleStates() { From 591041300166266b394270b994d878dc77fecb1d Mon Sep 17 00:00:00 2001 From: goldrn Date: Thu, 30 Nov 2023 19:43:23 -0500 Subject: [PATCH 14/20] beginning of the navx shenanagins Co-authored-by: Wide Glorg, Devourer of the Inhumane, Herald of Absorption Co-authored-by: Peter Edwards Co-authored-by: cjel4 --- src/main/java/frc/robot/commands/SwerveCommand.java | 1 + src/main/java/frc/robot/subsystems/SwerveSubsystem.java | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/commands/SwerveCommand.java b/src/main/java/frc/robot/commands/SwerveCommand.java index 9bb89c0..8e528db 100644 --- a/src/main/java/frc/robot/commands/SwerveCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommand.java @@ -63,6 +63,7 @@ public void execute() { // } // System.out.print("\n"); // + } diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 689500c..2177169 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.DriveConstants; +import frc.robot.helpers.Crashboard; public class SwerveSubsystem extends SubsystemBase { public static final double MAX_VOLTAGE = 12.0; @@ -104,6 +105,7 @@ public void periodic() { frontRight.logIt(); backLeft.logIt(); backRight.logIt(); + Crashboard.toDashboard("gyro angle", gyro.getAngle(), "navx"); // SmartDashboard.putNumber("Front Right Wheel Angle", frontRight.getAbsoluteEncoderDeg()); // SmartDashboard.putNumber("Back Left Wheel Angle", backLeft.getAbsoluteEncoderDeg()); // SmartDashboard.putNumber("Back Right Wheel Angle", backRight.getAbsoluteEncoderDeg()); From c1de77498054921c43430097f474b4567071ead7 Mon Sep 17 00:00:00 2001 From: JingerJesus Date: Sat, 2 Dec 2023 18:27:19 -0500 Subject: [PATCH 15/20] Push from turning. pain. Co-authored-by: Goldrn Co-authored-by: Peter Edwards Co-authored-by: cjel4 --- .../frc/robot/commands/SwerveCommand.java | 16 +++++++++++----- .../frc/robot/subsystems/SwerveSubsystem.java | 19 +++++++++++-------- 2 files changed, 22 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/commands/SwerveCommand.java b/src/main/java/frc/robot/commands/SwerveCommand.java index 8e528db..206fbdb 100644 --- a/src/main/java/frc/robot/commands/SwerveCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommand.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.OIConstants; +import frc.robot.helpers.Crashboard; import frc.robot.subsystems.SwerveSubsystem; public class SwerveCommand extends CommandBase { @@ -49,14 +50,19 @@ public void execute() { double ySpeed = Math.abs(controller.getLeftX()) > OIConstants.kDeadband ? controller.getLeftX() : 0.0; ySpeed = ySpeed * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; - double turningSpeed = controller.getRightX() * DriveConstants.kTeleDriveMaxAngularSpeedDegreesPerSecond; - turningSpeed = Math.abs(turningSpeed) > OIConstants.kDeadband ? turningSpeed : 0.0; - turningSpeed = 0; + double turningSpeed = Math.abs(controller.getRightX()) > OIConstants.kDeadband ? controller.getRightX() : 0.0; + turningSpeed = turningSpeed * DriveConstants.kTeleDriveMaxAngularSpeedDegreesPerSecond; + Crashboard.toDashboard("kTeleDriveMaxAngularSpeedDegreesPerSecond", DriveConstants.kTeleDriveMaxAngularSpeedDegreesPerSecond, "navx"); + //turningSpeed = Math.abs(turningSpeed) > OIConstants.kDeadband ? turningSpeed : 0.0; + //turningSpeed = 0; System.out.println("xSpeed: " + xSpeed + " ySpeed: " + ySpeed + " turningSpeed: " + turningSpeed); - - ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeed, Rotation2d.fromDegrees(swerveSubsystem.getHeading())); + double turningSpeedRadiansPerSecond = Rotation2d.fromDegrees(turningSpeed).getRadians(); + Rotation2d currentHeading = Rotation2d.fromDegrees(swerveSubsystem.getHeading()); + ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeedRadiansPerSecond, currentHeading); swerveSubsystem.setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds)); + Crashboard.toDashboard("turningSpeedRadiansPerSecond", turningSpeedRadiansPerSecond, "navx"); + Crashboard.toDashboard("currentHeading", currentHeading.getRadians(), "navx"); // this.swerveSubsystem.setModuleStates(new SwerveModuleState[] {new SwerveModuleState(0.5, new Rotation2d(0.1)), new SwerveModuleState(0.5, new Rotation2d(0.1)), new SwerveModuleState(0.5, new Rotation2d(0.1)), new SwerveModuleState(.5, new Rotation2d( 0.1))}); //for (int i = 0; i < this.swerveSubsystem.getModuleStates().length; i ++) { // System.out.print(this.swerveSubsystem.getModuleStates()[i] + ", " ); diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 2177169..24b72ef 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -67,6 +67,8 @@ public SwerveSubsystem() { odometer = new SwerveDriveOdometry(DriveConstants.kDriveKinematics, getRotation2d(), new SwerveModulePosition[] { frontLeft.getPosition(), frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()}); + + new Thread(() -> { try { @@ -96,23 +98,24 @@ public Pose2d getPose() { @Override public void periodic() { - // odometer.update(getRotation2d(), new SwerveModulePosition[] { - // frontLeft.getPosition(), frontRight.getPosition(), - // backLeft.getPosition(), backRight.getPosition() - // }); - //SmartDashboard.putNumber("Robot Heading", getHeading()); + odometer.update(getRotation2d(), new SwerveModulePosition[] { + frontLeft.getPosition(), frontRight.getPosition(), + backLeft.getPosition(), backRight.getPosition() + }); + Crashboard.toDashboard("Robot Heading", getHeading(), "navx"); frontLeft.logIt(); frontRight.logIt(); backLeft.logIt(); backRight.logIt(); Crashboard.toDashboard("gyro angle", gyro.getAngle(), "navx"); - // SmartDashboard.putNumber("Front Right Wheel Angle", frontRight.getAbsoluteEncoderDeg()); - // SmartDashboard.putNumber("Back Left Wheel Angle", backLeft.getAbsoluteEncoderDeg()); - // SmartDashboard.putNumber("Back Right Wheel Angle", backRight.getAbsoluteEncoderDeg()); + //SmartDashboard.putNumber("Front Right Wheel Angle", frontRight.getAbsoluteEncoderDeg()); + //SmartDashboard.putNumber("Back Left Wheel Angle", backLeft.getAbsoluteEncoderDeg()); + //SmartDashboard.putNumber("Back Right Wheel Angle", backRight.getAbsoluteEncoderDeg()); //SmartDashboard.putString("Robot Location", getPose().getTranslation().toString()); } public void stopModules() { + frontLeft.stop(); frontRight.stop(); backLeft.stop(); From 70ddf3f9899a10753d8a8f70fa414d2c3d6a23de Mon Sep 17 00:00:00 2001 From: JingerJesus Date: Fri, 8 Dec 2023 00:50:22 -0500 Subject: [PATCH 16/20] IT LIVES!!!! Co-authored-by: cjel4 Co-authored-by: Wevan Co-authored-by: Bionical-X Co-authored-by: Goldrn --- .../java/frc/robot/commands/SwerveCommand.java | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/SwerveCommand.java b/src/main/java/frc/robot/commands/SwerveCommand.java index 206fbdb..5b70319 100644 --- a/src/main/java/frc/robot/commands/SwerveCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommand.java @@ -58,11 +58,12 @@ public void execute() { System.out.println("xSpeed: " + xSpeed + " ySpeed: " + ySpeed + " turningSpeed: " + turningSpeed); double turningSpeedRadiansPerSecond = Rotation2d.fromDegrees(turningSpeed).getRadians(); - Rotation2d currentHeading = Rotation2d.fromDegrees(swerveSubsystem.getHeading()); + Rotation2d currentHeading = Rotation2d.fromDegrees(-swerveSubsystem.getHeading()); //inverted ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeedRadiansPerSecond, currentHeading); swerveSubsystem.setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds)); Crashboard.toDashboard("turningSpeedRadiansPerSecond", turningSpeedRadiansPerSecond, "navx"); Crashboard.toDashboard("currentHeading", currentHeading.getRadians(), "navx"); + //Crashboard.toDashboard("desired states", DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds)[0].toString(), "navx"); // this.swerveSubsystem.setModuleStates(new SwerveModuleState[] {new SwerveModuleState(0.5, new Rotation2d(0.1)), new SwerveModuleState(0.5, new Rotation2d(0.1)), new SwerveModuleState(0.5, new Rotation2d(0.1)), new SwerveModuleState(.5, new Rotation2d( 0.1))}); //for (int i = 0; i < this.swerveSubsystem.getModuleStates().length; i ++) { // System.out.print(this.swerveSubsystem.getModuleStates()[i] + ", " ); @@ -73,6 +74,19 @@ public void execute() { } + + public void getAngle() + { + double angle = Math.atan(controller.getRightY()/controller.getRightX()); + if(controller.getRightY() > 0) + { + return angle; + } + else{ + return angle + 90; + } + } + @Override public void end(boolean interrupted) { swerveSubsystem.stopModules(); From de45c14ec4f5b92ee457533764d34362da613084 Mon Sep 17 00:00:00 2001 From: JingerJesus Date: Fri, 8 Dec 2023 00:50:25 -0500 Subject: [PATCH 17/20] IT LIVES --- src/main/java/frc/robot/Constants.java | 14 +++++++------- src/main/java/frc/robot/helpers/Crashboard.java | 4 ++++ .../java/frc/robot/subsystems/SwerveModule.java | 5 +++-- 3 files changed, 14 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 65ac837..65a5c8a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -39,10 +39,10 @@ public static final class DriveConstants { //This should be relative to the center, but still check documentation about how the grid is set up for swerve kinetics //Ive changed it. It looked wrong and I fixed it based on the coordinate system at https://hhs-team670.github.io/MustangLib/frc/team670/robot/utils/math/Translation2d.html public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( - new Translation2d(-kWheelBase / 2, kTrackWidth / 2), //front left - new Translation2d(kWheelBase / 2, kTrackWidth / 2), //front right - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2), //back left - new Translation2d(kWheelBase / 2, -kTrackWidth / 2)); //backright + new Translation2d(kWheelBase / 2, kTrackWidth / 2), //front left + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), //front right + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), //back left + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); //backright //We need to update these motors // frontLeft Module @@ -50,7 +50,7 @@ public static final class DriveConstants { public static final int kFrontLeftDriveMotorPort = 57; //motors updated public static final int kFrontLeftTurningMotorPort = 56; //motors updated public static final boolean kFrontLeftDriveMotorReversed = true; //re updated //updated - public static final boolean kFrontLeftTurningMotorReversed = true; //updated + public static final boolean kFrontLeftTurningMotorReversed = true; //updated public static final int kFrontLeftDriveAbsoluteEncoderPort = 30; //updated public static final boolean kFrontLeftDriveAbsoluteEncoderReversed = false; //updated public static final double kFrontLeftDriveAbsoluteEncoderOffsetDeg = -0.134521; //updated, in degrees // they want this to be the negative of the reported values? @@ -97,8 +97,8 @@ public static final class DriveConstants { //If you want to ignore these then change the limit on the max speeds manually //these seem to be mostly fine but we may need change some things // also we need to change the physical dimensions of our base if we are going to use this - public static final double kPhysicalMaxSpeedMetersPerSecond = 6380.0 / 60.0 * (ModuleConstants.kDriveMotorGearRatio) * ModuleConstants.kWheelDiameterMeters * 360; - public static final double kPhysicalMaxAngularSpeedDegreesPerSecond = kPhysicalMaxSpeedMetersPerSecond / Math.hypot(DriveConstants.kTrackWidth / 2.0, DriveConstants.kWheelBase / 2.0 * 3); + public static final double kPhysicalMaxSpeedMetersPerSecond = 5676.0 / 60.0 * ModuleConstants.kDriveEncoderRot2Meter; + public static final double kPhysicalMaxAngularSpeedDegreesPerSecond = 360; //2 * Math.PI; //kPhysicalMaxSpeedMetersPerSecond / Math.hypot(DriveConstants.kTrackWidth / 2.0, DriveConstants.kWheelBase / 2.0 * 3); //These are the variables that determine the max speeds of our swerve drive public static final double kTeleDriveMaxSpeedMetersPerSecond = kPhysicalMaxSpeedMetersPerSecond / 4; diff --git a/src/main/java/frc/robot/helpers/Crashboard.java b/src/main/java/frc/robot/helpers/Crashboard.java index 2848357..17df4eb 100644 --- a/src/main/java/frc/robot/helpers/Crashboard.java +++ b/src/main/java/frc/robot/helpers/Crashboard.java @@ -64,6 +64,10 @@ public static GenericEntry toDashboard(String identifier, Sendable value, String return toDashboardGeneric(identifier, value, tab); } + public static GenericEntry toDashboard(String identifier, String value, String tab) { + return toDashboardGeneric(identifier, value, tab); + } + public static double clamp(double in, double min, double max) { if (in < min) diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 9bc2635..83eafa9 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import frc.robot.Constants; import frc.robot.Constants.ModuleConstants; import frc.robot.helpers.Crashboard; @@ -110,8 +111,8 @@ public void setDesiredState(SwerveModuleState state) { } state = SwerveModuleState.optimize(state, getState().angle); - // Calculate the drive output from the drive PID controller. - double driveSpeed = MathUtil.clamp(state.speedMetersPerSecond / 360, -.5 ,.5); + // Calculate the drive output from the drive PID controller. ;} + double driveSpeed = MathUtil.clamp(state.speedMetersPerSecond / Constants.DriveConstants.kTeleDriveMaxSpeedMetersPerSecond/* / 360*/, -.5 ,.5); driveMotor.set(driveSpeed); Crashboard.toDashboard("driveSpeed", driveSpeed, "Swerve"); From c66662b16c3bc82589120485b65b5d6c1ed4e578 Mon Sep 17 00:00:00 2001 From: JingerJesus Date: Sun, 17 Dec 2023 22:23:40 -0500 Subject: [PATCH 18/20] Absolute turning finished. Started work on PID characterization and implementation. Co-authored-by: Wevan Co-authored-by: cjel4 Co-authored-by: Bionical-X Co-authored-by: Goldrn --- src/main/java/frc/robot/Constants.java | 9 +++ src/main/java/frc/robot/RobotContainer.java | 4 +- .../frc/robot/commands/SwerveCommand.java | 79 +++++++++++++++---- .../frc/robot/subsystems/SwerveModule.java | 27 ++++++- 4 files changed, 101 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 65a5c8a..4c5ff41 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -36,6 +36,15 @@ public static final class DriveConstants { public static final double kTrackWidth = Units.inchesToMeters(21); public static final double kWheelBase = Units.inchesToMeters(21); + //PID constants + public static final double Ks = 0.10729; + public static final double Kv = 0.85456; + public static final double Ka = 0.047848; + + public static final double kP = 0.001; + public static final double kI = 0; + public static final double kD = 0; + //This should be relative to the center, but still check documentation about how the grid is set up for swerve kinetics //Ive changed it. It looked wrong and I fixed it based on the coordinate system at https://hhs-team670.github.io/MustangLib/frc/team670/robot/utils/math/Translation2d.html public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3fe1f5e..5035a15 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,9 +9,11 @@ public class RobotContainer { - private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem();; + private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); private final XboxController driverOne = new XboxController(0); + public static final boolean ABSOLUTE_TURNING_MODE = true; + public RobotContainer() { swerveSubsystem.setDefaultCommand(new SwerveCommand(swerveSubsystem, driverOne)); configureButtonBindings(); diff --git a/src/main/java/frc/robot/commands/SwerveCommand.java b/src/main/java/frc/robot/commands/SwerveCommand.java index 5b70319..463289e 100644 --- a/src/main/java/frc/robot/commands/SwerveCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommand.java @@ -10,6 +10,8 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.RobotContainer; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.OIConstants; import frc.robot.helpers.Crashboard; @@ -37,10 +39,14 @@ public void initialize() { @Override public void execute() { + //testing + Crashboard.toDashboard("updatedControllerAngle", getHeadingFromController(controller), "controllerAngle"); + Crashboard.toDashboard("x", controller.getRightX(), "controllerAngle"); + Crashboard.toDashboard("y", controller.getRightY(), "controllerAngle"); // :3 // - System.out.println("ANGLE = " + swerveSubsystem.frontLeft.getAbsoluteEncoderDeg() + ", SPEED = " + FrontLeftTurnSpark.get()); + //System.out.println("ANGLE = " + swerveSubsystem.frontLeft.getAbsoluteEncoderDeg() + ", SPEED = " + FrontLeftTurnSpark.get()); // \:3 @@ -57,9 +63,33 @@ public void execute() { //turningSpeed = 0; System.out.println("xSpeed: " + xSpeed + " ySpeed: " + ySpeed + " turningSpeed: " + turningSpeed); - double turningSpeedRadiansPerSecond = Rotation2d.fromDegrees(turningSpeed).getRadians(); - Rotation2d currentHeading = Rotation2d.fromDegrees(-swerveSubsystem.getHeading()); //inverted + + Rotation2d currentHeading = Rotation2d.fromDegrees(swerveSubsystem.getHeading()); //inverted + Rotation2d targetHeading = Rotation2d.fromRadians(getHeadingFromController(controller)); + double turningSpeedRadiansPerSecond; + + if (RobotContainer.ABSOLUTE_TURNING_MODE) { + turningSpeedRadiansPerSecond = 0; + double deadzoneRadians = 0.1; + + if (currentHeading.getDegrees() < targetHeading.getDegrees()) turningSpeedRadiansPerSecond = 0.2; + + if (currentHeading.getDegrees() > targetHeading.getDegrees()) turningSpeedRadiansPerSecond = -0.2; + + double offset = currentHeading.getDegrees() - targetHeading.getDegrees(); + + if (Math.abs(offset) <= deadzoneRadians) { + turningSpeedRadiansPerSecond = 0; + } + + } else { + turningSpeedRadiansPerSecond = Rotation2d.fromDegrees(turningSpeed).getRadians(); + } + + + ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeedRadiansPerSecond, currentHeading); + swerveSubsystem.setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds)); Crashboard.toDashboard("turningSpeedRadiansPerSecond", turningSpeedRadiansPerSecond, "navx"); Crashboard.toDashboard("currentHeading", currentHeading.getRadians(), "navx"); @@ -74,19 +104,6 @@ public void execute() { } - - public void getAngle() - { - double angle = Math.atan(controller.getRightY()/controller.getRightX()); - if(controller.getRightY() > 0) - { - return angle; - } - else{ - return angle + 90; - } - } - @Override public void end(boolean interrupted) { swerveSubsystem.stopModules(); @@ -96,4 +113,34 @@ public void end(boolean interrupted) { public boolean isFinished() { return false; } + + private double getHeadingFromController(XboxController c) { + + double deadzone = 0.05; + + double x = c.getRightX(); + double y = -c.getRightY(); + + if (Math.abs(x) < deadzone) { + x = 0; + } + if (Math.abs(y) < deadzone) { + y = 0; + } + + if (x == 0 && y == 0) return Math.toRadians(swerveSubsystem.getHeading()); + + + int holder = 1; + + double angle = -1 * Math.atan(x/y); + if (y < 0) { + if (x < 0) + { + holder = -1; + } + angle -= holder * Math.PI; + } + return -angle; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 83eafa9..bb783c3 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import frc.robot.Constants; +import frc.robot.Constants.DriveConstants; import frc.robot.Constants.ModuleConstants; import frc.robot.helpers.Crashboard; @@ -31,6 +32,10 @@ public class SwerveModule { private final RelativeEncoder driveEncoder; //private final RelativeEncoder turningEncoder; + private final SimpleMotorFeedforward driveFeedForward = new SimpleMotorFeedforward(DriveConstants.Ks, DriveConstants.Kv); + + private final PIDController drivePIDController = new PIDController(DriveConstants.kP, DriveConstants.kI, DriveConstants.kD); + private final PIDController turningPidController; private final PIDController drivePidController; @@ -104,6 +109,14 @@ public SwerveModuleState getState() { } public void setDesiredState(SwerveModuleState state) { + + final double driveOutput = drivePIDController.calculate(driveEncoder.getVelocity(), state.speedMetersPerSecond); // Convert to m/s + + final double driveFeedForwardOutput = driveFeedForward.calculate(state.speedMetersPerSecond); + + state.speedMetersPerSecond *= state.angle.minus(Rotation2d.fromDegrees(getAbsoluteEncoderDeg())).getCos(); + + if (Math.abs(state.speedMetersPerSecond) < 0.001) { stop(); @@ -114,7 +127,7 @@ public void setDesiredState(SwerveModuleState state) { // Calculate the drive output from the drive PID controller. ;} double driveSpeed = MathUtil.clamp(state.speedMetersPerSecond / Constants.DriveConstants.kTeleDriveMaxSpeedMetersPerSecond/* / 360*/, -.5 ,.5); - driveMotor.set(driveSpeed); + //driveMotor.set(driveSpeed); Crashboard.toDashboard("driveSpeed", driveSpeed, "Swerve"); Crashboard.toDashboard("speed in m/s", state.speedMetersPerSecond, "Swerve"); @@ -130,6 +143,18 @@ public void setDesiredState(SwerveModuleState state) { Crashboard.toDashboard(ModuleName + "Turn Speed Final", turnSpeed, "Swerve"); turningMotor.set(turnSpeed); + + double outVoltage = driveOutput + driveFeedForwardOutput; + Crashboard.toDashboard("drive voltage PID", driveOutput, this.ModuleName + " Swerve"); + Crashboard.toDashboard("drive voltage ff", driveFeedForwardOutput, this.ModuleName + " Swerve"); + Crashboard.toDashboard("output voltage", outVoltage, this.ModuleName + " Swerve"); + + + outVoltage = MathUtil.clamp(outVoltage, -5, 5); + + Crashboard.toDashboard("clamp voltage", outVoltage, this.ModuleName + " Swerve"); + + driveMotor.setVoltage(outVoltage); //System.out.println(ModuleName + "- DriveMotorCommand: " + driveSpeed + " - True Angle: " + getAbsoluteEncoderRad() + " AngleSetPoint: " + state.angle.getDegrees() + " AngleMotorCommand: " + turnSpeed); } From 32dfad9dcac6df7098a9fe08633c1cb27f4a745a Mon Sep 17 00:00:00 2001 From: JingerJesus Date: Fri, 5 Jan 2024 01:55:58 -0500 Subject: [PATCH 19/20] not sure! Co-authored-by: Goldrn Co-authored-by: cjel4 Co-authored-by: Peter Edwards Co-authored-by: Wevan --- .../frc/robot/subsystems/SwerveModule.java | 38 +++++++++++-------- 1 file changed, 22 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index bb783c3..fdb4da4 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -64,8 +64,8 @@ public SwerveModule(String ModuleName, int driveMotorId, int turningMotorId, boo driveEncoder = driveMotor.getEncoder(); //turningEncoder = turningMotor.getEncoder(); - driveEncoder.setPositionConversionFactor(ModuleConstants.kDriveEncoderRot2Meter); - driveEncoder.setVelocityConversionFactor(ModuleConstants.kDriveEncoderRPM2MeterPerSec); + //driveEncoder.setPositionConversionFactor(ModuleConstants.kDriveEncoderRot2Meter); + //driveEncoder.setVelocityConversionFactor(ModuleConstants.kDriveEncoderRPM2MeterPerSec); //turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderRot2Deg); //turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderRPM2DegPerSec); @@ -90,7 +90,7 @@ public SwerveModulePosition getPosition() { } public double getDriveVelocity() { - return driveEncoder.getVelocity(); + return driveEncoder.getVelocity() * ModuleConstants.kDriveEncoderRPM2MeterPerSec; } public double getAbsoluteEncoderDeg() { @@ -110,13 +110,15 @@ public SwerveModuleState getState() { public void setDesiredState(SwerveModuleState state) { - final double driveOutput = drivePIDController.calculate(driveEncoder.getVelocity(), state.speedMetersPerSecond); // Convert to m/s - - final double driveFeedForwardOutput = driveFeedForward.calculate(state.speedMetersPerSecond); - state.speedMetersPerSecond *= state.angle.minus(Rotation2d.fromDegrees(getAbsoluteEncoderDeg())).getCos(); - + Crashboard.toDashboard("driveVelocity", getDriveVelocity(), this.ModuleName + " Swerve"); + Crashboard.toDashboard("driveposition", this.driveEncoder.getPosition(), this.ModuleName + " Swerve"); + + double driveOutput = drivePIDController.calculate(driveEncoder.getVelocity(), state.speedMetersPerSecond); // Convert to m/s + //Crashboard.toDashboard(this.ModuleName + " driveSpeed", driveSpeed, "Swerve"); + + final double driveFeedForwardOutput = driveFeedForward.calculate(state.speedMetersPerSecond); if (Math.abs(state.speedMetersPerSecond) < 0.001) { stop(); @@ -128,31 +130,32 @@ public void setDesiredState(SwerveModuleState state) { double driveSpeed = MathUtil.clamp(state.speedMetersPerSecond / Constants.DriveConstants.kTeleDriveMaxSpeedMetersPerSecond/* / 360*/, -.5 ,.5); //driveMotor.set(driveSpeed); - Crashboard.toDashboard("driveSpeed", driveSpeed, "Swerve"); - Crashboard.toDashboard("speed in m/s", state.speedMetersPerSecond, "Swerve"); + Crashboard.toDashboard(this.ModuleName + " driveSpeed", driveSpeed, "Swerve"); + Crashboard.toDashboard(this.ModuleName + " speed in mps", state.speedMetersPerSecond, "Swerve"); double turnSpeed = (turningPidController.calculate(getAbsoluteEncoderDeg(), state.angle.getDegrees())); - System.out.println("Turn Speed Calculated " + this.ModuleName + ": " + turnSpeed); + //System.out.println("Turn Speed Calculated " + this.ModuleName + ": " + turnSpeed); if (turnSpeed > 0) turnSpeed = Math.min(turnSpeed, .2); else turnSpeed = Math.max(turnSpeed, -.2); - System.out.println("Turn Speed Final " + this.ModuleName + ": " + turnSpeed); + //System.out.println("Turn Speed Final " + this.ModuleName + ": " + turnSpeed); Crashboard.toDashboard(ModuleName + "Turn Speed Final", turnSpeed, "Swerve"); turningMotor.set(turnSpeed); - double outVoltage = driveOutput + driveFeedForwardOutput; + double outVoltage = driveOutput;// + driveFeedForwardOutput; + outVoltage = MathUtil.clamp(outVoltage, -4.0 ,4.0); Crashboard.toDashboard("drive voltage PID", driveOutput, this.ModuleName + " Swerve"); Crashboard.toDashboard("drive voltage ff", driveFeedForwardOutput, this.ModuleName + " Swerve"); - Crashboard.toDashboard("output voltage", outVoltage, this.ModuleName + " Swerve"); + Crashboard.toDashboard(" output voltage", outVoltage, this.ModuleName + " Swerve"); - outVoltage = MathUtil.clamp(outVoltage, -5, 5); + //outVoltage = MathUtil.clamp(outVoltage, -5, 5); - Crashboard.toDashboard("clamp voltage", outVoltage, this.ModuleName + " Swerve"); + Crashboard.toDashboard(this.ModuleName + " clamp voltage", outVoltage, "Swerve"); driveMotor.setVoltage(outVoltage); //System.out.println(ModuleName + "- DriveMotorCommand: " + driveSpeed + " - True Angle: " + getAbsoluteEncoderRad() + " AngleSetPoint: " + state.angle.getDegrees() + " AngleMotorCommand: " + turnSpeed); @@ -169,5 +172,8 @@ public CANcoder getCANcoder() { public void logIt() { Crashboard.toDashboard(ModuleName + " Wheel Angle", this.getAbsoluteEncoderDeg(), "swerve"); + + + } } \ No newline at end of file From c4399a8f321e9b6f5b0b82be04227e00ad4eca0d Mon Sep 17 00:00:00 2001 From: JingerJesus Date: Fri, 5 Jan 2024 01:56:02 -0500 Subject: [PATCH 20/20] No idea, hopefully something works --- src/main/java/frc/robot/Constants.java | 2 +- .../java/frc/robot/commands/SwerveCommand.java | 15 ++++++++++++--- .../frc/robot/subsystems/SwerveSubsystem.java | 2 ++ 3 files changed, 15 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4c5ff41..d73b627 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,7 +41,7 @@ public static final class DriveConstants { public static final double Kv = 0.85456; public static final double Ka = 0.047848; - public static final double kP = 0.001; + public static final double kP = 0.025; public static final double kI = 0; public static final double kD = 0; diff --git a/src/main/java/frc/robot/commands/SwerveCommand.java b/src/main/java/frc/robot/commands/SwerveCommand.java index 463289e..9da64a6 100644 --- a/src/main/java/frc/robot/commands/SwerveCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommand.java @@ -41,8 +41,10 @@ public void execute() { //testing Crashboard.toDashboard("updatedControllerAngle", getHeadingFromController(controller), "controllerAngle"); - Crashboard.toDashboard("x", controller.getRightX(), "controllerAngle"); - Crashboard.toDashboard("y", controller.getRightY(), "controllerAngle"); + Crashboard.toDashboard("Rightx", controller.getRightX(), "controllerAngle"); + Crashboard.toDashboard("Righty", controller.getRightY(), "controllerAngle"); + Crashboard.toDashboard("Leftx", controller.getLeftX(), "controllerAngle"); + Crashboard.toDashboard("Lefty", controller.getLeftY(), "controllerAngle"); // :3 // @@ -62,6 +64,9 @@ public void execute() { //turningSpeed = Math.abs(turningSpeed) > OIConstants.kDeadband ? turningSpeed : 0.0; //turningSpeed = 0; System.out.println("xSpeed: " + xSpeed + " ySpeed: " + ySpeed + " turningSpeed: " + turningSpeed); + Crashboard.toDashboard("xSpeed", xSpeed, "Swerve"); + Crashboard.toDashboard("ySpeed", ySpeed, "Swerve"); + Crashboard.toDashboard("kTeleDriveMaxSpeedMetersPerSecond", DriveConstants.kTeleDriveMaxSpeedMetersPerSecond, "Swerve"); Rotation2d currentHeading = Rotation2d.fromDegrees(swerveSubsystem.getHeading()); //inverted @@ -89,8 +94,12 @@ public void execute() { ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeedRadiansPerSecond, currentHeading); - + + Crashboard.toDashboard("Chassisspeed X", chassisSpeeds.vxMetersPerSecond, "Swerve 2"); + Crashboard.toDashboard("Chassisspeed Y", chassisSpeeds.vyMetersPerSecond, "Swerve 2"); + swerveSubsystem.setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds)); + Crashboard.toDashboard("turningSpeedRadiansPerSecond", turningSpeedRadiansPerSecond, "navx"); Crashboard.toDashboard("currentHeading", currentHeading.getRadians(), "navx"); //Crashboard.toDashboard("desired states", DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds)[0].toString(), "navx"); diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 24b72ef..9b40cff 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -128,6 +128,8 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { frontRight.setDesiredState(desiredStates[1]); backLeft.setDesiredState(desiredStates[2]); backRight.setDesiredState(desiredStates[3]); + + } public SwerveModuleState[] getModuleStates() {