Merge branch 'master' into photon-and-pathplanner-Overhaul-Swerve

src/main/java/frc4388/robot/Constants.java

@@ -51,6 +51,7 @@ import edu.wpi.first.units.measure.Distance;
 import frc4388.utility.CanDevice;
 import frc4388.utility.Gains;
 import frc4388.utility.LEDPatterns;
+import frc4388.utility.Trim;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -98,50 +99,52 @@ public final class Constants {
         public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084;
         public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
 
-        public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270
+        public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270
+
+        public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10);
 
         private static final class ModuleSpecificConstants {
             //Front Left
-            private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125);
+            private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25);
             private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
             private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
             private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
-            private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH);
+            private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH);
             private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
             
             //Front Right
-            private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625);
+            private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25);
             private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false;
             private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
             private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
             private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
-            private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
+            private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT);
 
             //Back Left
-            private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5);
+            private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5);
             private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
             private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
             private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
             private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH);
-            private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
+            private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT);
             
             //Back Right
-            private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5);
+            private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25);
             private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
             private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
             private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
-            private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH);
+            private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH);
             private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
         }
 
         public static final class IDs {
-            public static final CanDevice RIGHT_FRONT_WHEEL   = new CanDevice("RIGHT_FRONT_WHEEL", 2);
+            public static final CanDevice RIGHT_FRONT_WHEEL   = new CanDevice("RIGHT_FRONT_WHEEL", 4);
-            public static final CanDevice RIGHT_FRONT_STEER   = new CanDevice("RIGHT_FRONT_STEER", 3);
+            public static final CanDevice RIGHT_FRONT_STEER   = new CanDevice("RIGHT_FRONT_STEER", 5);
-            public static final CanDevice RIGHT_FRONT_ENCODER = new CanDevice("RIGHT_FRONT_ENCODER", 10);
+            public static final CanDevice RIGHT_FRONT_ENCODER = new CanDevice("RIGHT_FRONT_ENCODER", 11);
             
-            public static final CanDevice LEFT_FRONT_WHEEL    = new CanDevice("LEFT_FRONT_WHEEL", 4);
+            public static final CanDevice LEFT_FRONT_WHEEL    = new CanDevice("LEFT_FRONT_WHEEL", 2);
-            public static final CanDevice LEFT_FRONT_STEER    = new CanDevice("LEFT_FRONT_STEER", 5);
+            public static final CanDevice LEFT_FRONT_STEER    = new CanDevice("LEFT_FRONT_STEER", 3);
-            public static final CanDevice LEFT_FRONT_ENCODER  = new CanDevice("LEFT_FRONT_ENCODER", 11);
+            public static final CanDevice LEFT_FRONT_ENCODER  = new CanDevice("LEFT_FRONT_ENCODER", 10);
             
             public static final CanDevice LEFT_BACK_WHEEL     = new CanDevice("LEFT_BACK_WHEEL", 6);
             public static final CanDevice LEFT_BACK_STEER     = new CanDevice("LEFT_BACK_STEER", 7);

src/main/java/frc4388/robot/Robot.java

@@ -25,6 +25,7 @@ import frc4388.utility.DeferredBlock;
 import frc4388.utility.RobotTime;
 import frc4388.utility.Status;
 import frc4388.utility.Subsystem;
+import frc4388.utility.Trim;
 import frc4388.utility.Status.Report;
 import frc4388.utility.Status.ReportLevel;
 //import frc4388.robot.subsystems.LED;
@@ -160,6 +161,14 @@ public class Robot extends TimedRobot {
   //  m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft());
   }
 
+  /**
+   * This function is called periodically during operator control.
+   */
+  @Override
+  public void teleopExit() { // the only OTHER mode that teleop can enter into is disabled.
+    Trim.dumpAll();
+  }
+
   @Override
   public void testInit() {
 

src/main/java/frc4388/robot/RobotContainer.java

@@ -14,11 +14,12 @@ import java.util.ArrayList;
 import java.util.List;
 
 import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.GenericHID;
 import frc4388.utility.controller.XboxController;
 import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.robot.Constants.OIConstants;
-import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
+import frc4388.robot.Constants.SwerveDriveConstants;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
@@ -99,7 +100,7 @@ public class RobotContainer {
         m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
             m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
                                             getDeadbandedDriverController().getRight(),
-                                true);
+                                false);
         }, m_robotSwerveDrive)
         .withName("SwerveDrive DefaultCommand"));
         m_robotSwerveDrive.setToSlow();
