elevator tuning

grt192 · Feb 2, 2024 · efa1de8 · efa1de8
1 parent ed9f15d
commit efa1de8
Show file tree

Hide file tree

Showing 8 changed files with 57 additions and 45 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -24,20 +24,20 @@ public static class ElevatorConstants {
 
     public static final int ZERO_LIMIT_ID = 2;
 
-    public static final int GROUND_POSITION = 0;
-    public static final int SPEAKER_POSITION = 0;
-    public static final int AMP_POSITION = 0;
-    public static final int CHUTE_POSITION = 0;
-    public static final int TRAP_POSITION = 0;
-    public static final int START_POSITION = 0; 
-
-    public static final double EXTENSION_P= 2.4;
+    public static final double GROUND_POSITION = 0;
+    public static final double SPEAKER_POSITION = 0;
+    public static final double AMP_POSITION = Units.inchesToMeters(28);
+    public static final double CHUTE_POSITION = Units.inchesToMeters(25);
+    public static final double TRAP_POSITION = Units.inchesToMeters(30);
+    public static final double START_POSITION = 0; 
+
+    public static final double EXTENSION_P= 3.5;
     public static final double EXTENSION_I= 0;
     public static final double EXTENSION_D= 0;
     public static final double EXTENSION_TOLERANCE= 0.003;
 
-    public static final double POSITION_CONVERSION_FACTOR = 0;
-    public static final double VELOCITY_CONVERSION_FACTOR = 0;
+    public static final double POSITION_CONVERSION_FACTOR = Units.inchesToMeters(30.)/63.5;
+    public static final double VELOCITY_CONVERSION_FACTOR = 1;
   }
   public static class OperatorConstants {
     public static final int kDriverControllerPort = 0;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -15,6 +15,8 @@
 import frc.robot.controllers.XboxDriveController;
 import frc.robot.commands.climb.ClimbLowerCommand;
 import frc.robot.commands.climb.ClimbRaiseCommand;
+import frc.robot.commands.elevator.ElevatorToAMPCommand;
+import frc.robot.commands.elevator.ElevatorToGroundCommand;
 import frc.robot.subsystems.climb.ClimbSubsystem;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.swerve.BaseSwerveSubsystem;
@@ -91,14 +93,10 @@ public RobotContainer() {
   private void configureBindings() {
 
       elevatorSubsystem.setDefaultCommand(new InstantCommand(() -> {
-        if(mechController.getLeftTriggerAxis() != 0 && mechController.getRightTriggerAxis() != 0){
-          elevatorSubsystem.setManual();
-          elevatorSubsystem.setManualPower(mechController.getRightTriggerAxis()-mechController.getLeftTriggerAxis());
-        }
-        else{
-          elevatorSubsystem.setAuto();
-        }
-    }));
+
+        elevatorSubsystem.setManualPower(mechController.getRightTriggerAxis()-mechController.getLeftTriggerAxis());
+
+      }, elevatorSubsystem));
 
       bButton.onTrue(new InstantCommand(() -> {
         shooterFeederSubsystem.setFeederMotorSpeed(.4);
@@ -117,7 +115,7 @@ private void configureBindings() {
       }));
 
       shooterPivotSubsystem.setDefaultCommand(new InstantCommand(() -> {
-          shooterPivotSubsystem.setPivotMotorSpeed((.2 * mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis()));
+          // shooterPivotSubsystem.setPivotMotorSpeed((.2 * mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis()));
           // pivotSubsystem.printCurrentAngle();
       }, shooterPivotSubsystem));
 
@@ -139,6 +137,9 @@ private void configureBindings() {
         }
       }, intakeRollerSubsystem));
 
+      rightBumper.onTrue(new ElevatorToAMPCommand(elevatorSubsystem));
+      leftBumper.onTrue(new ElevatorToGroundCommand(elevatorSubsystem));
+
 
 
 

diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java
@@ -13,6 +13,10 @@ public ElevatorToAMPCommand(ElevatorSubsystem elevatorSubsystem){
 
     @Override
     public void end(boolean interrupted){
+        System.out.println("TO AMP FINISHED");
+        if (interrupted){
+            System.out.println("TO AMP INTERUPTED");
+        }
         return;
     }
 
@@ -23,9 +27,6 @@ public void execute(){
 
     @Override
     public boolean isFinished(){
-        if(this.elevatorSubsystem.getState()==ElevatorState.AMP){
-            return true;
-        }
-        else return false;
+        return elevatorSubsystem.atState(ElevatorState.AMP);
     }
 }
diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java
@@ -23,9 +23,6 @@ public void execute(){
 
     @Override
     public boolean isFinished(){
-        if(this.elevatorSubsystem.getState()==ElevatorState.CHUTE){
-            return true;
-        }
-        else return false;
+        return elevatorSubsystem.atState(ElevatorState.CHUTE);
     }
 }
diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java
@@ -13,19 +13,25 @@ public ElevatorToGroundCommand(ElevatorSubsystem elevatorSubsystem){
 
     @Override
     public void end(boolean interrupted){
+        System.out.println("TO GROUND FINISHED");
+        if (interrupted){
+            System.out.println("TO GROUND INTERUPTED");
+        }
         return;
     }
 
+    @Override
+    public void initialize() {
+
+    }
+
     @Override
     public void execute(){
-        this.elevatorSubsystem.setTargetState(ElevatorState.GROUND);
+        elevatorSubsystem.setTargetState(ElevatorState.GROUND);
     }
 
     @Override
     public boolean isFinished(){
-        if(this.elevatorSubsystem.getState()==ElevatorState.GROUND){
-            return true;
-        }
-        else return false;
+        return elevatorSubsystem.atState(ElevatorState.GROUND);
     }
 }
diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java
@@ -23,9 +23,6 @@ public void execute(){
 
     @Override
     public boolean isFinished(){
-        if(this.elevatorSubsystem.getState()==ElevatorState.SPEAKER){
-            return true;
-        }
-        else return false;
+        return elevatorSubsystem.atState(ElevatorState.SPEAKER);
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java
@@ -12,7 +12,7 @@
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj.DigitalInput;
-
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.NetworkTable;
@@ -42,6 +42,7 @@ public class ElevatorSubsystem extends SubsystemBase{
     private final CANSparkMax extensionMotor;
     private RelativeEncoder extensionEncoder;
     private SparkPIDController extensionPidController;
+    private final Timer timer = new Timer();
 
     //PID Values
 
@@ -54,6 +55,8 @@ public class ElevatorSubsystem extends SubsystemBase{
     public ElevatorSubsystem() {
         //Print out current position for debug & measurement
         //System.out.print(extensionEncoder.getPosition());
+
+        timer.start();
 
         this.zeroLimitSwitch = new DigitalInput(ElevatorConstants.ZERO_LIMIT_ID); 
         elevatorNetworkTableInstance = NetworkTableInstance.getDefault();
@@ -91,6 +94,9 @@ else if(currentTargetState.equals(ElevatorState.SPEAKER)){
         //this entry is working!
         extensionMotor = new CANSparkMax(ElevatorConstants.EXTENSION_ID, MotorType.kBrushless);
         extensionMotor.setIdleMode(IdleMode.kBrake);
+        extensionMotor.setInverted(true);
+        extensionMotor.setClosedLoopRampRate(0.3);
+
 
         extensionEncoder = extensionMotor.getEncoder();
         extensionEncoder.setPositionConversionFactor(ElevatorConstants.POSITION_CONVERSION_FACTOR);
@@ -100,17 +106,22 @@ else if(currentTargetState.equals(ElevatorState.SPEAKER)){
         extensionFollow = new CANSparkMax(ElevatorConstants.EXTENSION_FOLLOW_ID, MotorType.kBrushless);
         extensionFollow.follow(extensionMotor);
         extensionFollow.setIdleMode(IdleMode.kBrake);
+        extensionFollow.setInverted(true);
 
         extensionPidController = extensionMotor.getPIDController();
         extensionPidController.setP(ElevatorConstants.EXTENSION_P);
         extensionPidController.setI(ElevatorConstants.EXTENSION_I);
         extensionPidController.setD(ElevatorConstants.EXTENSION_D);
         extensionPidController.setSmartMotionAllowedClosedLoopError(ElevatorConstants.EXTENSION_TOLERANCE, 0);
+        extensionPidController.setOutputRange(-0.7, 1);
     }
     @Override
     public void periodic(){
-        //System.out.println(elevatorNetworkTablePositionEntry.getString("default")); 
-        System.out.println(this.getExtensionMeters());
+        //System.out.println(elevatorNetworkTablePositionEntry.getString("default"));
+        if(timer.advanceIfElapsed(.2)){
+            System.out.println(Units.metersToInches(getExtensionMeters()));
+        }
+
         //System.out.println(this.getTargetState());
         if(isManual){
             //Add some factors for better control.
@@ -127,12 +138,10 @@ public void periodic(){
 
         //Start move to target posision
         if (targetState != state){
-            extensionPidController.setReference(targetState.getExtendDistanceMeters(), ControlType.kPosition, 0, 0.03, ArbFFUnits.kPercentOut);
-        }
-        if(atState(this.targetState)){
-            this.setState(this.getTargetState());
+
         }
 
+
     }
 
     public boolean atState(ElevatorState state){
@@ -151,7 +160,7 @@ public void setState(ElevatorState state) {
     }
 
     public void setTargetState(ElevatorState targetState){
-        this.targetState = targetState;
+        extensionPidController.setReference(targetState.getExtendDistanceMeters(), ControlType.kPosition, 0, 0.03, ArbFFUnits.kPercentOut);
         return;
     }
 

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java
@@ -17,7 +17,7 @@ public class ShooterPivotSubsystem extends SubsystemBase {
     final double GEARBOX_RATIO = 18.16; //ask cadders
     public final int ERRORTOLERANCE = 5; //error tolerance for pid
     final int LIMIT_SWITCH_ID = 3; //placeholder
-    final double CONVERSION_FACTOR = Math.PI/(2*4.57);
+    final double CONVERSION_FACTOR = Math.PI/(2.*4.57);
 
     //motors
     private final CANSparkMax pivotMotor;
@@ -66,6 +66,7 @@ public ShooterPivotSubsystem(boolean alliance){
 
         //encoder stuff
         rotationEncoder.setPositionConversionFactor(CONVERSION_FACTOR);
+        rotationEncoder.setVelocityConversionFactor(CONVERSION_FACTOR * 60);
         //rotationEncoder.setInverted(true);
         rotationPIDController.setSmartMotionAllowedClosedLoopError(ERRORTOLERANCE, 0); //what does 0 do (slotID is from 0-3)