no worky

src/main/deploy/pathplanner/paths/HP to 330 Right.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 3.6442622950819668,
+        "x": 3.609900518622585,
-        "y": 5.031967213114754
+        "y": 5.005924534374863
       },
       "prevControl": {
-        "x": 3.328722911520323,
+        "x": 3.2943611350609414,
-        "y": 5.557866185717494
+        "y": 5.531823506977602
       },
       "nextControl": null,
       "isLocked": false,

src/main/deploy/pathplanner/paths/L Backup.path

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.6442622950819668,
+        "x": 3.609900518622585,
-        "y": 5.031967213114754
+        "y": 5.005924534374863
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.5117625600811717,
+        "x": 3.47740078362179,
-        "y": 5.243966789116026
+        "y": 5.217924110376135
       },
       "isLocked": false,
       "linkedName": "L"

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

@@ -7,7 +7,7 @@ package frc.robot;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.Logger;
-//import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
@@ -35,7 +35,7 @@ public Robot() {
 
     if (isReal()) {
         Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
-        //Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+        Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
         new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
     } else {
         setUseTiming(false); // Run as fast as possible

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

@@ -111,9 +111,9 @@ public class RobotContainer {
 
 		drivetrain.setDefaultCommand(
 			drivetrain.drive(
-				() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
+				() -> Math.pow(driver.getLeftY(), 3),
-				() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
+				() -> Math.pow(driver.getLeftX(), 3),
-				driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
+				() -> driver.getRightX(), 
 				() -> true
 			)
 		);
@@ -424,26 +424,6 @@ public class RobotContainer {
 		sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
 			.withWidget(BuiltInWidgets.kGraph);
 
-		sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
-			.withSize(1, 1)
-			.withPosition(5, 0)
-			.withWidget(BuiltInWidgets.kTextView);
-
-		sensorTab.addDouble("Elevator error up", elevator::getPIDUpError)
-			.withSize(1, 1)
-			.withPosition(5, 1)
-			.withWidget(BuiltInWidgets.kTextView);
-
-			sensorTab.addDouble("Elevator setpoint down", elevator::getPIDDownSetpoint)
-			.withSize(1, 1)
-			.withPosition(5, 0)
-			.withWidget(BuiltInWidgets.kTextView);
-
-		sensorTab.addDouble("Elevator error down", elevator::getPIDDownError)
-			.withSize(1, 1)
-			.withPosition(5, 1)
-			.withWidget(BuiltInWidgets.kTextView);
-
 		sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);
 
 		sensorTab.addDouble("velocity", drivetrain::getVelocity);

src/main/java/frc/robot/constants/AutoConstants.java

@@ -16,6 +16,8 @@ public class AutoConstants {
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 
+    public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.5;
+
     public static final double kPXYController = 3.5;
     public static final double kPThetaController = 5;
 

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 public class DrivetrainConstants {
     // Driving Parameters - Note that these are not the maximum capable speeds of
     // the robot, rather the allowed maximum speeds
-    public static final double kMaxSpeedMetersPerSecond = 5.5;
+    public static final double kMaxSpeedMetersPerSecond = 5.5 * 0.75;
     public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
 
     // Chassis configuration

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -5,6 +5,7 @@ import static edu.wpi.first.units.Units.Second;
 import static edu.wpi.first.units.Units.Seconds;
 
 import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
@@ -22,6 +23,7 @@ public class ElevatorConstants {
 
     public static final int kCurrentLimit = 40;
 
+    /*
     public static final double kUpControllerP = 5.6;//7; //
     public static final double kUpControllerI = 0;
     public static final double kUpControllerD = 0.28;//0.28
@@ -31,6 +33,11 @@ public class ElevatorConstants {
     public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35
 
     public static final double kMaintainP = 3;
+    */
+
+    public static final double kP = 3;//7; //
+    public static final double kI = 0;
+    public static final double kD = 0;//.28;//0.28 
 
     public static final double kAllowedError = 1;
 
@@ -38,8 +45,10 @@ public class ElevatorConstants {
     public static final double kFeedForwardG = (0.95 + 0.2)/2;  /* kG too high + kG too low / 2 */    // calculated value 0.6
     public static final double kFeedForwardV = 0.12;   // calculated value 0.12
 
-    public static final double kMaxVelocity = 150.0;    // 120 inches per second (COOKING) calculated max is 184 in/s
+    public static final double kMaxVelocity = 100.0;    // 100 inches per second (COOKING) calculated max is 184 in/s
-    public static final double kMaxAcceleration = 240;    // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
+    public static final double kMaxAcceleration = 50;    // 50 inches per second^2 (also COOKING) calculated max is 600 in/s^2
+    public static final double kMaxVelocityAlgae = 120;
+    public static final double kMaxAccelerationAlgae = 400;
 
     public static final double kCoralIntakePosition = 0;
     public static final double kL1Position = 17;
@@ -83,5 +92,9 @@ public class ElevatorConstants {
         motorConfig.encoder
             .positionConversionFactor(kEncoderPositionConversionFactor)
             .velocityConversionFactor(kEncoderVelocityConversionFactor);
+        motorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1);
     }
 }

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

@@ -362,8 +362,8 @@ public class Drivetrain extends SubsystemBase {
                 );
 
                 double speed = Math.hypot(controlEffort.vxMetersPerSecond, controlEffort.vyMetersPerSecond);
-                if (speed > AutoConstants.kMaxSpeedMetersPerSecond) {
+                if (speed > AutoConstants.kMaxSpeedMetersPerSecondAutoAlign) {
-                    double mul = AutoConstants.kMaxSpeedMetersPerSecond / speed;
+                    double mul = AutoConstants.kMaxSpeedMetersPerSecondAutoAlign / speed;
                     controlEffort.vxMetersPerSecond *= mul;
                     controlEffort.vyMetersPerSecond *= mul;
                 }

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

@@ -5,14 +5,17 @@ import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
 
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
-import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -26,13 +29,15 @@ public class Elevator extends SubsystemBase {
 
     private DigitalInput bottomLimitSwitch;
 
-    private PIDController pidControllerUp;
-    private PIDController pidControllerDown;
-
-    private PIDController maintainPID;
-
     private ElevatorFeedforward feedForward;
 
+    private TrapezoidProfile trapProfile;
+    private TrapezoidProfile trapProfileAlgae;
+    private TrapezoidProfile.State goal;
+    private TrapezoidProfile.State setpoint;
+
+    private SparkClosedLoopController controller;
+    
     public Elevator() {
         elevatorMotor1 = new SparkMax(
             ElevatorConstants.kElevatorMotor1ID, 
@@ -62,33 +67,17 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kBottomLimitSwitchID
         );
 
-        pidControllerDown = new PIDController(
-            ElevatorConstants.kDownControllerP, 
-            ElevatorConstants.kDownControllerI,
-            ElevatorConstants.kDownControllerD
-        );
-        pidControllerDown.setSetpoint(0);
-
-        pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
-
-        pidControllerUp = new PIDController(
-            ElevatorConstants.kUpControllerP, 
-            ElevatorConstants.kUpControllerI,
-            ElevatorConstants.kUpControllerD
-        );
-        pidControllerUp.setSetpoint(0);
-
-        pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
-
-        maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0);
-
-        maintainPID.setTolerance(ElevatorConstants.kAllowedError);
-
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
             ElevatorConstants.kFeedForwardG, 
             ElevatorConstants.kFeedForwardV
         );
+
+        trapProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocity, ElevatorConstants.kMaxAcceleration));
+
+        trapProfileAlgae = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocityAlgae, ElevatorConstants.kMaxAccelerationAlgae));
+
+        controller = elevatorMotor1.getClosedLoopController();
     }
 
     @Override
@@ -98,8 +87,6 @@ public class Elevator extends SubsystemBase {
         }
 
         Logger.recordOutput("elevator position", getEncoderPosition());
-        Logger.recordOutput("elevator up setpoint", pidControllerUp.getSetpoint());
-        Logger.recordOutput("elevator down setpoint", pidControllerDown.getSetpoint());
     }
 
     /**
@@ -153,28 +140,15 @@ public class Elevator extends SubsystemBase {
     public Command maintainPosition() {
 
         return startRun(() -> {
-            maintainPID.reset();
+
-            maintainPID.setSetpoint(pidControllerUp.getSetpoint());
         }, 
         () -> {
-
+            controller.setReference(
-            double maintainOutput = maintainPID.calculate(getEncoderPosition());
+                encoder.getPosition(), 
-
+                ControlType.kPosition, 
-            if(!maintainPID.atSetpoint()) 
+                ClosedLoopSlot.kSlot0,
-                elevatorMotor1.setVoltage( MathUtil.clamp(
+                feedForward.calculate(0.0)
-                    maintainOutput + feedForward.calculate(0), -2, 2)
             );
-            else{
-                elevatorMotor1.setVoltage(
-                    feedForward.calculate(0)
-                );
-            } 
-            
-/* 
-            elevatorMotor1.setVoltage(
-                    feedForward.calculate(0)
-                );
-                */
 
         });
         
