Merge branch 'main' into testing

.SysId/sysid.json

@@ -0,0 +1 @@
+{}

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

@@ -11,11 +11,13 @@
 import frc.robot.auto.PhotonRunnable;
 import frc.robot.constants.OperatorConstants;
 import frc.robot.elevator.Elevator;
+
 import frc.robot.elevator.commands.AmpScoreKill;
 import frc.robot.elevator.commands.ElevatorClimb;
 import frc.robot.elevator.commands.ElevatorDown;
 import frc.robot.elevator.commands.ElevatorUp;
 import frc.robot.elevator.commands.ElevatorSmallUp;
+
 import frc.robot.intake.Intake;
 import frc.robot.intake.commands.IntakePickup;
 import frc.robot.intake.commands.IntakeShooterFeed;
@@ -47,6 +49,7 @@ public class RobotContainer {
     private final Command ElevatorSmallUp = new ElevatorSmallUp(elevator);
 
     private final Command intakePickup = new IntakePickup(intake);
+
     private final Command intakeShooterFeed = new IntakeShooterFeed(intake);
     private final Command intakeShooterFeedPreload = new IntakeShooterFeedPreload(intake,shooter);
 
@@ -77,13 +80,15 @@ public class RobotContainer {
          new IntakeShooterFeedPreload(intake,shooter)
      );
 
+
     SendableChooser<Command> autoSelector = new SendableChooser<>();
 
     public RobotContainer() {
         configureAutonomous();
         configureSubsystemCommands();
         configureSwerveDriveCommands();
 
+
         swerveDrive.setDefaultCommand(
             new RunCommand(
                 () -> swerveDrive.drive(
@@ -96,6 +101,7 @@ public RobotContainer() {
                 swerveDrive
             )
         );
+
     }
 
     private void configureSubsystemCommands() {
@@ -126,7 +132,9 @@ private void configureSwerveDriveCommands() {
                 )
             );
 
+
             controller.getLeftStick().whileTrue(new RunCommand(() -> swerveDrive.lockPosition(), swerveDrive));
+
     }
 
     private void configureAutonomous() {

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

@@ -15,25 +15,27 @@ public class ElevatorConstants {
     public static final double ENCODER_POSITION_FACTOR = (SPOOL_DIAMETER * Math.PI) / GEAR_RATIO;
     public static final double ENCODER_VELOCITY_FACTOR = ENCODER_POSITION_FACTOR / 60.0;
 
-    public static final double P = 0.04;
+    public static final double P = 0.0;
     public static final double I = 0.0;
     public static final double D = 0.0;
     public static final double PID_MINIMUM_OUTPUT = -1.0;
     public static final double PID_MAXIMUM_OUTPUT = 1.0;
 
-    public static final double S = 0.0;
-    public static final double G = 0.0;
-    public static final double V = 0.0;
-    public static final double A = 0.0;
+    public static final double S = 0.15944; // Volts
+    public static final double G = 0.68283; // Volts
+    public static final double V = 0.076034; // Volts times seconds per inch
+    public static final double A = 0.022109; // Volts times seconds squared per inch
 
-    public static final double MAX_VELOCITY = 55.0; // Inches per second
-    public static final double MAX_ACCELERATION = 550.0; // Inches per second squared. Assuming we can accelerate to max velocity in 0.1 seconds.
+    public static final double MAX_VELOCITY = 10; // Inches per second
+    public static final double MAX_ACCELERATION = 5; // Inches per second squared. Assuming we can accelerate to max velocity in 0.1 seconds.
 
     // Inches
+
     public static final double MAX_HEIGHT = 28.0;
     public static final double TARGET_HEIGHT = 28.0;
     public static final double TOLERANCE = 0.1;
 
+
     public static final double TEMP_MAX_HEIGHT = 5.0;
 
     public static final double MOTOR_SLOWRAISE_SPEED = 0.75;

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

@@ -4,9 +4,13 @@
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.ElevatorConstants;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import edu.wpi.first.wpilibj.Timer;
+
 
 public class Elevator extends SubsystemBase {
     private final CANSparkMax motor;
@@ -25,8 +29,6 @@ public Elevator() {
         motor.burnFlash();
     }
 
-
-
     public void getMotorRPM() {
         SmartDashboard.putNumber("Evt. Motor RPM", encoder.getVelocity());
     }

src/main/java/frc/robot/elevator/commands/ElevatorUp.java

@@ -1,3 +1,4 @@
+
 package frc.robot.elevator.commands;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;