@@ -150,7 +151,18 @@ public class RobotContainer {
 
         DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
-            
+        
+        // @ /* Trim Test Buttons */
+        
+        new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
+            .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepUp()));
+        
+        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+            .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepDown()));
+        
+        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+            .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.load()));
+
         // ! /* Speed */
         new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
             .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -79,7 +79,7 @@ public class SwerveDrive extends Subsystem {
     
     if (fieldRelative) {
       swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
-        .withVelocityX(leftStick.getX()*-speedAdjust)
+        .withVelocityX(leftStick.getX()*speedAdjust)
         .withVelocityY(leftStick.getY()*speedAdjust)
         .withRotationalRate(rightStick.getX()*rotSpeedAdjust)
       );
@@ -181,7 +181,7 @@ public class SwerveDrive extends Subsystem {
   }
 
   public void stopModules() {
-    swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
+    // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
   }
 
   @Override

src/main/java/frc4388/utility/Trim.java

@@ -0,0 +1,118 @@
+// 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 frc4388.utility;
+
+import java.io.FileInputStream;
+import java.io.FileOutputStream;
+import java.util.ArrayList;
+
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+
+/**
+ * Reboot persistant Trims.
+ * @author Zachary Wilke
+ */
+public class Trim {
+    private static ArrayList<Trim> trims = new ArrayList<Trim>();
+    private static ShuffleboardTab trimTab = Shuffleboard.getTab("Trims");
+
+    private String trimName; 
+    private double upperBound;
+    private double lowerBound;
+    private double step;
+
+    private boolean modified = false;
+    private double currentValue;
+
+    private GenericEntry trimElement = null;
+    
+    /**
+     * Creates a Trim with a given name, upper and lower bounds, step size and intial value
+     * @param trimName please keep the trim name without special symbols
+     * @param upperBound the upper limit inclusive
+     * @param lowerBound the lower limit inclusive
+     * @param step the step size
+     * @param inital the inital value, will get overridden if the persistant trim exists on disk.
+     */
+    public Trim(String trimName, double upperBound, double lowerBound, double step, double inital) {
+        this.trimName = trimName;
+        this.upperBound = upperBound;
+        this.lowerBound = lowerBound;
+        this.step = step;
+        currentValue = inital;
+        load();
+        trimElement = trimTab.add(trimName, currentValue).getEntry();
+
+        trims.add(this);
+    }
+
+    private void clampModify() {
+        currentValue = Math.min(upperBound, Math.max(currentValue, lowerBound));
+        if (trimElement != null) 
+            trimElement.setValue(currentValue);
+        modified = true;
+    }
+
+    public void stepUp() {
+        this.currentValue += step;
+        clampModify();
+    }
+
+    public void stepDown() {
+        this.currentValue -= step;
+        clampModify();
+    }
+    
+    public void set(double value) {
+        this.currentValue = value;
+        clampModify();
+    }
+
+    public double get() {
+        return this.currentValue;
+    }
+
+    public boolean isModified() {
+        return modified;
+    }
+
+    public boolean load() {
+        try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) {
+            double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8));
+            currentValue = fileValue;
+            clampModify();
+            modified = false;
+            if (fileValue != currentValue) {
+                System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping...");
+                modified = true;
+            }
+            return true;
+        } catch (Exception e) {
+            // e.printStackTrace();
+            System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value...");
+            return false;
+        }
+    }
+
+    public void dump() {
+        try (FileOutputStream stream = new FileOutputStream("/home/lvuser/trims/" + trimName)) {
+            stream.write(DataUtils.doubleToByteArray(currentValue));
+            modified = false;
+        } catch (Exception e) {
+            // e.printStackTrace();
+            System.out.println("TRIMS: Unable to write to trim file `" + trimName + "`!?!");
+        }
+    }
+
+    public static void dumpAll() {
+        for (int i = 0; i < trims.size(); i++) {
+            Trim trim = trims.get(i);
+            if (trim.isModified()) 
+                trim.dump();
+        }
+    }
+}