calibrations

src/main/java/frc/robot/Robot.java

@@ -5,13 +5,19 @@
 package frc.robot;
 
 import com.pathplanner.lib.auto.NamedCommands;
+import com.spikes2212.util.PlaystationControllerWrapper;
+import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.commands.*;
 import frc.robot.subsystems.*;
 
 public class Robot extends TimedRobot {
 
+    private PlaystationControllerWrapper ps = new PlaystationControllerWrapper(0);
+
+    private Drivetrain drivetrain;
     private Elevator elevator;
     private Storage storage;
     private Gripper gripper;
@@ -20,8 +26,10 @@ public class Robot extends TimedRobot {
 
     @Override
     public void robotInit() {
-        getInstances();
-        registerNamedCommands();
+        drivetrain = Drivetrain.getInstance();
+        ps.getR1Button().onTrue(new InstantCommand(drivetrain::resetGyro));
+//        getInstances();
+//        registerNamedCommands();
     }
 
     @Override
@@ -41,7 +49,6 @@ public void disabledPeriodic() {
 
     @Override
     public void autonomousInit() {
-
     }
 
     @Override
@@ -51,7 +58,9 @@ public void autonomousPeriodic() {
 
     @Override
     public void teleopInit() {
-
+        drivetrain.resetRelativeEncoders();
+        drivetrain.setDefaultCommand(new Drive(drivetrain, () -> -ps.getLeftY(), () -> -ps.getLeftX(), () -> ps.getRightX() * 2,
+                true, false, false));
     }
 
     @Override

src/main/java/frc/robot/subsystems/AlgaeJoint.java

@@ -2,7 +2,7 @@
 
 import com.revrobotics.spark.SparkLowLevel;
 import com.spikes2212.command.genericsubsystem.smartmotorcontrollersubsystem.SmartMotorControllerGenericSubsystem;
-import com.spikes2212.util.smartmotorcontrollers.SparkWrapper;
+import frc.robot.util.SparkWrapper;
 import edu.wpi.first.wpilibj.DigitalInput;
 import frc.robot.RobotMap;
 

src/main/java/frc/robot/subsystems/CoralJoint.java

@@ -2,7 +2,7 @@
 
 import com.revrobotics.spark.SparkLowLevel;
 import com.spikes2212.command.genericsubsystem.smartmotorcontrollersubsystem.SmartMotorControllerGenericSubsystem;
-import com.spikes2212.util.smartmotorcontrollers.SparkWrapper;
+import frc.robot.util.SparkWrapper;
 import edu.wpi.first.wpilibj.DigitalInput;
 import frc.robot.RobotMap;
 

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -10,16 +10,20 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.*;
 import edu.wpi.first.wpilibj.DriverStation;
+import frc.robot.commands.Drive;
 import frc.robot.util.VisionService;
 
+import java.util.function.Supplier;
+
 public class Drivetrain extends DashboardedSubsystem {
 
     public static final double MAX_SPEED = 4;
-    public static final double MIN_SPEED = 0.05;
+    public static final double MIN_SPEED = 0.02;
     public static final double MAX_TURN_SPEED = 3;
 
     private static final double TRACK_WIDTH = 0.6;
     private static final double TRACK_LENGTH = 0.6;
+    private final Supplier<Double> timestep = namespace.addConstantDouble("timestep", 0);
 
     private static final Translation2d CENTER_OF_ROBOT = new Translation2d(0, 0);
     private static final Translation2d FRONT_LEFT_WHEEL_POSITION =
@@ -135,6 +139,7 @@ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fi
         } else {
             speeds = new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed);
         }
+        speeds = ChassisSpeeds.discretize(speeds, timestep.get());
         SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds, CENTER_OF_ROBOT);
         SwerveDriveKinematics.desaturateWheelSpeeds(states, MAX_SPEED);
         frontLeft.set(states[0], usePID);
@@ -186,5 +191,6 @@ public double getYaw() {
     @Override
     public void configureDashboard() {
         namespace.putNumber("gyro yaw", gyro::getAngle);
+        namespace.putCommand("check skew", new Drive(this, () -> 0.0, () -> 0.5, () -> 1.0, true, false, false));
     }
 }

src/main/java/frc/robot/subsystems/Elevator.java

@@ -2,7 +2,7 @@
 
 import com.revrobotics.spark.SparkLowLevel;
 import com.spikes2212.command.genericsubsystem.smartmotorcontrollersubsystem.SmartMotorControllerGenericSubsystem;
-import com.spikes2212.util.smartmotorcontrollers.SparkWrapper;
+import frc.robot.util.SparkWrapper;
 import edu.wpi.first.wpilibj.DigitalInput;
 import frc.robot.RobotMap;
 

src/main/java/frc/robot/subsystems/Storage.java

@@ -2,7 +2,7 @@
 
 import com.revrobotics.spark.SparkLowLevel;
 import com.spikes2212.command.genericsubsystem.MotoredGenericSubsystem;
-import com.spikes2212.util.smartmotorcontrollers.SparkWrapper;
+import frc.robot.util.SparkWrapper;
 import edu.wpi.first.wpilibj.DigitalInput;
 import frc.robot.RobotMap;
 

src/main/java/frc/robot/subsystems/SwerveModule.java

@@ -4,15 +4,17 @@
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.spikes2212.command.DashboardedSubsystem;
+import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;
+import com.spikes2212.dashboard.SpikesLogger;
 import com.spikes2212.util.UnifiedControlMode;
-import com.spikes2212.util.smartmotorcontrollers.SparkWrapper;
 import com.spikes2212.util.smartmotorcontrollers.TalonFXWrapper;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import frc.robot.util.SparkWrapper;
 
 public class SwerveModule extends DashboardedSubsystem {
 
@@ -39,6 +41,8 @@ public class SwerveModule extends DashboardedSubsystem {
     private final PIDSettings turnPIDSettings;
     private final FeedForwardSettings driveFeedForwardSettings;
     private final FeedForwardSettings turnFeedForwardSettings;
+    SpikesLogger logger = new SpikesLogger();
+
 
     public SwerveModule(String namespace, TalonFXWrapper driveMotor, SparkWrapper turnMotor, CANcoder absoluteEncoder,
                         boolean cancoderInverted, boolean driveInverted, double offset,
@@ -56,6 +60,7 @@ public SwerveModule(String namespace, TalonFXWrapper driveMotor, SparkWrapper tu
         this.turnPIDSettings = turnPIDSettings;
         this.driveFeedForwardSettings = driveFeedForwardSettings;
         this.turnFeedForwardSettings = turnFeedForwardSettings;
+        turnMotor.setCurrentLimit(40);
         configureDriveController();
         configureTurnController();
         configureAbsoluteEncoder();
@@ -91,25 +96,29 @@ private void setSpeed(double speed, boolean usePID) {
     }
 
     private void setAngle(double angle) {
-        turnMotor.pidSet(UnifiedControlMode.POSITION, angle, turnPIDSettings, turnFeedForwardSettings,
-                true);
+        turnMotor.pidSet(UnifiedControlMode.POSITION, angle, turnPIDSettings, turnFeedForwardSettings, true);
     }
 
     public void stop() {
         driveMotor.stopMotor();
         turnMotor.stopMotor();
     }
 
-    public void set(SwerveModuleState state, boolean usePID) {
-        if (Math.abs(state.speedMetersPerSecond) < Drivetrain.MIN_SPEED) {
+    public void set(SwerveModuleState state, boolean usePID, boolean limitSpeed) {
+        if (Math.abs(state.speedMetersPerSecond) < Drivetrain.MIN_SPEED && limitSpeed) {
             stop();
             return;
         }
         state = optimize(state, turnMotor.getPosition());
+//        state.speedMetersPerSecond *= state.angle.minus(Rotation2d.fromDegrees(turnMotor.getPosition())).getCos();
         setAngle(state.angle.getDegrees());
         setSpeed(state.speedMetersPerSecond, usePID);
     }
 
+    public void set(SwerveModuleState state, boolean usePID) {
+        set(state, usePID, true);
+    }
+
     private double normalizeAngleRelativeToEncoder(double currentAngle, double desiredAngle) {
         int rotations = (int) (currentAngle / DEGREES_IN_ROTATIONS);
         if (currentAngle < 0) rotations--;
@@ -152,5 +161,26 @@ public SwerveModuleState getState() {
     public void configureDashboard() {
         namespace.putNumber("absolute angle", this::getAbsoluteAngle);
         namespace.putNumber("relative angle", turnMotor::getPosition);
+        namespace.putCommand("set angle to 0", new RunCommand(() -> setAngle(0)) {
+            @Override
+            public void end(boolean fuckYou) {
+                turnMotor.stopMotor();
+            }
+        });
+        namespace.putCommand("rotate", new RunCommand(() -> {
+            turnMotor.set(0.2);
+        }) {
+            @Override
+            public void end(boolean fuckYou) {
+                turnMotor.stopMotor();
+            }
+        });
+        namespace.putNumber("velocity", driveMotor::getVelocity);
+        namespace.putCommand("move", new RunCommand(() -> driveMotor.set(0.1)) {
+            @Override
+            public void end(boolean fuckYou) {
+                driveMotor.stopMotor();
+            }
+        });
     }
 }

src/main/java/frc/robot/subsystems/SwerveModuleHolder.java

@@ -6,7 +6,7 @@
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;
 import com.spikes2212.dashboard.RootNamespace;
-import com.spikes2212.util.smartmotorcontrollers.SparkWrapper;
+import frc.robot.util.SparkWrapper;
 import com.spikes2212.util.smartmotorcontrollers.TalonFXWrapper;
 import frc.robot.RobotMap;
 
@@ -15,8 +15,8 @@ public class SwerveModuleHolder {
     private static final RootNamespace namespace = new RootNamespace("swerve module holder");
 
     private static final boolean FRONT_LEFT_DRIVE_INVERTED = false;
-    private static final boolean FRONT_RIGHT_DRIVE_INVERTED = false;
-    private static final boolean BACK_LEFT_DRIVE_INVERTED = false;
+    private static final boolean FRONT_RIGHT_DRIVE_INVERTED = true;
+    private static final boolean BACK_LEFT_DRIVE_INVERTED = true;
     private static final boolean BACK_RIGHT_DRIVE_INVERTED = false;
 
     private static final boolean FRONT_LEFT_CANCODER_INVERTED = false;
@@ -29,10 +29,10 @@ public class SwerveModuleHolder {
     private static final String BACK_LEFT_NAMESPACE_NAME = "back left";
     private static final String BACK_RIGHT_NAMESPACE_NAME = "back right";
 
-    private static final double FRONT_LEFT_OFFSET = 0;
-    private static final double FRONT_RIGHT_OFFSET = 0;
-    private static final double BACK_LEFT_OFFSET = 0;
-    private static final double BACK_RIGHT_OFFSET = 0;
+    private static final double FRONT_LEFT_OFFSET = -0.164;
+    private static final double FRONT_RIGHT_OFFSET = -0.973;
+    private static final double BACK_LEFT_OFFSET = -0.574;
+    private static final double BACK_RIGHT_OFFSET = -0.972;
 
     private static final PIDSettings drivePIDSettings = namespace.addPIDNamespace("drive");
     private static final PIDSettings turnPIDSettings = namespace.addPIDNamespace("turn");