From 6c9134bc6de7d05951d189e90971a1b1a025e9e3 Mon Sep 17 00:00:00 2001 From: Wispy Sparks Date: Thu, 24 Oct 2024 14:38:52 -0500 Subject: [PATCH] testing --- src/main/java/frc/robot/Robot.java | 5 ++++- src/main/java/frc/robot/subsystems/Swerve.java | 4 ++-- src/main/java/swervelib/SwerveModule.java | 3 +++ src/main/java/swervelib/motors/TalonSRXSwerve.java | 5 ++++- 4 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e55beb6..7f81d99 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,6 +6,8 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -33,10 +35,11 @@ public Robot() { // xboxController.getHID()::getBButton // ); + var slider = Shuffleboard.getTab("testing").add("Speed", 0).withWidget(BuiltInWidgets.kNumberSlider).getEntry(); // Fill in parameter info Command driveFieldOrientedAngularVelocity = swerve.driveCommand( () -> MathUtil.applyDeadband(-xboxController.getLeftY(), DriveK.leftJoystickDeadband), - () -> MathUtil.applyDeadband(-xboxController.getLeftX(), DriveK.leftJoystickDeadband), + () -> slider.getDouble(0), () -> MathUtil.applyDeadband(-xboxController.getRightX(), DriveK.rightJoystickDeadband) ); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 008e259..923e858 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -41,7 +41,7 @@ public class Swerve extends SubsystemBase { * @param directory Directory of swerve drive config files. */ public Swerve() { - double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(SwerveK.steerGearRatio); + double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(SwerveK.steerGearRatio, 4096); double driveConversionFactor = SwerveMath.calculateMetersPerRotation(SwerveK.driveGearRatio, SwerveK.wheelDiameter.in(Inches)); SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; @@ -95,7 +95,7 @@ public Command driveCommand(DoubleSupplier TranslationX, DoubleSupplier Translat return runOnce(() -> drive( new Translation2d(TranslationX.getAsDouble() * swerveDrive.getMaximumVelocity(), TranslationY.getAsDouble() * swerveDrive.getMaximumVelocity()), angularVelocity.getAsDouble() * swerveDrive.getMaximumAngularVelocity(), - true, true)); + false, true)); } public Command turnCommand(Measure targetAngle, Measure currentAngle, boolean fieldRelative) { diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 2cdbf44..e4a2b4e 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -2,6 +2,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.MotorFeedbackSensor; + import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -386,6 +387,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, if (absoluteEncoder != null && synchronizeEncoderQueued && synchronizeEncoderEnabled) { double absoluteEncoderPosition = getAbsolutePosition(); + // System.out.println(absoluteEncoderPosition); if(Math.abs(angleMotor.getPosition() - absoluteEncoderPosition) >= synchronizeEncoderDeadband) { angleMotor.setPosition(absoluteEncoderPosition); } @@ -393,6 +395,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, synchronizeEncoderQueued = false; } else { + // System.out.println("set reference + " + desiredState.angle.getDegrees()); angleMotor.setReference(desiredState.angle.getDegrees(), 0); } diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.java index 30db007..b1778ed 100644 --- a/src/main/java/swervelib/motors/TalonSRXSwerve.java +++ b/src/main/java/swervelib/motors/TalonSRXSwerve.java @@ -7,6 +7,7 @@ import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.math.SwerveMath; import swervelib.parser.PIDFConfig; @@ -265,6 +266,7 @@ public double convertToNativeSensorUnits(double setpoint, double position) { setpoint = isDriveMotor ? setpoint * .1 : SwerveMath.placeInAppropriate0To360Scope(position, setpoint); + System.out.println(motor.getDeviceID() + ": Setpoint: " + setpoint); return setpoint / positionConversionFactor; } @@ -292,7 +294,7 @@ public void setReference(double setpoint, double feedforward, double position) { burnFlash(); - + // System.out.println(motor.getDeviceID() + ": Setpoint: " + setpoint + " Position: " + position + " Native Setpoint: " + convertToNativeSensorUnits(setpoint, position)); motor.set( isDriveMotor ? ControlMode.Velocity : ControlMode.Position, convertToNativeSensorUnits(setpoint, position), @@ -363,6 +365,7 @@ public double getPosition() { pos += 360; } + // System.out.println(pos + " " + pos % 360); return pos; } }