From 1ff2c870ddcab505512fd349a82e103a9f847434 Mon Sep 17 00:00:00 2001 From: Mihir Patankar Date: Sat, 30 Mar 2024 18:40:26 -0400 Subject: [PATCH] add auto trapping --- src/main/java/frc/robot/FieldConstants.java | 4 +++ src/main/java/frc/robot/Robot.java | 11 ++++--- src/main/java/frc/robot/arm/Arm.java | 31 +++++++++---------- src/main/java/frc/robot/arm/ArmIOTalonFX.java | 16 +++++----- src/main/java/frc/robot/climber/Climber.java | 15 ++++++--- .../frc/robot/drivetrain/DriveToPose.java | 9 ++++-- 6 files changed, 53 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 290915a..4606dcc 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -1,6 +1,8 @@ package frc.robot; import com.pathplanner.lib.util.GeometryUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; @@ -8,6 +10,8 @@ public class FieldConstants { public static final double kFieldLength = Constants.fieldLayout.getFieldLength(); public static final double kFieldWidth = Constants.fieldLayout.getFieldWidth(); + public static final Pose2d trapPose = + GeometryUtil.flipFieldPose(new Pose2d(11.896, 4.658, new Rotation2d(-2.093))); public static class NotePositions { private static final double kCenterlineX = kFieldLength / 2.0; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f1243e2..c83b21f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,6 +17,7 @@ import frc.robot.arm.Arm; import frc.robot.arm.Arm.ArmSetpoints; import frc.robot.climber.Climber; +import frc.robot.drivetrain.DriveToPose; import frc.robot.drivetrain.Drivetrain; import frc.robot.drivetrain.WheelRadiusCharacterization; import frc.robot.drivetrain.WheelRadiusCharacterization.Direction; @@ -231,10 +232,12 @@ public Robot() { m_driverController.a().onTrue(m_arm.goToSetpoint(ArmSetpoints.kClimb)); m_driverController.b().onTrue(m_arm.goToSetpoint(ArmSetpoints.kTrap)); m_driverController.b().whileTrue(Commands.parallel(m_climber.windWinch())); - - // m_driverController.x().onTrue(m_arm.goToSetpoint(ArmSetpoints.kTrap)); - m_driverController.y().whileTrue(Commands.parallel(m_climber.windWinch())); - + m_driverController + .x() + .whileTrue(Superstructure.trapRoutine(m_arm, m_climber, m_shooter, m_feeder)); + m_driverController + .y() + .whileTrue(new DriveToPose(FieldConstants.trapPose, m_drivetrain, m_lights)); m_driverController.povRight().onTrue(m_shooter.trapShot()); m_driverController.leftBumper().whileTrue(Superstructure.sensorIntake(m_feeder, m_intake)); diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index 1847d85..f4f7eb2 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -207,26 +208,24 @@ public Command aimWrist(double wristAngleRad) { } public Command aimElbowForTuning(DoubleSupplier driveEffort) { - return this.run( - () -> { - m_io.setWristRotations(Units.radiansToRotations(ArmSetpoints.kStowed.wristAngle)); - final double driveVoltage = - MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 3.0; - if (driveVoltage == 0.0) m_io.setElbowRotations(m_inputs.elbowPositionRot); - else m_io.setElbowVoltage(driveVoltage); - }) + var state = + new Object() { + double currentPos = ArmSetpoints.kStowed.elbowAngle; + }; + return Commands.run( + () -> state.currentPos += MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 0.05) + .alongWith(aim(() -> 0, () -> state.currentPos)) .finallyDo(() -> holdSetpoint().schedule()); } public Command aimWristForTuning(DoubleSupplier driveEffort) { - return this.run( - () -> { - m_io.setElbowRotations(Units.radiansToRotations(ArmSetpoints.kStowed.elbowAngle)); - final double driveVoltage = - MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 1.0; - if (driveVoltage == 0.0) m_io.setWristRotations(m_inputs.wristPositionRot); - else m_io.setWristVoltage(driveVoltage); - }) + var state = + new Object() { + double currentPos = ArmSetpoints.kStowed.wristAngle; + }; + return Commands.run( + () -> state.currentPos += MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 0.05) + .alongWith(aim(() -> 1, () -> state.currentPos)) .finallyDo(() -> holdSetpoint().schedule()); } diff --git a/src/main/java/frc/robot/arm/ArmIOTalonFX.java b/src/main/java/frc/robot/arm/ArmIOTalonFX.java index e5c74cc..fc9397d 100644 --- a/src/main/java/frc/robot/arm/ArmIOTalonFX.java +++ b/src/main/java/frc/robot/arm/ArmIOTalonFX.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.hardware.ParentDevice; import com.ctre.phoenix6.hardware.TalonFX; @@ -24,7 +25,8 @@ public class ArmIOTalonFX implements ArmIO { m_elbowRightFollowerMotor, m_wristLeftMotor, m_wristRightFollowerMotor; - private final MotionMagicVoltage m_elbowControl, m_wristControl; + private final PositionVoltage m_elbowControl; + private final MotionMagicVoltage m_wristControl; private final StatusSignal m_elbowPositionSignal, m_elbowPositionSetpointSignal, m_elbowVelocitySignal, @@ -55,7 +57,7 @@ public ArmIOTalonFX() { final TalonFXConfiguration elbowLeftMotorConfig = new TalonFXConfiguration(); elbowLeftMotorConfig.CurrentLimits = currentLimitsConfig; elbowLeftMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - elbowLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + elbowLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; elbowLeftMotorConfig.Feedback.SensorToMechanismRatio = ArmConstants.kElbowReduction; elbowLeftMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; elbowLeftMotorConfig.Slot0.kP = 62.122; @@ -66,6 +68,7 @@ public ArmIOTalonFX() { elbowLeftMotorConfig.Slot0.kG = 0.37766; elbowLeftMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 0.5; elbowLeftMotorConfig.MotionMagic.MotionMagicAcceleration = 2.0; + elbowLeftMotorConfig.MotorOutput.DutyCycleNeutralDeadband = 0.01; elbowLeftMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40.0; elbowLeftMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40.0; elbowLeftMotorConfig.Voltage.PeakForwardVoltage = 6.0; @@ -75,7 +78,7 @@ public ArmIOTalonFX() { final TalonFXConfiguration wristLeftMotorConfig = new TalonFXConfiguration(); wristLeftMotorConfig.CurrentLimits = currentLimitsConfig; wristLeftMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - wristLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + wristLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; wristLeftMotorConfig.ClosedLoopGeneral.ContinuousWrap = true; wristLeftMotorConfig.Feedback.SensorToMechanismRatio = ArmConstants.kWristReduction; wristLeftMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; @@ -89,6 +92,7 @@ public ArmIOTalonFX() { wristLeftMotorConfig.MotionMagic.MotionMagicAcceleration = 2.0; wristLeftMotorConfig.Voltage.PeakForwardVoltage = 3.0; wristLeftMotorConfig.Voltage.PeakReverseVoltage = -3.0; + wristLeftMotorConfig.MotorOutput.DutyCycleNeutralDeadband = 0.01; wristLeftMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40.0; wristLeftMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40.0; @@ -141,10 +145,8 @@ public ArmIOTalonFX() { ParentDevice.optimizeBusUtilizationForAll( m_elbowLeftMotor, m_elbowRightFollowerMotor, m_wristLeftMotor, m_wristRightFollowerMotor); - m_elbowControl = - new MotionMagicVoltage(0.0, true, 0.0, 0, false, false, false).withUpdateFreqHz(0.0); - m_wristControl = - new MotionMagicVoltage(0.0, true, 0.0, 0, false, false, false).withUpdateFreqHz(0.0); + m_elbowControl = new PositionVoltage(0.0, 0.0, true, 0.0, 0, false, false, false); + m_wristControl = new MotionMagicVoltage(0.0, true, 0.0, 0, false, false, false); } @Override diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java index 124053f..ce7e189 100644 --- a/src/main/java/frc/robot/climber/Climber.java +++ b/src/main/java/frc/robot/climber/Climber.java @@ -48,9 +48,16 @@ public Command unwindWinch() { public Command goToTrapLimit() { return Commands.parallel( - this.run(() -> m_io.setLeftVoltage(12.0)) - .until(() -> m_inputs.leftWinchPositionRot > kTrapLimit), - this.run(() -> m_io.setRightVoltage(12.0)) - .until(() -> m_inputs.leftWinchPositionRot > kTrapLimit)); + this.run( + () -> { + if (m_inputs.leftWinchPositionRot <= kTrapLimit) m_io.setLeftVoltage(12.0); + else m_io.setLeftVoltage(0.0); + if (m_inputs.leftWinchPositionRot <= kTrapLimit) m_io.setRightVoltage(12.0); + else m_io.setRightVoltage(0.0); + })) + .until( + () -> + m_inputs.leftWinchPositionRot > kTrapLimit + && m_inputs.leftWinchPositionRot > kTrapLimit); } } diff --git a/src/main/java/frc/robot/drivetrain/DriveToPose.java b/src/main/java/frc/robot/drivetrain/DriveToPose.java index e74486f..012ccdb 100644 --- a/src/main/java/frc/robot/drivetrain/DriveToPose.java +++ b/src/main/java/frc/robot/drivetrain/DriveToPose.java @@ -11,20 +11,23 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.PoseEstimation; +import frc.robot.lights.Lights; import org.littletonrobotics.junction.Logger; public class DriveToPose extends Command { private final Drivetrain m_drivetrain; + private final Lights m_lights; private final ProfiledPIDController m_translationController, m_thetaController; private Translation2d m_lastSetpointTranslation; private Pose2d m_pose; private Pose2d m_goalPose; - public DriveToPose(Pose2d pose, Drivetrain drivetrain) { + public DriveToPose(Pose2d pose, Drivetrain drivetrain, Lights lights) { m_pose = pose; m_translationController = new ProfiledPIDController(2.0, 0.0, 0.0, new TrapezoidProfile.Constraints(3.5, 2.2)); @@ -32,11 +35,13 @@ public DriveToPose(Pose2d pose, Drivetrain drivetrain) { new ProfiledPIDController( 5.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, 4 * Math.PI)); this.m_drivetrain = drivetrain; - addRequirements(drivetrain); + this.m_lights = lights; + addRequirements(drivetrain, lights); } @Override public void initialize() { + m_lights.setBlink(Color.kWhite); m_goalPose = Constants.onRedAllianceSupplier.getAsBoolean() ? GeometryUtil.flipFieldPose(m_pose)