diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 01d239c..5b70a53 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,6 +15,7 @@ import frc.robot.subsystems.ClimberRollers; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Manipulator; +import frc.robot.subsystems.sysid.ElevatorSysID; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; @@ -30,6 +31,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; public class RobotContainer { private ClimberPivot climberPivot; @@ -108,8 +110,8 @@ public class RobotContainer { ); elevator.setDefaultCommand( - elevator.runManualElevator( - () -> -operator.getLeftY() * .5 + elevator.goToSetpoint( + () -> 0 ) ); @@ -144,24 +146,22 @@ public class RobotContainer { driver.start().and(driver.back()).onTrue( startingConfig() ); - + */ driver.povDown().whileTrue(climberPivot.runPivot(-0.5)); driver.povUp().whileTrue(climberPivot.runPivot(0.5)); driver.povLeft().whileTrue(climberRollers.runRoller(0.5)); driver.povRight().whileTrue(climberRollers.runRoller(-0.5)); - */ - - operator.povUp().onTrue( - elevator.goToSetpoint(() -> 20).until(elevator::eitherAtGoal) + + + operator.povUp().whileTrue( + elevator.goToSetpoint(() -> 50) ); - operator.povDown().onTrue( - elevator.goToSetpoint(() -> 0).until(elevator::eitherAtGoal) + operator.povDown().whileTrue( + elevator.goToSetpoint(() -> 0) ); - operator.a().whileTrue(elevator.maintainPosition()); - /* operator.a().whileTrue(elevator.runManualElevator(() -> 0.2)); @@ -231,7 +231,7 @@ public class RobotContainer { sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition) .withSize(2, 1) .withPosition(0, 0) - .withWidget(BuiltInWidgets.kTextView); + .withWidget(BuiltInWidgets.kGraph); sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition) .withSize(2, 1) @@ -258,9 +258,14 @@ public class RobotContainer { .withPosition(4, 1) .withWidget(BuiltInWidgets.kBooleanBox); - sensorTab.addDouble("ElevMotor1", elevator::getMotor1); + sensorTab.addDouble("ElevMotor1", elevator::getMotor1) + .withWidget(BuiltInWidgets.kGraph); - sensorTab.addDouble("ElevMotor2", elevator::getMotor2); + sensorTab.addDouble("ElevMotor2", elevator::getMotor2) + .withWidget(BuiltInWidgets.kGraph); + + sensorTab.addDouble("PID output", elevator::currentPIDOut) + .withWidget(BuiltInWidgets.kGraph); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 5a3f458..792ba1a 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -18,23 +18,23 @@ public class ElevatorConstants { // 60/11 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position public static final double kEncoderPositionConversionFactor = 11.0/60.0 * (22.0*0.25) * 2.0; - public static final double kEncoderVelocityConversionFactor = 11.0/60.0 * (22.0*0.25) * 2.0 / 60; + public static final double kEncoderVelocityConversionFactor = kEncoderPositionConversionFactor / 60; public static final int kCurrentLimit = 40; - public static final double kUpControllerP = 4;//7; // + public static final double kUpControllerP = 5.6;//7; // public static final double kUpControllerI = 0; - public static final double kUpControllerD = 0.35;//0.1;//0.35 + public static final double kUpControllerD = 0.28;//0.28 - public static final double kDownControllerP = 6;//7; // + public static final double kDownControllerP = 5.6;//7; // public static final double kDownControllerI = 0; - public static final double kDownControllerD = 0.175;//0.1;//0.35 + public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35 - public static final double kAllowedError = 0.75; + public static final double kAllowedError = 1; public static final double kFeedForwardS = (0.95 - 0.2)/2*0.8; /* kG too high - kG too low / 2 0.95, 0.2 */ 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.06; // calculated value 0.12 + 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 kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2 diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index b7fb1f0..a241be5 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -29,6 +29,8 @@ public class Elevator extends SubsystemBase { private ProfiledPIDController pidControllerDown; private ElevatorFeedforward feedForward; + + private double currentPIDOut; public Elevator() { elevatorMotor1 = new SparkMax( @@ -53,8 +55,6 @@ public class Elevator extends SubsystemBase { PersistMode.kPersistParameters ); - //elevatorClosedLoop = elevatorMotor1.getClosedLoopController(); - encoder = elevatorMotor1.getEncoder(); bottomLimitSwitch = new DigitalInput( @@ -170,14 +170,24 @@ public class Elevator extends SubsystemBase { ElevatorConstants.kMaxHeight ); - return run(() -> { - //pidController.reset(encoder.getPosition(), encoder.getVelocity()); + pidControllerDown.setGoal(clampedSetpoint); - elevatorMotor1.setVoltage( - pidControllerUp.calculate( + return run(() -> { + //pidControllerUp.reset(encoder.getPosition(), encoder.getVelocity()); + + if(!pidControllerDown.atGoal()){ + currentPIDOut = pidControllerDown.calculate( encoder.getPosition(), clampedSetpoint - ) + feedForward.calculate(pidControllerUp.getSetpoint().velocity) + ); + System.out.println("CALCULATED"); + }else{ + currentPIDOut = 0; + System.out.println("SET ZERO"); + } + + elevatorMotor1.setVoltage( + currentPIDOut + feedForward.calculate(pidControllerDown.getSetpoint().velocity) ); }); @@ -237,4 +247,8 @@ public class Elevator extends SubsystemBase { public double getMotor2() { return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage(); } + + public double currentPIDOut(){ + return currentPIDOut; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java b/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java index fdb7e7e..c650930 100644 --- a/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java +++ b/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java @@ -14,17 +14,23 @@ import frc.robot.constants.ElevatorConstants; import frc.robot.subsystems.Elevator; public class ElevatorSysID extends Elevator { - private MutVoltage appliedVoltage = Volts.mutable(0);; + private MutVoltage appliedVoltage; - private MutDistance elevatorPosition = Inches.mutable(0);; + private MutDistance elevatorPosition; - private MutLinearVelocity elevatorVelocity = InchesPerSecond.mutable(0); + private MutLinearVelocity elevatorVelocity; private SysIdRoutine routine; public ElevatorSysID() { super(); + appliedVoltage = Volts.mutable(0); + + elevatorPosition = Inches.mutable(0); + + elevatorVelocity = InchesPerSecond.mutable(0); + routine = new SysIdRoutine( ElevatorConstants.kSysIDConfig, new SysIdRoutine.Mechanism(