@@ -192,123 +166,41 @@ public class Elevator extends SubsystemBase {
      * Moves the elevator to a target destination (setpoint). 
      * 
      * @param setpoint Target destination of the subsystem
-     * @param timeout Time to achieve the setpoint before quitting
      * @return Sets motor voltage to achieve the target destination
      */
-    public Command goToSetpoint(DoubleSupplier setpoint) {
+    public Command goToSetpoint(DoubleSupplier setGoal) {
         
-        if (setpoint.getAsDouble() == 0) {
         return startRun(() -> {
+            goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0);
+        }, () -> {
+            setpoint = trapProfile.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);
 
-                pidControllerUp.reset();
+            controller.setReference(
-                pidControllerDown.reset();
+                setpoint.position, 
-                pidControllerUp.setSetpoint(setpoint.getAsDouble());
+                ControlType.kPosition, 
-                pidControllerDown.setSetpoint(setpoint.getAsDouble());
+                ClosedLoopSlot.kSlot0,
-                
+                feedForward.calculate(encoder.getVelocity())
-                }, 
-                () -> {
-                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
-                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
-        
-                    if(setpoint.getAsDouble()>encoder.getPosition()) 
-                        elevatorMotor1.setVoltage( MathUtil.clamp(
-                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
-                        );
-                    else{
-                        elevatorMotor1.setVoltage(
-                            MathUtil.clamp(
-                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
             );
+
+        }).until(() -> trapProfile.isFinished(encoder.getPosition()));
+
     }          
 
-                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
+    public Command goToSetpointAlgae(DoubleSupplier setGoal) {
-                    .andThen(runManualElevator(() -> -.5)
-                    .until(() -> encoder.getPosition() == 0));
             
-        } else {
         return startRun(() -> {
+            goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0);
+        }, () -> {
+            setpoint = trapProfileAlgae.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);
 
-                pidControllerUp.reset();
+            controller.setReference(
-                pidControllerDown.reset();
+                setpoint.position, 
-                pidControllerUp.setSetpoint(setpoint.getAsDouble());
+                ControlType.kPosition, 
-                pidControllerDown.setSetpoint(setpoint.getAsDouble());
+                ClosedLoopSlot.kSlot0,
-        
+                feedForward.calculate(setpoint.velocity)
-                },
-                () -> {
-                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
-                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
-        
-                    if(setpoint.getAsDouble()>encoder.getPosition()) 
-                        elevatorMotor1.setVoltage( MathUtil.clamp(
-                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
             );
-                    else{
-                        elevatorMotor1.setVoltage(
-                            MathUtil.clamp(
-                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
-                        );
-                    }
 
-                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
+        }).until(() -> trapProfileAlgae.isFinished(encoder.getPosition()));
-        }
-    }          
-
-    public Command goToSetpointAlgae(DoubleSupplier setpoint) {
-            
-        if (setpoint.getAsDouble() == 0) {
-            return startRun(() -> {
-        
-                pidControllerUp.reset();
-                pidControllerDown.reset();
-                pidControllerUp.setSetpoint(setpoint.getAsDouble());
-                pidControllerDown.setSetpoint(setpoint.getAsDouble());
-                
-                }, 
-                () -> {
-                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
-                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
-        
-                    if(setpoint.getAsDouble()>encoder.getPosition()) 
-                        elevatorMotor1.setVoltage( MathUtil.clamp(
-                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
-                        );
-                    else{
-                        elevatorMotor1.setVoltage(
-                            MathUtil.clamp(
-                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
-                        );
-                    }
-                
-                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
-                    .andThen(runManualElevator(() -> -.5)
-                    .until(() -> encoder.getPosition() == 0));
-
-        } else {
-            return startRun(() -> {
-        
-                pidControllerUp.reset();
-                pidControllerDown.reset();
-                pidControllerUp.setSetpoint(setpoint.getAsDouble());
-                pidControllerDown.setSetpoint(setpoint.getAsDouble());
-        
-                },
-                () -> {
-                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
-                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
-        
-                    if(setpoint.getAsDouble()>encoder.getPosition()) 
-                        elevatorMotor1.setVoltage( MathUtil.clamp(
-                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
-                        );
-                    else{
-                        elevatorMotor1.setVoltage(
-                            MathUtil.clamp(
-                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
-                        );
-                    }
-                
-                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
-        }
     }   
 
     /**
@@ -348,19 +240,4 @@ public class Elevator extends SubsystemBase {
         return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
     }
 
-    public double getPIDUpSetpoint() {
-        return pidControllerUp.getSetpoint();
-    }
-
-    public double getPIDUpError() {
-        return pidControllerUp.getError();
-    }
-
-    public double getPIDDownSetpoint() {
-        return pidControllerDown.getSetpoint();
-    }
-
-    public double getPIDDownError() {
-        return pidControllerDown.getError();
-    }
 }