diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json new file mode 100644 index 0000000..7535ebb --- /dev/null +++ b/.DataLogTool/datalogtool.json @@ -0,0 +1,6 @@ +{ + "download": { + "localDir": "C:\\Users\\infin\\Downloads", + "serverTeam": "2648" + } +} diff --git a/.SysId/sysid.json b/.SysId/sysid.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.SysId/sysid.json @@ -0,0 +1 @@ +{} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 046f1b9..ff53e13 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,12 +9,15 @@ import frc.robot.constants.ClimberPivotConstants; import frc.robot.constants.ElevatorConstants; import frc.robot.constants.OIConstants; import frc.robot.subsystems.ManipulatorPivot; +import frc.robot.subsystems.sysid.ElevatorSysID; import frc.robot.subsystems.ClimberPivot; import frc.robot.subsystems.ClimberRollers; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Manipulator; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; @@ -35,6 +38,7 @@ public class RobotContainer { private Drivetrain drivetrain; private Elevator elevator; + //private ElevatorSysID elevator; private Manipulator manipulator; @@ -53,6 +57,7 @@ public class RobotContainer { drivetrain = new Drivetrain(); elevator = new Elevator(); + //elevator = new ElevatorSysID(); manipulator = new Manipulator(); @@ -64,12 +69,22 @@ public class RobotContainer { autoChooser = AutoBuilder.buildAutoChooser(); configureButtonBindings(); + //elevatorSysIDBindings(); configureNamedCommands(); configureShuffleboard(); } + /*private void elevatorSysIDBindings() { + elevator.setDefaultCommand(elevator.maintainPosition()); + + operator.a().whileTrue(elevator.sysIdQuasistatic(Direction.kForward)); + operator.b().whileTrue(elevator.sysIdQuasistatic(Direction.kReverse)); + operator.x().whileTrue(elevator.sysIdDynamic(Direction.kForward)); + operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse)); + }*/ + private void configureButtonBindings() { //Default commands climberPivot.setDefaultCommand( @@ -91,7 +106,7 @@ public class RobotContainer { elevator.setDefaultCommand( elevator.runManualElevator( - () -> -operator.getLeftY() + () -> -operator.getLeftY() * .5 ) ); @@ -134,8 +149,12 @@ public class RobotContainer { driver.povRight().whileTrue(climberRollers.runRoller(-0.5)); */ - operator.povUp().whileTrue( - elevator.goToSetpoint(20) + operator.povUp().onTrue( + elevator.goToSetpoint(() -> 20) + ); + + operator.povDown().onTrue( + elevator.goToSetpoint(() -> 0) ); /* @@ -233,6 +252,10 @@ public class RobotContainer { .withSize(1, 1) .withPosition(4, 1) .withWidget(BuiltInWidgets.kBooleanBox); + + sensorTab.addDouble("ElevMotor1", elevator::getMotor1); + + sensorTab.addDouble("ElevMotor2", elevator::getMotor2); } public Command getAutonomousCommand() { @@ -317,13 +340,13 @@ public class RobotContainer { return Commands.either( Commands.either( - elevator.goToSetpoint(elevatorPosition).andThen(manipulatorPivot.goToSetpoint(armPosition)), - elevator.goToSetpoint(elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(armPosition)), + elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(armPosition)), + elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(armPosition)), () -> sequential ), Commands.either( - manipulatorPivot.goToSetpoint(armPosition).andThen(elevator.goToSetpoint(elevatorPosition)), - manipulatorPivot.goToSetpoint(armPosition).alongWith(elevator.goToSetpoint(elevatorPosition)), + manipulatorPivot.goToSetpoint(armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)), + manipulatorPivot.goToSetpoint(armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)), () -> sequential ), () -> elevatorFirst @@ -335,14 +358,14 @@ public class RobotContainer { if(!isL4){ return Commands.sequence( manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition), - elevator.goToSetpoint(elevatorPosition), + elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(armPosition)); }else{ return Commands.sequence( manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition), - elevator.goToSetpoint(elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition), - Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(elevatorPosition)); + elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition), + Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(() -> elevatorPosition)); } } diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 60a9444..073e436 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -21,24 +21,18 @@ public class ElevatorConstants { public static final int kCurrentLimit = 40; - public static final double kPositionControllerP = 0.1; + public static final double kPositionControllerP = 3; // public static final double kPositionControllerI = 0; - public static final double kPositionControllerD = 0; - - public static final double kAllowedError = 0.5; - - /* - public static final double kVelocityControllerP = 0; - public static final double kVelocityControllerI = 0; - public static final double kVelocityControllerD = 0; - */ + public static final double kPositionControllerD = 0.1;//0.35 + + public static final double kAllowedError = 0.75; public static final double kFeedForwardS = 0; - public static final double kFeedForwardG = 0.53; // calculated value - public static final double kFeedForwardV = 4.78; // calculated value + public static final double kFeedForwardG = .7;//1; // calculated value .6 + public static final double kFeedForwardV = 0.12; // calculated value .12 INCHES NOT METERS public static final double kMaxVelocity = 120.0; // 120 inches per second (COOKING) calculated max is 184 in/s - public static final double kMaxAcceleration = 500.0; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2 + public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2 public static final double kCoralIntakePosition = 0; public static final double kL1Position = 0; @@ -51,11 +45,11 @@ public class ElevatorConstants { public static final double kProcessorPosition = 4.0; /**The position of the top of the elevator brace */ public static final double kBracePosition = 5.5; - public static final double kMaxHeight = 53.0; + public static final double kMaxHeight = 47.5; //actual is 53 // 1, 7, 10 are the defaults for these, change as necessary - public static final double kSysIDRampRate = 1; - public static final double kSysIDStepVolts = 7; + public static final double kSysIDRampRate = .25; + public static final double kSysIDStepVolts = 3; public static final double kSysIDTimeout = 10; public static final IdleMode kIdleMode = IdleMode.kBrake; @@ -76,10 +70,9 @@ public class ElevatorConstants { .idleMode(kIdleMode) .inverted(true); motorConfig.encoder - //.inverted(true) - .positionConversionFactor(kEncoderConversionFactor); - /* - motorConfig.closedLoop + .positionConversionFactor(kEncoderConversionFactor) + .velocityConversionFactor(kEncoderConversionFactor); + /*motorConfig.closedLoop .p(kPositionControllerP) .i(kPositionControllerI) .d(kPositionControllerD) @@ -87,8 +80,7 @@ public class ElevatorConstants { motorConfig.closedLoop.maxMotion .maxAcceleration(kMaxAcceleration) .maxVelocity(kMaxVelocity) - .allowedClosedLoopError(kAllowedError); - */ + .allowedClosedLoopError(kAllowedError);*/ } public static final SparkMaxConfig motorConfig2 = new SparkMaxConfig(); diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 5bfab7e..03e0ae6 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -15,6 +15,8 @@ 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.controller.ProfiledPIDController; +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; @@ -30,7 +32,7 @@ public class Elevator extends SubsystemBase { private DigitalInput bottomLimitSwitch; - private PIDController pidController; + private ProfiledPIDController pidController; private ElevatorFeedforward feedForward; @@ -52,7 +54,7 @@ public class Elevator extends SubsystemBase { ); elevatorMotor2.configure( - ElevatorConstants.motorConfig2.follow(ElevatorConstants.kElevatorMotor1ID), + ElevatorConstants.motorConfig.follow(ElevatorConstants.kElevatorMotor1ID), ResetMode.kResetSafeParameters, PersistMode.kPersistParameters ); @@ -65,10 +67,14 @@ public class Elevator extends SubsystemBase { ElevatorConstants.kBottomLimitSwitchID ); - pidController = new PIDController( + pidController = new ProfiledPIDController( ElevatorConstants.kPositionControllerP, ElevatorConstants.kPositionControllerI, - ElevatorConstants.kPositionControllerD + ElevatorConstants.kPositionControllerD, + new TrapezoidProfile.Constraints( + ElevatorConstants.kMaxVelocity, + ElevatorConstants.kMaxAcceleration + ) ); pidController.setTolerance(ElevatorConstants.kAllowedError); @@ -115,8 +121,15 @@ public class Elevator extends SubsystemBase { */ public Command runManualElevator(DoubleSupplier speed) { return run(() -> { - //elevatorClosedLoop.setReference(speed.getAsDouble(), ControlType.kDutyCycle); - elevatorMotor1.set(speed.getAsDouble()); + elevatorMotor1.set( + speed.getAsDouble() + ); + }); + } + + public Command maintainPosition() { + return run(() -> { + elevatorMotor1.setVoltage(feedForward.calculate(0)); }); } @@ -127,27 +140,34 @@ public class Elevator extends SubsystemBase { * @param timeout Time to achieve the setpoint before quitting * @return Sets motor voltage to achieve the target destination */ - public Command goToSetpoint(double setpoint) { + public Command goToSetpoint(DoubleSupplier setpoint) { double clampedSetpoint = MathUtil.clamp( - setpoint, + setpoint.getAsDouble(), 0, ElevatorConstants.kMaxHeight ); return run(() -> { - elevatorMotor1.setVoltage( - pidController.calculate( - encoder.getPosition(), - clampedSetpoint - ) + feedForward.calculate(0) - ); - /* - elevatorClosedLoop.setReference(clampedSetpoint, - SparkBase.ControlType.kMAXMotionPositionControl, + /* + if(pidController.atGoal()) { + elevatorMotor1.setVoltage(feedForward.calculate(0)); + } else { +*/ + elevatorMotor1.setVoltage( + pidController.calculate( + encoder.getPosition(), + clampedSetpoint + ) + feedForward.calculate(pidController.getSetpoint().velocity) + ); + + + /*elevatorClosedLoop.setReference( + clampedSetpoint, + ControlType.kMAXMotionPositionControl, ClosedLoopSlot.kSlot0, feedForward.calculate(0) - ); - */ + );*/ + }); } @@ -170,4 +190,12 @@ public class Elevator extends SubsystemBase { public boolean getBottomLimitSwitch() { return bottomLimitSwitch.get(); } + + public double getMotor1() { + return elevatorMotor1.getOutputCurrent(); + } + + public double getMotor2() { + return elevatorMotor2.getOutputCurrent(); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java index 0fd5018..4f0d009 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java @@ -92,7 +92,7 @@ public class ManipulatorPivot extends SubsystemBase { return run(() -> { pivotClosedLoopController.setReference(clampedSetpoint, SparkBase.ControlType.kMAXMotionPositionControl, - ClosedLoopSlot.kSlot0, + ClosedLoopSlot.kSlot1, feedForward.calculate(absoluteEncoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, absoluteEncoder.getVelocity())); }); } diff --git a/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java b/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java index 6d3b70d..fdb7e7e 100644 --- a/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java +++ b/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java @@ -1,8 +1,8 @@ package frc.robot.subsystems.sysid; -import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Volts; -import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.InchesPerSecond; import edu.wpi.first.units.measure.MutDistance; import edu.wpi.first.units.measure.MutLinearVelocity; @@ -14,23 +14,17 @@ import frc.robot.constants.ElevatorConstants; import frc.robot.subsystems.Elevator; public class ElevatorSysID extends Elevator { - private MutVoltage appliedVoltage; + private MutVoltage appliedVoltage = Volts.mutable(0);; - private MutDistance elevatorPosition; + private MutDistance elevatorPosition = Inches.mutable(0);; - private MutLinearVelocity elevatorVelocity; + private MutLinearVelocity elevatorVelocity = InchesPerSecond.mutable(0); private SysIdRoutine routine; public ElevatorSysID() { super(); - appliedVoltage = Volts.mutable(0); - - elevatorPosition = Meters.mutable(0); - - elevatorVelocity = MetersPerSecond.mutable(0); - routine = new SysIdRoutine( ElevatorConstants.kSysIDConfig, new SysIdRoutine.Mechanism( @@ -41,10 +35,10 @@ public class ElevatorSysID extends Elevator { elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts )) .linearPosition(elevatorPosition.mut_replace( - encoder.getPosition(), Meters + encoder.getPosition(), Inches )) .linearVelocity(elevatorVelocity.mut_replace( - encoder.getVelocity(), MetersPerSecond + encoder.getVelocity(), InchesPerSecond )); }, this @@ -52,6 +46,11 @@ public class ElevatorSysID extends Elevator { ); } + @Override + public void periodic() { + + } + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return routine.quasistatic(direction); }