Elevator Tuning; faster

Author: EvanWoodfin

.DataLogTool/datalogtool.json

@@ -0,0 +1,10 @@
+{
+  "download": {
+    "deleteAfter": false,
+    "localDir": "C:\\Users\\ewood\\Desktop",
+    "serverTeam": "619"
+  },
+  "output": {
+    "outputFolder": "C:\\Users\\ewood\\Desktop"
+  }
+}

src/main/java/frc/robot/Constants.java

@@ -206,8 +206,8 @@ public static final class ElevatorConstants {
     public static final double wristMotorReduction = 1.0;
 
 
-    public static final double maxVelocityMetersPerSec = 1.0; // Arbitrary
-    public static final double maxAccelerationMetersPerSecSqrd = 1.5;
+    public static final double maxVelocityMetersPerSec = 2.0; // Arbitrary
+    public static final double maxAccelerationMetersPerSecSqrd = 4.0;
 
 
 
@@ -235,29 +235,16 @@ public static final class ElevatorConstants {
     public static final double maxHeightEncoderVal = 68.75753021240234;
 
 
-
-
-
-
-
-
-
-
-
-
-
-
-
     public static final double encoderZeroOffsetRotations = 0;
 
-    public static final double kpElevator = 9;
+    public static final double kpElevator = 11.5;
     public static final double kiElevator = 0.0;
     public static final double kdElevator = 0.0;
 
     public static final double ksFeedforward = 0.3;
-    public static final double kvFeedforward = 4.4;
+    public static final double kvFeedforward = 4.81;
 
-    public static final double feedforwardGravity = 0.40;
+    public static final double feedforwardGravity = 0.29;
 
     public static final DCMotor elevatorGearbox = DCMotor.getNeoVortex(2);
 

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

@@ -13,6 +13,9 @@
 
 package frc.robot;
 
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.util.datalog.StringLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.Threads;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -78,9 +81,9 @@ public Robot() {
 
     //Uncomment if NT errors are occuring weirdly
 
-    // StringLogEntry entry = new StringLogEntry(DataLogManager.getLog(), "/ntlog");
-    // NetworkTableInstance.getDefault().addLogger(0, 100,
-    // event -> entry.append(event.logMessage.filename + ":" + event.logMessage.line + ":" + event.logMessage.message));
+    StringLogEntry entry = new StringLogEntry(DataLogManager.getLog(), "/ntlog");
+    NetworkTableInstance.getDefault().addLogger(0, 100,
+    event -> entry.append(event.logMessage.filename + ":" + event.logMessage.line + ":" + event.logMessage.message));
     // Initialize URCL
     Logger.registerURCL(URCL.startExternal());
 

src/main/java/frc/robot/RobotContainer.java

@@ -240,8 +240,8 @@ private Command robotGoToHeightCommandCreator(ElevatorHeight height){
         }else{
             command = Commands.sequence(
                 new WristGoToPositionCommand(wrist, WristAngleRad.L2L3),
-                new ElevatorGoToPositionPositionCommand(elevator, height),
-                new WristGoToPositionCommand(wrist, getEndWristAngleForGivenElevatorHeight(height))
+                new ElevatorGoToPositionPositionCommand(elevator, height)
+                //new WristGoToPositionCommand(wrist, getEndWristAngleForGivenElevatorHeight(height))
             ).withInterruptBehavior(InterruptionBehavior.kCancelSelf);
         }
 

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

@@ -77,6 +77,7 @@ public void periodic(){
         double voltage = elevatorController.calculate(getPositionMeters());
 
         inputs.elevatorSetpointPositionMeters = elevatorController.getSetpoint().position;
+        inputs.elevatorSetpointVelocityMPS = elevatorController.getSetpoint().velocity;
 
 
         if(inputs.elevatorSetpointPositionMeters > Constants.ElevatorConstants.minHeightMeters + 0.02){

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

@@ -13,6 +13,7 @@ public static class ElevatorIOInputs {
         public double elevatorSetpointPositionMeters = 0.0;
         public double elevatorHeightMeters = ElevatorHeight.HOME.heightMeters;
         public double elevatorVelocityMPS = 0;
+        public double elevatorSetpointVelocityMPS = 0;
 
         public double elevatorGoalMeters = ElevatorHeight.HOME.heightMeters;
         public ElevatorHeight elevatorGoalEnum = ElevatorHeight.HOME;

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

@@ -99,7 +99,7 @@ private double encoderValToHeightMeters(double encoderVal){
 
     private double encoderVelToVelocityMetersPerSec(double encoderVel){
         return (encoderVel / Constants.ElevatorConstants.maxHeightEncoderVal) *
-        (Constants.ElevatorConstants.maxHeightMeters - Constants.ElevatorConstants.minHeightMeters);
+        (Constants.ElevatorConstants.maxHeightMeters - Constants.ElevatorConstants.minHeightMeters) / 60.0;
     }
 
 

src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java

@@ -34,9 +34,9 @@ public class SparkOdometryThread {
   private final List<SparkBase> sparks = new ArrayList<>();
   private final List<DoubleSupplier> sparkSignals = new ArrayList<>();
   private final List<DoubleSupplier> genericSignals = new ArrayList<>();
-  private final List<Queue<Double>> sparkQueues = new ArrayList<>();
-  private final List<Queue<Double>> genericQueues = new ArrayList<>();
-  private final List<Queue<Double>> timestampQueues = new ArrayList<>();
+  private final List<ArrayBlockingQueue<Double>> sparkQueues = new ArrayList<>();
+  private final List<ArrayBlockingQueue<Double>> genericQueues = new ArrayList<>();
+  private final List<ArrayBlockingQueue<Double>> timestampQueues = new ArrayList<>();
 
   private static SparkOdometryThread instance = null;
   private Notifier notifier = new Notifier(this::run);
@@ -60,7 +60,7 @@ public void start() {
 
   /** Registers a Spark signal to be read from the thread. */
   public Queue<Double> registerSignal(SparkBase spark, DoubleSupplier signal) {
-    Queue<Double> queue = new ArrayBlockingQueue<>(20);
+    ArrayBlockingQueue<Double> queue = new ArrayBlockingQueue<>(20);
     Drive.odometryLock.lock();
     try {
       sparks.add(spark);
@@ -74,7 +74,7 @@ public Queue<Double> registerSignal(SparkBase spark, DoubleSupplier signal) {
 
   /** Registers a generic signal to be read from the thread. */
   public Queue<Double> registerSignal(DoubleSupplier signal) {
-    Queue<Double> queue = new ArrayBlockingQueue<>(20);
+    ArrayBlockingQueue<Double> queue = new ArrayBlockingQueue<>(20);
     Drive.odometryLock.lock();
     try {
       genericSignals.add(signal);
@@ -87,7 +87,7 @@ public Queue<Double> registerSignal(DoubleSupplier signal) {
 
   /** Returns a new queue that returns timestamp values for each sample. */
   public Queue<Double> makeTimestampQueue() {
-    Queue<Double> queue = new ArrayBlockingQueue<>(20);
+    ArrayBlockingQueue<Double> queue = new ArrayBlockingQueue<>(20);
     Drive.odometryLock.lock();
     try {
       timestampQueues.add(queue);