testing

Armada2508 · Oct 24, 2024 · 6c9134b · 6c9134b
1 parent bfd8812
commit 6c9134b
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 4 deletions.
diff --git 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
@@ -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<Angle> targetAngle, Measure<Angle> currentAngle, boolean fieldRelative) {

diff --git 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,13 +387,15 @@ 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);
       }
       angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition);
       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
@@ -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;
     }
   }