From 534ba8477c95f47b78331eff03269ad0f4b732ca Mon Sep 17 00:00:00 2001 From: Oscar Date: Mon, 13 Mar 2023 11:51:20 -0700 Subject: [PATCH 01/12] Made Butterscotch --- src/main/java/frc/robot/Constants.java | 14 +-- src/main/java/frc/robot/Robot.java | 20 +++- src/main/java/frc/robot/RobotContainer.java | 7 +- .../java/frc/robot/autos/ButterAutoTest.java | 35 ++++++ .../robot/commands/CustomSwerveTrajTrap.java | 102 ++++++++++++++++++ .../commands/CustomSwerveTrajectory.java | 82 ++++++++++++++ .../java/frc/robot/subsystems/Swerve.java | 13 ++- 7 files changed, 256 insertions(+), 17 deletions(-) create mode 100644 src/main/java/frc/robot/autos/ButterAutoTest.java create mode 100644 src/main/java/frc/robot/commands/CustomSwerveTrajTrap.java create mode 100644 src/main/java/frc/robot/commands/CustomSwerveTrajectory.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ad3b4db..554a4d5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -45,13 +45,13 @@ public static final class Swerve { public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW- /* Drivetrain Constants */ - public static final double trackWidth = Units.inchesToMeters(24) /* 0.521 */; - public static final double wheelBase = Units.inchesToMeters(25) /* 0.622 */; + public static final double trackWidth = Units.inchesToMeters(17.5) /* 0.521 */; + public static final double wheelBase = Units.inchesToMeters(18.5) /* 0.622 */; public static final double wheelDiameter = Units.inchesToMeters(3.94); public static final double wheelCircumference = wheelDiameter * Math.PI; public static final double openLoopRamp = 0.2; - public static final double closedLoopRamp = 0.5; + public static final double closedLoopRamp = 0.61 ; public static final double driveGearRatio = (6.12 / 1.0); //8:14:1 public static final double angleGearRatio = (12.8 / 1.0); //12.8:1 @@ -111,7 +111,7 @@ public static final class Mod0 { public static final int driveMotorID = /* 2 */5; public static final int angleMotorID = /* 1 */4; public static final int canCoderID = /* 3 */6; - public static final double angleOffset = /* 7.9 */ 304; + public static final double angleOffset = /* 274; */ 13; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -121,7 +121,7 @@ public static final class Mod1 { public static final int driveMotorID = /* 11 */11; public static final int angleMotorID = /* 10 */10; public static final int canCoderID = /* 12 */12; - public static final double angleOffset = /* 74 */218 ; + public static final double angleOffset = /* 240 ; */ 218; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -131,7 +131,7 @@ public static final class Mod2 { public static final int driveMotorID = /* 5 */2; public static final int angleMotorID = /* 4 */1; public static final int canCoderID = /* 6 */3; - public static final double angleOffset = /* 307 */312; + public static final double angleOffset = /* 319; */ 310; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -141,7 +141,7 @@ public static final class Mod3 { public static final int driveMotorID = /* 8 */8; public static final int angleMotorID = /* 7 */7; public static final int canCoderID = /* 9 */9; - public static final double angleOffset = /* 146 */ 289 ; + public static final double angleOffset = /* 305 ; */ 288; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ebec9ad..efc2e56 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,12 +5,17 @@ package frc.robot; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.lib.math.Conversions; import frc.robot.subsystems.CANdleSub; /** @@ -53,7 +58,6 @@ public void robotInit() { */ @Override public void robotPeriodic() { - m_robotContainer.s_Swerve.updateOdometry(); CommandScheduler.getInstance().run(); } @@ -71,6 +75,7 @@ public void disabledPeriodic() { @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + m_robotContainer.s_Swerve.resetOdometry(new Pose2d(Units.inchesToMeters(72), Units.inchesToMeters(176), new Rotation2d(0))); // schedule the autonomous command (example) if (m_autonomousCommand != null) { @@ -80,12 +85,17 @@ public void autonomousInit() { /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + m_robotContainer.s_Swerve.updateOdometry(); + + } @Override public void teleopInit() { - m_robotContainer.s_Swerve.normalizeOdometry(); - m_robotContainer.s_Swerve.autoZeroGyro(); + // m_robotContainer.s_Swerve.normalizeOdometry(); + // m_robotContainer.s_Swerve.autoZeroGyro(); + m_robotContainer.s_Swerve.zeroGyro(); + m_robotContainer.s_Swerve.resetOdometry(new Pose2d(Units.inchesToMeters(72), Units.inchesToMeters(176), new Rotation2d(0))); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove @@ -100,6 +110,8 @@ public void teleopInit() { public void teleopPeriodic() { Alliance alliance; alliance = DriverStation.getAlliance(); +m_robotContainer.s_Swerve.updateOdometry(); + // System.out.println(alliance); // candlesub.enabledLed(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 21a2b6e..9386b7c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -140,11 +140,13 @@ private void configureButtonBindings() { xboxRightJoyButton.whileTrue(new HoldArmPos(armSub, ArmPositions.CONE_UPRIGHT).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // leftBumper.toggleOnTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); leftBumper.whileTrue(new HoldArmPos(armSub, ArmPositions.SHELF_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); rightBumper.toggleOnTrue(new ShelfIntake(gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - operatorDeploy.onTrue(new TurnInPlace(s_Swerve, true, false, 0)); + // operatorDeploy.onTrue(new TurnInPlace(s_Swerve, true, false, 0)); + operatorDeploy.onTrue(new CustomSwerveTrajectory(s_Swerve, 10, 4, 0, 3, 10)); // operatorRightEmpty.onTrue(new BalanceAuto(s_Swerve)); // TODO RED SIDE // LeftLittleButton.onTrue(new AutoPlaceRed(s_Swerve, armSub, gripper)) ; @@ -186,7 +188,8 @@ public Command getAutonomousCommand() { // An ExampleCommand will run in autonomous // return new BalanceAuto(s_Swerve, armSub, gripper); // return new BalanceAutoRed(s_Swerve, armSub, gripper); - return m_chooser.getSelected(); + // return m_chooser.getSelected(); + return new ButterAutoTest(s_Swerve); // return new LeftSideAuto(s_Swerve, armSub, gripper); // return new LeftSideAutoRed(s_Swerve, armSub, gripper); } diff --git a/src/main/java/frc/robot/autos/ButterAutoTest.java b/src/main/java/frc/robot/autos/ButterAutoTest.java new file mode 100644 index 0000000..8f10373 --- /dev/null +++ b/src/main/java/frc/robot/autos/ButterAutoTest.java @@ -0,0 +1,35 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.autos; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.CustomSwerveTrajTrap; +import frc.robot.commands.CustomSwerveTrajectory; +import frc.robot.subsystems.Swerve; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class ButterAutoTest extends SequentialCommandGroup { + Swerve swerve; + /** Creates a new ButterAutoTest. */ + public ButterAutoTest(Swerve swerve) { + this.swerve = swerve; + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + new CustomSwerveTrajectory(swerve, 6.86, 4.56, 0, 0.3, 4), + new CustomSwerveTrajectory(swerve, 3 , 4.97, 180, 0.3, 4), + new CustomSwerveTrajectory(swerve, 6.86, 4.56, 0, 0.3, 4), + new CustomSwerveTrajectory(swerve, 3 , 4.97, 180, 0.3, 4) + +/* new CustomSwerveTrajTrap(swerve, 6.86, 4.56, 0, 0.1, 6), +new CustomSwerveTrajTrap(swerve, 3 , 4.97, 180, 0.1, 6), +new CustomSwerveTrajTrap(swerve, 6.86, 4.56, 0, 0.1, 6), +new CustomSwerveTrajTrap(swerve, 3 , 4.97, 180, 0.1, 6) + */ + ); + } +} diff --git a/src/main/java/frc/robot/commands/CustomSwerveTrajTrap.java b/src/main/java/frc/robot/commands/CustomSwerveTrajTrap.java new file mode 100644 index 0000000..e1feb78 --- /dev/null +++ b/src/main/java/frc/robot/commands/CustomSwerveTrajTrap.java @@ -0,0 +1,102 @@ +package frc.robot.commands; + +import frc.robot.Constants; +import frc.robot.subsystems.Swerve; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.CommandBase; + + +public class CustomSwerveTrajTrap extends CommandBase { + + private double rotation; + private Translation2d translation; + private boolean fieldRelative; + private boolean openLoop; + + private Swerve s_Swerve; + private Joystick controller; + private int translationAxis; + private int strafeAxis; + private int rotationAxis; + + ProfiledPIDController xPID; + ProfiledPIDController yPID; + PIDController rotPID; + + double goalX; + double goalY; + double goalRot; + + double maxSpeed; + double maxAngularVelocity; + + TrapezoidProfile.Constraints constraints; + + + /** + * Driver control + */ + public CustomSwerveTrajTrap(Swerve s_Swerve, double goalX, double goalY, double goalRot, double maxSpeed, double maxAngularVelocity) { + this.s_Swerve = s_Swerve; + addRequirements(s_Swerve); + this.goalX = goalX; + this.goalY = goalY; + this.goalRot = goalRot; + this.maxSpeed = maxSpeed; + this.maxAngularVelocity = maxAngularVelocity; + + + constraints = new TrapezoidProfile.Constraints(maxSpeed, 1); + xPID = new ProfiledPIDController(/* 0.65 */2, 0.0, 0.02,constraints ); + yPID = new ProfiledPIDController(/* 0.65 */2, 0.0, 0.02, constraints); + rotPID = new PIDController(0.003, 0.0, 0); + + xPID.setTolerance(0.025); + yPID.setTolerance(0.025); + rotPID.setTolerance(1); + rotPID.enableContinuousInput(-180, 180); + + + + + + + } + @Override + public void initialize() { + + xPID.reset(s_Swerve.getPose().getX()); + yPID.reset(s_Swerve.getPose().getY()); + + xPID.setGoal(new TrapezoidProfile.State(goalX, 0)); + yPID.setGoal(new TrapezoidProfile.State(goalY, 0)); + + } + + + @Override + public void execute() { + double yAxis = xPID.calculate(s_Swerve.getPose().getX()); + double xAxis = yPID.calculate(s_Swerve.getPose().getY()); + double rAxis = rotPID.calculate(s_Swerve.getPose().getRotation().getDegrees(), goalRot); + + /* Deadbands */ + + + + translation = new Translation2d(yAxis, xAxis).times(11.5); + rotation = rAxis * maxAngularVelocity; + s_Swerve.drive(translation, rotation, true, false); + } + + @Override + public boolean isFinished() { + // return swerveControllerCommand.isFinished(); + return xPID.atSetpoint() && yPID.atSetpoint() && rotPID.atSetpoint(); + } + +} diff --git a/src/main/java/frc/robot/commands/CustomSwerveTrajectory.java b/src/main/java/frc/robot/commands/CustomSwerveTrajectory.java new file mode 100644 index 0000000..6e928ef --- /dev/null +++ b/src/main/java/frc/robot/commands/CustomSwerveTrajectory.java @@ -0,0 +1,82 @@ +package frc.robot.commands; + +import frc.robot.Constants; +import frc.robot.subsystems.Swerve; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; + + +public class CustomSwerveTrajectory extends CommandBase { + + private double rotation; + private Translation2d translation; + private boolean fieldRelative; + private boolean openLoop; + + private Swerve s_Swerve; + private Joystick controller; + private int translationAxis; + private int strafeAxis; + private int rotationAxis; + + PIDController xPID; + PIDController yPID; + PIDController rotPID; + + double goalX; + double goalY; + double goalRot; + + double maxSpeed; + double maxAngularVelocity; + + + /** + * Driver control + */ + public CustomSwerveTrajectory(Swerve s_Swerve, double goalX, double goalY, double goalRot, double maxSpeed, double maxAngularVelocity) { + this.s_Swerve = s_Swerve; + addRequirements(s_Swerve); + + xPID = new PIDController(/* 0.65 */2, 0.0, 0.05); + yPID = new PIDController(/* 0.65 */2, 0.0, 0.05); + rotPID = new PIDController(0.003, 0.0, 0); + + xPID.setTolerance(0.025); + yPID.setTolerance(0.025); + rotPID.setTolerance(1); + rotPID.enableContinuousInput(-180, 180); + + + + this.goalX = goalX; + this.goalY = goalY; + this.goalRot = goalRot; + this.maxSpeed = maxSpeed; + this.maxAngularVelocity = maxAngularVelocity; + } + + @Override + public void execute() { + double yAxis = xPID.calculate(s_Swerve.getPose().getX(), goalX); + double xAxis = yPID.calculate(s_Swerve.getPose().getY(), goalY); + double rAxis = rotPID.calculate(s_Swerve.getPose().getRotation().getDegrees(), goalRot); + + /* Deadbands */ + + + + translation = new Translation2d(yAxis, xAxis).times(maxSpeed); + rotation = rAxis * maxAngularVelocity; + s_Swerve.drive(translation, rotation, true, false); + } + + @Override + public boolean isFinished() { + // return swerveControllerCommand.isFinished(); + return xPID.atSetpoint() && yPID.atSetpoint() && rotPID.atSetpoint(); + } + +} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 2a1e990..f5d8c09 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -130,9 +130,9 @@ public void updateOdometry() { m_poseEstimator.addVisionMeasurement( camPose.estimatedPose.toPose2d(), camPose.timestampSeconds); - m_fieldSim.getObject("Cam Est Pos").setPose( m_poseEstimator.getEstimatedPosition() ); + // m_fieldSim.getObject("Cam Est Pos").setPose( m_poseEstimator.getEstimatedPosition() ); } else { - m_fieldSim.getObject("Cam Est Pos").setPose(m_poseEstimator.getEstimatedPosition()); + // m_fieldSim.getObject("Cam Est Pos").setPose(m_poseEstimator.getEstimatedPosition()); } m_poseEstimator.update(getYaw(), new SwerveModulePosition[] { @@ -141,7 +141,7 @@ public void updateOdometry() { m_backRight.getPosition(), m_backLeft.getPosition()}); - m_fieldSim.setRobotPose(m_poseEstimator.getEstimatedPosition()); + m_fieldSim.setRobotPose(getPose()); SmartDashboard.putNumber("lmao X", m_poseEstimator.getEstimatedPosition().getX()); SmartDashboard.putNumber("lmao Y", m_poseEstimator.getEstimatedPosition().getY()); SmartDashboard.putBoolean("AprilTag in View", result.isPresent()); @@ -195,7 +195,12 @@ public Pose2d getGlobalPose(){ public void resetOdometry(Pose2d pose) { // swerveOdometry.resetPosition(getYaw(), mSwerveModulePositions, pose); - swerveOdometry.resetPosition(getYaw(), mSwerveModulePositions, new Pose2d()); + swerveOdometry.resetPosition(getYaw(), new SwerveModulePosition[] { + m_frontRight.getPosition(), + m_frontLeft.getPosition(), + m_backRight.getPosition(), + m_backLeft.getPosition() + }, pose); } public void normalizeOdometry(){ From 5fb10e8383f58e3b9d2143ada4871dd21b4d60cb Mon Sep 17 00:00:00 2001 From: Oscar Date: Mon, 13 Mar 2023 16:28:22 -0700 Subject: [PATCH 02/12] Lmao --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../frc/robot/commands/GripperCommand.java | 6 +- .../java/frc/robot/subsystems/ArmSub.java | 111 +++++++++++++----- .../java/frc/robot/subsystems/Gripper.java | 16 ++- 4 files changed, 103 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9386b7c..a07d3aa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -126,10 +126,11 @@ private void configureButtonBindings() { operatorIntakeDown.whileTrue(new HoldArmPos(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); operatorIntakeDown.whileFalse(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); operatorLatch.toggleOnTrue(new InstantCommand(() -> gripper.setClaw(GripperState.CONE)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - operatorHook.toggleOnTrue(new InstantCommand(() -> gripper.setClaw(GripperState.OPEN)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // operatorHook.toggleOnTrue(new InstantCommand(() -> gripper.setClaw(GripperState.OPEN)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); operatorSpinup.whileTrue(new LockToGamePiece(s_Swerve, joystickPanel, translationAxis, strafeAxis, true, false, GlobalVariables.gamePiece).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); operatorLeftEmpty.toggleOnTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + operatorHook.whileTrue(new GripperCommand(gripper, 0.4)); rightJoy.onTrue(new InstantCommand(() -> s_Swerve.zeroGyro())); leftJoy.whileTrue(new LockToGamePiece(s_Swerve, joystickPanel, translationAxis, strafeAxis, true, true, GlobalVariables.gamePiece).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); diff --git a/src/main/java/frc/robot/commands/GripperCommand.java b/src/main/java/frc/robot/commands/GripperCommand.java index 031da2c..70a9d99 100644 --- a/src/main/java/frc/robot/commands/GripperCommand.java +++ b/src/main/java/frc/robot/commands/GripperCommand.java @@ -25,13 +25,15 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - gripper.moveGripper(percent); + // gripper.moveGripper(percent); + gripper.moveGripperPos(20); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - gripper.moveGripper(0); + // gripper.moveGripper(0); + gripper.moveGripperPos(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/ArmSub.java b/src/main/java/frc/robot/subsystems/ArmSub.java index e9fb27e..bb2839d 100644 --- a/src/main/java/frc/robot/subsystems/ArmSub.java +++ b/src/main/java/frc/robot/subsystems/ArmSub.java @@ -16,7 +16,14 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.AbsoluteSensorRange; import com.ctre.phoenix.sensors.CANCoder; +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.CANSparkMax; import com.revrobotics.ColorSensorV3; +import com.revrobotics.EncoderType; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxAbsoluteEncoder; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.SparkMaxRelativeEncoder.Type; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.I2C; @@ -30,12 +37,22 @@ public class ArmSub extends SubsystemBase { public TalonFX leftArm; public TalonFX rightArm; - public PIDController pidLoop; - public TalonSRX encorderTalon; + + public TalonFX telescopeArm; + + public CANSparkMax gripperJointEncoder; + public CANSparkMax gripperJointNeo; + public SparkMaxAbsoluteEncoder encoder; + public PIDController armPID; + public PIDController jointPID; + public PIDController telescopePID; public CANCoder testCanCoder; - public TalonSRXFeedbackDevice encoder; - public double maxSpeedFor; - public double maxSpeedRev; + + public double rotMaxSpeedFor; + public double rotMaxSpeedRev; + + public double telMaxSpeedFor; + public double telMaxSpeedRev; @@ -45,23 +62,32 @@ public ArmSub() { testCanCoder = new CANCoder(14, "Karen"); testCanCoder.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360); + + + rotMaxSpeedFor = 0.4; + rotMaxSpeedRev = 0.65; + + gripperJointEncoder = new CANSparkMax(55, MotorType.kBrushless); + encoder = gripperJointEncoder.getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type.kDutyCycle); + gripperJointNeo = new CANSparkMax(56, MotorType.kBrushless); + gripperJointNeo.clearFaults(); + gripperJointNeo.restoreFactoryDefaults(); - maxSpeedFor = 0.4; - maxSpeedRev = 0.65; // pidLoop = new PIDController(0.006, 0.003, 0.001); - pidLoop = new PIDController(0.014, 0.002, 0.00); - pidLoop.setTolerance(0.5); + armPID = new PIDController(0.014, 0.002, 0.00); + armPID.setTolerance(0.5); // pidLoop. leftArm = new WPI_TalonFX(20, "Karen"); rightArm = new WPI_TalonFX(21, "Karen"); + telescopeArm = new WPI_TalonFX(22, "Karen"); leftArm.configFactoryDefault(); leftArm.setNeutralMode(NeutralMode.Brake); - leftArm.configPeakOutputForward(maxSpeedFor); - leftArm.configPeakOutputReverse(-maxSpeedRev); + leftArm.configPeakOutputForward(rotMaxSpeedFor); + leftArm.configPeakOutputReverse(-rotMaxSpeedRev); leftArm.config_kP(0, 0.05); leftArm.config_kI(0, 0.0); leftArm.config_kD(0, 0.0); @@ -74,8 +100,8 @@ public ArmSub() { rightArm.configFactoryDefault(); - rightArm.configPeakOutputForward(maxSpeedFor); - rightArm.configPeakOutputReverse(-maxSpeedRev); + rightArm.configPeakOutputForward(rotMaxSpeedFor); + rightArm.configPeakOutputReverse(-rotMaxSpeedRev); rightArm.setSensorPhase(true); rightArm.setInverted(InvertType.FollowMaster); rightArm.config_kP(0, 0.05); @@ -84,7 +110,22 @@ public ArmSub() { rightArm.config_kF(0, 0.06); rightArm.setNeutralMode(NeutralMode.Brake); rightArm.follow(leftArm); + + telMaxSpeedFor = 0.5; + telMaxSpeedRev = 0.5; + telescopeArm.configFactoryDefault(); + telescopeArm.configPeakOutputForward(telMaxSpeedFor); + telescopeArm.configPeakOutputReverse(-telMaxSpeedRev); + telescopeArm.setSensorPhase(true); + telescopeArm.setInverted(InvertType.None); + telescopeArm.config_kP(0, 0.05); + telescopeArm.config_kI(0, 0.0); + telescopeArm.config_kD(0, 0.0); + telescopeArm.config_kF(0, 0.06); + telescopeArm.setNeutralMode(NeutralMode.Brake); + + } @@ -106,12 +147,24 @@ private void resetToAbsolute(){ double absolutePosition = Conversions.degreesToFalcon(testCanCoder.getAbsolutePosition(), (56)); leftArm.setSelectedSensorPosition(absolutePosition); } +/** + * Moves the arm telescope using percent control. + * @param percent + */ +public void extendArmPercentOutput(double percent){ + telescopeArm.set(TalonFXControlMode.PercentOutput, percent); +} + +public void extendArmPosition(double position){ + telescopeArm.set(TalonFXControlMode.Position, position); +} + /** * Moves the arm to a dedicated degree. * @param position Desired arm position in degrees */ public void moveArmPosition(double position){ - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(testCanCoder.getAbsolutePosition(), position)); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(testCanCoder.getAbsolutePosition(), position)); } /** * Moves the arm to a dedicated preset positions @@ -121,57 +174,57 @@ public void armPresetPositions(GlobalVariables.ArmPositions position){ switch(position){ case GROUND_PICKUP: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.grndIntakePosValue)); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.grndIntakePosValue)); // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.grndIntakePosValue)); break; case STOWED: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.stowedPosValue)); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.stowedPosValue)); // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.stowedPosValue)); break; case GROUND_SCORE: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.grndScorePosValueCone)); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.grndScorePosValueCone)); // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.grndScorePosValue)); break; case MID_SCORE_CONE: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.midScorePosValueCone)); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.midScorePosValueCone)); // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.midScorePosValue)); break; case HIGH_SCORE_CONE: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.highScorePosValueCone)); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.highScorePosValueCone)); // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.highScorePosValue)); break; case MID_SCORE_CUBE: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.midScorePosValueCube)); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.midScorePosValueCube)); // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.midScorePosValue)); break; case HIGH_SCORE_CUBE: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.highScorePosValueCube)); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.highScorePosValueCube)); // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.highScorePosValue)); break; case SHELF_PICKUP: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.shelfIntakePosValue)); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.shelfIntakePosValue)); // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.shelfIntakePosValue)); break; case MID_SCORE_ADAPTIVE: if(GlobalVariables.gamePiece == 0){ - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.midScorePosValueCone)); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.midScorePosValueCone)); } else{ - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.midScorePosValueCube)); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.midScorePosValueCube)); } // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.shelfIntakePosValue)); @@ -179,10 +232,10 @@ public void armPresetPositions(GlobalVariables.ArmPositions position){ break; case HIGH_SCORE_ADAPTIVE: if(GlobalVariables.gamePiece == 0){ - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.highScorePosValueCone)); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.highScorePosValueCone)); } else{ - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.highScorePosValueCube)); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.highScorePosValueCube)); } // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.shelfIntakePosValue)); @@ -190,14 +243,14 @@ public void armPresetPositions(GlobalVariables.ArmPositions position){ break; case CONE_UPRIGHT: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.coneUprightPosValue)); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.coneUprightPosValue)); // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.highScorePosValue)); break; default: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.shelfIntakePosValue)); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.shelfIntakePosValue)); // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.shelfIntakePosValue)); break; @@ -231,7 +284,7 @@ public double getArmMotorPos(){ @Override public void periodic() { SmartDashboard.putNumber("PID OUTPUT ", leftArm.getMotorOutputPercent()); - SmartDashboard.putNumber("PID ERROR ", pidLoop.getPositionError()); + SmartDashboard.putNumber("PID ERROR ", armPID.getPositionError()); SmartDashboard.putNumber("Arm Encoder Position", getArmPosition() ); SmartDashboard.putNumber("Arm Motor Position", getArmMotorPos()); SmartDashboard.putNumber("Arm Motor 1 Current", leftArm.getStatorCurrent()); diff --git a/src/main/java/frc/robot/subsystems/Gripper.java b/src/main/java/frc/robot/subsystems/Gripper.java index bfe4b3e..ae714a7 100644 --- a/src/main/java/frc/robot/subsystems/Gripper.java +++ b/src/main/java/frc/robot/subsystems/Gripper.java @@ -6,6 +6,8 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.ColorSensorV3; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; @@ -25,6 +27,7 @@ public class Gripper extends SubsystemBase { CANSparkMax gripperMotor; DoubleSolenoid cubeSolenoid; Solenoid coneSolenoid; + SparkMaxPIDController gripperPID; public PneumaticsControlModule module; Compressor compressor; public final I2C.Port i2cPort = I2C.Port.kMXP; @@ -42,7 +45,13 @@ public Gripper() { gripperMotor = new CANSparkMax(42, MotorType.kBrushless); gripperMotor.restoreFactoryDefaults(); gripperMotor.setIdleMode(IdleMode.kBrake); - + gripperMotor.getEncoder().setPosition(0); + + gripperPID = gripperMotor.getPIDController(); + gripperPID.setP(1); + gripperPID.setI(0); + gripperPID.setD(0); + gripperPID.setFeedbackDevice(gripperMotor.getEncoder()); compressor = new Compressor(PneumaticsModuleType.REVPH); compressor.enableDigital(); @@ -84,6 +93,10 @@ public void moveGripper(double percent ){ gripperMotor.set(percent); } + public void moveGripperPos(double position){ + gripperPID.setReference(position, ControlType.kPosition); + } + public void setClaw(GripperState state){ switch(state){ case OPEN: @@ -110,5 +123,6 @@ public void periodic() { SmartDashboard.putNumber("BLUE", colorSensor.getBlue()); SmartDashboard.putNumber("GREEN", colorSensor.getGreen()); SmartDashboard.putNumber("Distance", colorSensor.getProximity()); + SmartDashboard.putNumber("SPARKMAX POS", gripperMotor.getEncoder().getPosition()); } } From 67902514ed09b00398085d451ee69f0cc1cd0f02 Mon Sep 17 00:00:00 2001 From: Oscar Date: Wed, 15 Mar 2023 11:57:58 -0700 Subject: [PATCH 03/12] Began Butterscotch Integration --- src/main/java/frc/robot/Constants.java | 110 +++++ src/main/java/frc/robot/GlobalVariables.java | 10 +- src/main/java/frc/robot/RobotContainer.java | 38 +- .../java/frc/robot/autos/AutoPlaceRed.java | 122 ++--- .../java/frc/robot/autos/AutoPlaceTest2.java | 118 ++--- .../java/frc/robot/autos/BalanceAuto.java | 314 ++++++------- .../java/frc/robot/autos/BalanceAutoRed.java | 314 ++++++------- .../java/frc/robot/autos/LeftSideAuto.java | 434 +++++++++--------- .../java/frc/robot/autos/LeftSideAutoRed.java | 428 ++++++++--------- .../frc/robot/commands/ArmExtendTest.java | 43 ++ .../robot/commands/ArmPositionCommand.java | 2 +- .../java/frc/robot/commands/GroundIntake.java | 2 +- .../java/frc/robot/commands/HoldArmPos.java | 2 +- .../java/frc/robot/subsystems/ArmSub.java | 266 ++++++++--- .../java/frc/robot/subsystems/Gripper.java | 3 + .../java/frc/robot/subsystems/Swerve.java | 21 + vendordeps/REV2mDistanceSensor.json | 55 +++ 17 files changed, 1333 insertions(+), 949 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ArmExtendTest.java create mode 100644 vendordeps/REV2mDistanceSensor.json diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 554a4d5..e58c37b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -39,6 +39,116 @@ public final class Constants { public static final double coneUprightPosValue = 150; + public static final class ArmRotationValues { + + public static final double armRotStow = 0; + + public static final double armRotForHighCone = 0; + public static final double armRotForHighCube = 0; + + public static final double armRotForMidCone = 0; + public static final double armRotForMidCube = 0; + + public static final double armRotForLowCone = 0; + public static final double armRotForLowCube = 0; + + public static final double armRotForConePickup = 0; + public static final double armRotForCubePickup = 0; + + public static final double armRotForShelfCone = 0; + public static final double armRotForShelfCube = 0; + + public static final double armRotRevHighCone = 0; + public static final double armRotRevHighCube = 0; + + public static final double armRotRevMidCone = 0; + public static final double armRotRevMidCube = 0; + + public static final double armRotRevLowCone = 0; + public static final double armRotRevLowCube = 0; + + public static final double armRotRevConePickup = 0; + public static final double armRotRevCubePickup = 0; + + public static final double armRotRevShelfCone = 0; + public static final double armRotRevShelfCube = 0; + + } + + public static final class ArmExtendValues { + public static final double armExtendStow = 0; + + public static final double armExtendConePickup = 0; + public static final double armExtendCubePickup = 0; + + + public static final double armExtendForHighCone = 0; + public static final double armExtendForHighCube = 0; + + public static final double armExtendForMidCone = 0; + public static final double armExtendForMidCube = 0; + + public static final double armExtendForLowCone = 0; + public static final double armExtendForLowCube = 0; + + public static final double armExtendForConePickup = 0; + public static final double armExtendForCubePickup = 0; + + public static final double armExtendForShelfCone = 0; + public static final double armExtendForShelfCube = 0; + + + public static final double armExtendRevHighCone = 0; + public static final double armExtendRevHighCube = 0; + + public static final double armExtendRevMidCone = 0; + public static final double armExtendRevMidCube = 0; + + public static final double armExtendRevLowCone = 0; + public static final double armExtendRevLowCube = 0; + + public static final double armExtendRevShelfCone = 0; + public static final double armExtendRevShelfCube = 0; + } + + public static final class JointRotationValues { + public static final double JointRotStow = 0; + + public static final double JointRotConePickup = 0; + public static final double JointRotCubePickup = 0; + + + public static final double JointRotForHighCone = 0; + public static final double JointRotForHighCube = 0; + + public static final double JointRotForMidCone = 0; + public static final double JointRotForMidCube = 0; + + public static final double JointRotForLowCone = 0; + public static final double JointRotForLowCube = 0; + + public static final double JointRotForConePickup = 0; + public static final double JointRotForCubePickup = 0; + + public static final double JointRotForShelfCone = 0; + public static final double JointRotForShelfCube = 0; + + + public static final double JointRotRevHighCone = 0; + public static final double JointRotRevHighCube = 0; + + public static final double JointRotRevMidCone = 0; + public static final double JointRotRevMidCube = 0; + + public static final double JointRotRevLowCone = 0; + public static final double JointRotRevLowCube = 0; + + public static final double JointRotRevShelfCone = 0; + public static final double JointRotRevShelfCube = 0; + } + + + public static final class Swerve { public static final int pigeonID = 1; diff --git a/src/main/java/frc/robot/GlobalVariables.java b/src/main/java/frc/robot/GlobalVariables.java index e57ff04..d57e4d7 100644 --- a/src/main/java/frc/robot/GlobalVariables.java +++ b/src/main/java/frc/robot/GlobalVariables.java @@ -11,6 +11,8 @@ public class GlobalVariables { public static int tag; public static int rowSelect; public static int height; + public static boolean robotDirection; + public static double gripperConePlacement; public static enum StationSelect{ CLOSE_RAMP, LEFT_SHELF, LEFT_RAMP, RIGHT_SHELF, RIGHT_RAMP @@ -20,14 +22,14 @@ public static enum StationSelect{ public static enum ArmPositions{ - STOWED, - GROUND_PICKUP, - GROUND_SCORE, + STOWED_ADAPTIVE, + GROUND_PICKUP_ADAPTIVE, + GROUND_SCORE_ADAPTIVE, MID_SCORE_CUBE, MID_SCORE_CONE, HIGH_SCORE_CUBE, HIGH_SCORE_CONE, - SHELF_PICKUP, + SHELF_PICKUP_ADAPTIVE, MID_SCORE_ADAPTIVE, HIGH_SCORE_ADAPTIVE, CONE_UPRIGHT diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a07d3aa..9d7e645 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -76,10 +76,10 @@ public class RobotContainer { public final CANdleSub candle = new CANdleSub(); public SendableChooser m_chooser = new SendableChooser<>(); - private final SequentialCommandGroup leftSideBlue = new LeftSideAuto(s_Swerve, armSub, gripper); - private final SequentialCommandGroup rightSideRed = new LeftSideAutoRed(s_Swerve, armSub, gripper); - private final SequentialCommandGroup balanceBlue = new BalanceAuto(s_Swerve, armSub, gripper); - private final SequentialCommandGroup balanceRed = new BalanceAutoRed(s_Swerve, armSub, gripper); + // private final SequentialCommandGroup leftSideBlue = new LeftSideAuto(s_Swerve, armSub, gripper); + // private final SequentialCommandGroup rightSideRed = new LeftSideAutoRed(s_Swerve, armSub, gripper); + // private final SequentialCommandGroup balanceBlue = new BalanceAuto(s_Swerve, armSub, gripper); + // private final SequentialCommandGroup balanceRed = new BalanceAutoRed(s_Swerve, armSub, gripper); /* Subsystems */ @@ -96,16 +96,16 @@ public RobotContainer() { operatorPanel.setOutput(13, true ); operatorPanel.setOutput(16, true ); - m_chooser.setDefaultOption("Balance Blue", balanceBlue); - m_chooser.addOption("Balance Red", balanceRed); - m_chooser.addOption("Left Blue", leftSideBlue); - m_chooser.addOption("Right Red", rightSideRed); + // m_chooser.setDefaultOption("Balance Blue", balanceBlue); + // m_chooser.addOption("Balance Red", balanceRed); + // m_chooser.addOption("Left Blue", leftSideBlue); + // m_chooser.addOption("Right Red", rightSideRed); SmartDashboard.putData(m_chooser); s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, joystickPanel, translationAxis, strafeAxis, rotationAxis, fieldRelative, openLoop). withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - armSub.setDefaultCommand(new HoldArmPos(armSub, ArmPositions.STOWED).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + armSub.setDefaultCommand(new HoldArmPos(armSub, ArmPositions.STOWED_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); //candle.setDefaultCommand(new InstantCommand(() -> candle.setColor(operatorExtraRight.getAsBoolean()))); candle.setDefaultCommand(new SetLedCommand(candle)); configureButtonBindings(); @@ -121,29 +121,29 @@ private void configureButtonBindings() { operatorExtraRight.whileTrue(new ConeVCube(0)); operatorExtraRight.whileFalse(new ConeVCube(1)); - operatorIntakeUp.whileTrue(new HoldArmPos(armSub, ArmPositions.STOWED).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - operatorIntakeUp.whileFalse(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - operatorIntakeDown.whileTrue(new HoldArmPos(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - operatorIntakeDown.whileFalse(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // operatorIntakeUp.whileTrue(new HoldArmPos(armSub, ArmPositions.STOWED).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // operatorIntakeUp.whileFalse(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // operatorIntakeDown.whileTrue(new HoldArmPos(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // operatorIntakeDown.whileFalse(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); operatorLatch.toggleOnTrue(new InstantCommand(() -> gripper.setClaw(GripperState.CONE)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); // operatorHook.toggleOnTrue(new InstantCommand(() -> gripper.setClaw(GripperState.OPEN)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); operatorSpinup.whileTrue(new LockToGamePiece(s_Swerve, joystickPanel, translationAxis, strafeAxis, true, false, GlobalVariables.gamePiece).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); operatorLeftEmpty.toggleOnTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - operatorHook.whileTrue(new GripperCommand(gripper, 0.4)); + operatorHook.whileTrue(new ArmExtendTest(armSub)); rightJoy.onTrue(new InstantCommand(() -> s_Swerve.zeroGyro())); leftJoy.whileTrue(new LockToGamePiece(s_Swerve, joystickPanel, translationAxis, strafeAxis, true, true, GlobalVariables.gamePiece).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - buttonX.toggleOnTrue(new HoldArmPos(armSub, ArmPositions.STOWED).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - buttonY.whileTrue(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - buttonB.whileTrue(new HoldArmPos(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // buttonX.toggleOnTrue(new HoldArmPos(armSub, ArmPositions.STOWED).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // buttonY.whileTrue(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // buttonB.whileTrue(new HoldArmPos(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); buttonA.whileTrue(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); xboxRightJoyButton.whileTrue(new HoldArmPos(armSub, ArmPositions.CONE_UPRIGHT).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); // leftBumper.toggleOnTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - leftBumper.whileTrue(new HoldArmPos(armSub, ArmPositions.SHELF_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // leftBumper.whileTrue(new HoldArmPos(armSub, ArmPositions.SHELF_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); rightBumper.toggleOnTrue(new ShelfIntake(gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); // operatorDeploy.onTrue(new TurnInPlace(s_Swerve, true, false, 0)); @@ -152,7 +152,7 @@ private void configureButtonBindings() { // TODO RED SIDE // LeftLittleButton.onTrue(new AutoPlaceRed(s_Swerve, armSub, gripper)) ; // TODO BLUE SIDE - LeftLittleButton.onTrue(new AutoPlaceTest2(s_Swerve, armSub, gripper)); + // LeftLittleButton.onTrue(new AutoPlaceTest2(s_Swerve, armSub, gripper)); } /* _ diff --git a/src/main/java/frc/robot/autos/AutoPlaceRed.java b/src/main/java/frc/robot/autos/AutoPlaceRed.java index 265ebbb..e81d55a 100644 --- a/src/main/java/frc/robot/autos/AutoPlaceRed.java +++ b/src/main/java/frc/robot/autos/AutoPlaceRed.java @@ -1,71 +1,71 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. -package frc.robot.autos; +// package frc.robot.autos; -import java.util.List; +// import java.util.List; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.GlobalVariables.ArmPositions; -import frc.robot.commands.ArmPositionCommand; -import frc.robot.commands.GripperStateCommand; -import frc.robot.commands.HoldArmPos; -import frc.robot.commands.HoldArmPosAuto; -import frc.robot.commands.Test; -import frc.robot.commands.TestRed; -import frc.robot.subsystems.ArmSub; -import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; +// import edu.wpi.first.math.controller.PIDController; +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants; +// import frc.robot.GlobalVariables.ArmPositions; +// import frc.robot.commands.ArmPositionCommand; +// import frc.robot.commands.GripperStateCommand; +// import frc.robot.commands.HoldArmPos; +// import frc.robot.commands.HoldArmPosAuto; +// import frc.robot.commands.Test; +// import frc.robot.commands.TestRed; +// import frc.robot.subsystems.ArmSub; +// import frc.robot.subsystems.Gripper; +// import frc.robot.subsystems.Swerve; +// import frc.robot.subsystems.Gripper.GripperState; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AutoPlaceRed extends SequentialCommandGroup { - Swerve swerve; - ArmSub armSub; - Gripper gripper; - SwerveControllerCommand swerveControllerCommand; +// // NOTE: Consider using this command inline, rather than writing a subclass. For more +// // information, see: +// // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +// public class AutoPlaceRed extends SequentialCommandGroup { +// Swerve swerve; +// ArmSub armSub; +// Gripper gripper; +// SwerveControllerCommand swerveControllerCommand; - /** Creates a new AutoPlaceTest. */ - public AutoPlaceRed(Swerve swerve, ArmSub armSub, Gripper gripper) { - this.swerve = swerve; - this.armSub = armSub; - this.gripper = gripper; +// /** Creates a new AutoPlaceTest. */ +// public AutoPlaceRed(Swerve swerve, ArmSub armSub, Gripper gripper) { +// this.swerve = swerve; +// this.armSub = armSub; +// this.gripper = gripper; - addCommands( - // new InstantCommand(() -> swerve.normalizeOdometry()), - // new Test(swerve, 0.7, 0).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), - // new GripperStateCommand(gripper, GripperState.CONE), - // new HoldArmPos(armSub, ArmPositions.MID_SCORE).raceWith (new WaitCommand(1)), - // new Test(swerve, 0, 0.7).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), - // new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(0.1)), - // new GripperStateCommand(gripper, GripperState.OPEN) - new InstantCommand(() -> swerve.normalizeOdometry()), - new WaitCommand(0.5).raceWith(new HoldArmPosAuto(armSub)), - new TestRed(swerve, 0.6, 0, false).raceWith(new HoldArmPosAuto(armSub)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new HoldArmPosAuto(armSub)), - new TestRed(swerve, 0.2, 0.7, true).raceWith(new WaitCommand(1.5).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), - new GripperStateCommand(gripper, GripperState.CONE).raceWith(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP)), - new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(1)) +// addCommands( +// // new InstantCommand(() -> swerve.normalizeOdometry()), +// // new Test(swerve, 0.7, 0).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), +// // new GripperStateCommand(gripper, GripperState.CONE), +// // new HoldArmPos(armSub, ArmPositions.MID_SCORE).raceWith (new WaitCommand(1)), +// // new Test(swerve, 0, 0.7).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), +// // new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(0.1)), +// // new GripperStateCommand(gripper, GripperState.OPEN) +// new InstantCommand(() -> swerve.normalizeOdometry()), +// new WaitCommand(0.5).raceWith(new HoldArmPosAuto(armSub)), +// new TestRed(swerve, 0.6, 0, false).raceWith(new HoldArmPosAuto(armSub)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new HoldArmPosAuto(armSub)), +// new TestRed(swerve, 0.2, 0.7, true).raceWith(new WaitCommand(1.5).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), +// new GripperStateCommand(gripper, GripperState.CONE).raceWith(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP)), +// new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(1)) - ); +// ); -// 16.54 +// // 16.54 - } -} +// } +// } diff --git a/src/main/java/frc/robot/autos/AutoPlaceTest2.java b/src/main/java/frc/robot/autos/AutoPlaceTest2.java index 7801443..2aa7940 100644 --- a/src/main/java/frc/robot/autos/AutoPlaceTest2.java +++ b/src/main/java/frc/robot/autos/AutoPlaceTest2.java @@ -1,70 +1,70 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. -package frc.robot.autos; +// package frc.robot.autos; -import java.util.List; +// import java.util.List; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.GlobalVariables.ArmPositions; -import frc.robot.commands.ArmPositionCommand; -import frc.robot.commands.GripperStateCommand; -import frc.robot.commands.HoldArmPos; -import frc.robot.commands.HoldArmPosAuto; -import frc.robot.commands.Test; -import frc.robot.subsystems.ArmSub; -import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; +// import edu.wpi.first.math.controller.PIDController; +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants; +// import frc.robot.GlobalVariables.ArmPositions; +// import frc.robot.commands.ArmPositionCommand; +// import frc.robot.commands.GripperStateCommand; +// import frc.robot.commands.HoldArmPos; +// import frc.robot.commands.HoldArmPosAuto; +// import frc.robot.commands.Test; +// import frc.robot.subsystems.ArmSub; +// import frc.robot.subsystems.Gripper; +// import frc.robot.subsystems.Swerve; +// import frc.robot.subsystems.Gripper.GripperState; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AutoPlaceTest2 extends SequentialCommandGroup { - Swerve swerve; - ArmSub armSub; - Gripper gripper; - SwerveControllerCommand swerveControllerCommand; +// // NOTE: Consider using this command inline, rather than writing a subclass. For more +// // information, see: +// // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +// public class AutoPlaceTest2 extends SequentialCommandGroup { +// Swerve swerve; +// ArmSub armSub; +// Gripper gripper; +// SwerveControllerCommand swerveControllerCommand; - /** Creates a new AutoPlaceTest. */ - public AutoPlaceTest2(Swerve swerve, ArmSub armSub, Gripper gripper) { - this.swerve = swerve; - this.armSub = armSub; - this.gripper = gripper; +// /** Creates a new AutoPlaceTest. */ +// public AutoPlaceTest2(Swerve swerve, ArmSub armSub, Gripper gripper) { +// this.swerve = swerve; +// this.armSub = armSub; +// this.gripper = gripper; - addCommands( - // new InstantCommand(() -> swerve.normalizeOdometry()), - // new Test(swerve, 0.7, 0).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), - // new GripperStateCommand(gripper, GripperState.CONE), - // new HoldArmPos(armSub, ArmPositions.MID_SCORE).raceWith (new WaitCommand(1)), - // new Test(swerve, 0, 0.7).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), - // new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(0.1)), - // new GripperStateCommand(gripper, GripperState.OPEN) - new InstantCommand(() -> swerve.normalizeOdometry()), - new WaitCommand(0.5).raceWith(new HoldArmPosAuto(armSub)), - new Test(swerve, 0.6, 0, false).raceWith(new HoldArmPosAuto(armSub)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new HoldArmPosAuto(armSub)), - new Test(swerve, 0.2, 0.7, true).raceWith(new WaitCommand(1.5).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), - new GripperStateCommand(gripper, GripperState.CONE).raceWith(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP)), - new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(1)) +// addCommands( +// // new InstantCommand(() -> swerve.normalizeOdometry()), +// // new Test(swerve, 0.7, 0).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), +// // new GripperStateCommand(gripper, GripperState.CONE), +// // new HoldArmPos(armSub, ArmPositions.MID_SCORE).raceWith (new WaitCommand(1)), +// // new Test(swerve, 0, 0.7).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), +// // new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(0.1)), +// // new GripperStateCommand(gripper, GripperState.OPEN) +// new InstantCommand(() -> swerve.normalizeOdometry()), +// new WaitCommand(0.5).raceWith(new HoldArmPosAuto(armSub)), +// new Test(swerve, 0.6, 0, false).raceWith(new HoldArmPosAuto(armSub)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new HoldArmPosAuto(armSub)), +// new Test(swerve, 0.2, 0.7, true).raceWith(new WaitCommand(1.5).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), +// new GripperStateCommand(gripper, GripperState.CONE).raceWith(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP)), +// new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(1)) - ); +// ); - } -} +// } +// } diff --git a/src/main/java/frc/robot/autos/BalanceAuto.java b/src/main/java/frc/robot/autos/BalanceAuto.java index a776b7d..37fff26 100644 --- a/src/main/java/frc/robot/autos/BalanceAuto.java +++ b/src/main/java/frc/robot/autos/BalanceAuto.java @@ -1,111 +1,111 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.autos; - - -import java.util.List; - -// package edu.wpi.first.math.trajectory.constraint; - -import edu.wpi.first.math.geometry.Pose2d; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; -import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.GlobalVariables.ArmPositions; -import frc.robot.commands.AutoBalanceStation; -import frc.robot.commands.AutoLockToGamePiece; -import frc.robot.commands.AutoSwervePositions; -import frc.robot.commands.BalanceStation; -import frc.robot.commands.GripperStateCommand; -import frc.robot.commands.GroundIntake; -import frc.robot.commands.HoldArmPos; -import frc.robot.commands.TeleopSwerve; -import frc.robot.commands.Test; -import frc.robot.subsystems.ArmSub; -import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html - -public class BalanceAuto extends SequentialCommandGroup { - Swerve swerve; - ArmSub armsub; - Gripper gripper; - Joystick controller; - - SwerveControllerCommand swerveControllerCommand1; - SwerveControllerCommand swerveControllerCommand2; - SwerveControllerCommand swerveControllerCommand3; - SwerveControllerCommand swerveControllerCommand4; - SwerveControllerCommand swerveControllerCommand5; - SwerveControllerCommand swerveControllerCommand6; - - - /** Creates a new AutoPlaceTest. */ - public BalanceAuto(Swerve swerve, ArmSub armsub, Gripper gripper) { - this.swerve = swerve; - this.armsub = armsub; - this.gripper = gripper; - controller = new Joystick(1); +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. + +// package frc.robot.autos; + + +// import java.util.List; + +// // package edu.wpi.first.math.trajectory.constraint; + +// import edu.wpi.first.math.geometry.Pose2d; + +// import edu.wpi.first.math.controller.PIDController; +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; +// import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; +// import edu.wpi.first.wpilibj.Joystick; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants; +// import frc.robot.GlobalVariables.ArmPositions; +// import frc.robot.commands.AutoBalanceStation; +// import frc.robot.commands.AutoLockToGamePiece; +// import frc.robot.commands.AutoSwervePositions; +// import frc.robot.commands.BalanceStation; +// import frc.robot.commands.GripperStateCommand; +// import frc.robot.commands.GroundIntake; +// import frc.robot.commands.HoldArmPos; +// import frc.robot.commands.TeleopSwerve; +// import frc.robot.commands.Test; +// import frc.robot.subsystems.ArmSub; +// import frc.robot.subsystems.Gripper; +// import frc.robot.subsystems.Swerve; +// import frc.robot.subsystems.Gripper.GripperState; + +// // NOTE: Consider using this command inline, rather than writing a subclass. For more +// // information, see: +// // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html + +// public class BalanceAuto extends SequentialCommandGroup { +// Swerve swerve; +// ArmSub armsub; +// Gripper gripper; +// Joystick controller; + +// SwerveControllerCommand swerveControllerCommand1; +// SwerveControllerCommand swerveControllerCommand2; +// SwerveControllerCommand swerveControllerCommand3; +// SwerveControllerCommand swerveControllerCommand4; +// SwerveControllerCommand swerveControllerCommand5; +// SwerveControllerCommand swerveControllerCommand6; + + +// /** Creates a new AutoPlaceTest. */ +// public BalanceAuto(Swerve swerve, ArmSub armsub, Gripper gripper) { +// this.swerve = swerve; +// this.armsub = armsub; +// this.gripper = gripper; +// controller = new Joystick(1); - // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) - TrajectoryConfig config = new TrajectoryConfig( - 0.6, - 1) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); - config.addConstraint(new CentripetalAccelerationConstraint(0.3)); - TrajectoryConfig configrev = new TrajectoryConfig( - 0.8, - 1) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); - configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - - - Trajectory traj1 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(2.6, 3.25, new Rotation2d(Math.PI)), - new Pose2d(1.82, 3.25, new Rotation2d(Math.PI))), - config); - Trajectory traj3 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(1.97, 3.25, new Rotation2d(Math.PI)), - new Pose2d(3.85, 3.25, new Rotation2d(Math.PI))), - configrev); - - Trajectory traj4 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(3.85, 3.25, new Rotation2d(Math.PI)), - new Pose2d(5.4, 3.25, new Rotation2d(Math.PI))), - configrev); - Trajectory traj5 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(5.4, 3.25, new Rotation2d(Math.PI)), - new Pose2d(3.7, 3.25, new Rotation2d(Math.PI))), - config); +// // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) +// TrajectoryConfig config = new TrajectoryConfig( +// 0.6, +// 1) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); +// config.addConstraint(new CentripetalAccelerationConstraint(0.3)); +// TrajectoryConfig configrev = new TrajectoryConfig( +// 0.8, +// 1) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); +// configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); + + + +// Trajectory traj1 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(2.6, 3.25, new Rotation2d(Math.PI)), +// new Pose2d(1.82, 3.25, new Rotation2d(Math.PI))), +// config); +// Trajectory traj3 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(1.97, 3.25, new Rotation2d(Math.PI)), +// new Pose2d(3.85, 3.25, new Rotation2d(Math.PI))), +// configrev); + +// Trajectory traj4 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(3.85, 3.25, new Rotation2d(Math.PI)), +// new Pose2d(5.4, 3.25, new Rotation2d(Math.PI))), +// configrev); +// Trajectory traj5 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(5.4, 3.25, new Rotation2d(Math.PI)), +// new Pose2d(3.7, 3.25, new Rotation2d(Math.PI))), +// config); @@ -113,66 +113,66 @@ public BalanceAuto(Swerve swerve, ArmSub armsub, Gripper gripper) { - var thetaController = new ProfiledPIDController( - 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); - - thetaController.enableContinuousInput(-Math.PI, Math.PI); - swerveControllerCommand1 = new SwerveControllerCommand( - traj1, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand3 = new SwerveControllerCommand( - traj3, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - swerveControllerCommand4 = new SwerveControllerCommand( - traj4, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); +// var thetaController = new ProfiledPIDController( +// 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); + +// thetaController.enableContinuousInput(-Math.PI, Math.PI); +// swerveControllerCommand1 = new SwerveControllerCommand( +// traj1, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand3 = new SwerveControllerCommand( +// traj3, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); +// swerveControllerCommand4 = new SwerveControllerCommand( +// traj4, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); - swerveControllerCommand5 = new SwerveControllerCommand( - traj5, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); +// swerveControllerCommand5 = new SwerveControllerCommand( +// traj5, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); - addCommands( - new WaitCommand(0.5).raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), - swerveControllerCommand1.raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - swerveControllerCommand3.raceWith(new WaitCommand(1).andThen(new HoldArmPos(armsub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), - // swerveControllerCommand3, - swerveControllerCommand4.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), - swerveControllerCommand5.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), - new AutoBalanceStation(swerve, false, false).deadlineWith(new WaitCommand(10)) +// addCommands( +// new WaitCommand(0.5).raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), +// swerveControllerCommand1.raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// swerveControllerCommand3.raceWith(new WaitCommand(1).andThen(new HoldArmPos(armsub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), +// // swerveControllerCommand3, +// swerveControllerCommand4.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), +// swerveControllerCommand5.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), +// new AutoBalanceStation(swerve, false, false).deadlineWith(new WaitCommand(10)) - ); +// ); - } -} +// } +// } diff --git a/src/main/java/frc/robot/autos/BalanceAutoRed.java b/src/main/java/frc/robot/autos/BalanceAutoRed.java index b39c557..e3a3ef2 100644 --- a/src/main/java/frc/robot/autos/BalanceAutoRed.java +++ b/src/main/java/frc/robot/autos/BalanceAutoRed.java @@ -1,111 +1,111 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.autos; - - -import java.util.List; - -// package edu.wpi.first.math.trajectory.constraint; - -import edu.wpi.first.math.geometry.Pose2d; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; -import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.GlobalVariables.ArmPositions; -import frc.robot.commands.AutoBalanceStation; -import frc.robot.commands.AutoLockToGamePiece; -import frc.robot.commands.AutoSwervePositions; -import frc.robot.commands.BalanceStation; -import frc.robot.commands.GripperStateCommand; -import frc.robot.commands.GroundIntake; -import frc.robot.commands.HoldArmPos; -import frc.robot.commands.TeleopSwerve; -import frc.robot.commands.Test; -import frc.robot.subsystems.ArmSub; -import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html - -public class BalanceAutoRed extends SequentialCommandGroup { - Swerve swerve; - ArmSub armsub; - Gripper gripper; - Joystick controller; - - SwerveControllerCommand swerveControllerCommand1; - SwerveControllerCommand swerveControllerCommand2; - SwerveControllerCommand swerveControllerCommand3; - SwerveControllerCommand swerveControllerCommand4; - SwerveControllerCommand swerveControllerCommand5; - SwerveControllerCommand swerveControllerCommand6; - - - /** Creates a new AutoPlaceTest. */ - public BalanceAutoRed(Swerve swerve, ArmSub armsub, Gripper gripper) { - this.swerve = swerve; - this.armsub = armsub; - this.gripper = gripper; - controller = new Joystick(1); +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. + +// package frc.robot.autos; + + +// import java.util.List; + +// // package edu.wpi.first.math.trajectory.constraint; + +// import edu.wpi.first.math.geometry.Pose2d; + +// import edu.wpi.first.math.controller.PIDController; +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; +// import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; +// import edu.wpi.first.wpilibj.Joystick; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants; +// import frc.robot.GlobalVariables.ArmPositions; +// import frc.robot.commands.AutoBalanceStation; +// import frc.robot.commands.AutoLockToGamePiece; +// import frc.robot.commands.AutoSwervePositions; +// import frc.robot.commands.BalanceStation; +// import frc.robot.commands.GripperStateCommand; +// import frc.robot.commands.GroundIntake; +// import frc.robot.commands.HoldArmPos; +// import frc.robot.commands.TeleopSwerve; +// import frc.robot.commands.Test; +// import frc.robot.subsystems.ArmSub; +// import frc.robot.subsystems.Gripper; +// import frc.robot.subsystems.Swerve; +// import frc.robot.subsystems.Gripper.GripperState; + +// // NOTE: Consider using this command inline, rather than writing a subclass. For more +// // information, see: +// // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html + +// public class BalanceAutoRed extends SequentialCommandGroup { +// Swerve swerve; +// ArmSub armsub; +// Gripper gripper; +// Joystick controller; + +// SwerveControllerCommand swerveControllerCommand1; +// SwerveControllerCommand swerveControllerCommand2; +// SwerveControllerCommand swerveControllerCommand3; +// SwerveControllerCommand swerveControllerCommand4; +// SwerveControllerCommand swerveControllerCommand5; +// SwerveControllerCommand swerveControllerCommand6; + + +// /** Creates a new AutoPlaceTest. */ +// public BalanceAutoRed(Swerve swerve, ArmSub armsub, Gripper gripper) { +// this.swerve = swerve; +// this.armsub = armsub; +// this.gripper = gripper; +// controller = new Joystick(1); - // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) - TrajectoryConfig config = new TrajectoryConfig( - 0.6, - 1) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); - config.addConstraint(new CentripetalAccelerationConstraint(0.3)); - TrajectoryConfig configrev = new TrajectoryConfig( - 0.8, - 1) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); - configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - - - Trajectory traj1 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(13.94, 3.25, new Rotation2d(0)), - new Pose2d(14.72, 3.25, new Rotation2d(0))), - config); - Trajectory traj3 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(14.57, 3.25, new Rotation2d(0)), - new Pose2d(12.69, 3.25, new Rotation2d(0))), - configrev); - - Trajectory traj4 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(12.69, 3.25, new Rotation2d(0)), - new Pose2d(11.14, 3.25, new Rotation2d(0))), - configrev); - Trajectory traj5 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(11.14, 3.25, new Rotation2d(0)), - new Pose2d(12.84, 3.25, new Rotation2d(0))), - config); +// // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) +// TrajectoryConfig config = new TrajectoryConfig( +// 0.6, +// 1) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); +// config.addConstraint(new CentripetalAccelerationConstraint(0.3)); +// TrajectoryConfig configrev = new TrajectoryConfig( +// 0.8, +// 1) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); +// configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); + + + +// Trajectory traj1 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(13.94, 3.25, new Rotation2d(0)), +// new Pose2d(14.72, 3.25, new Rotation2d(0))), +// config); +// Trajectory traj3 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(14.57, 3.25, new Rotation2d(0)), +// new Pose2d(12.69, 3.25, new Rotation2d(0))), +// configrev); + +// Trajectory traj4 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(12.69, 3.25, new Rotation2d(0)), +// new Pose2d(11.14, 3.25, new Rotation2d(0))), +// configrev); +// Trajectory traj5 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(11.14, 3.25, new Rotation2d(0)), +// new Pose2d(12.84, 3.25, new Rotation2d(0))), +// config); @@ -113,66 +113,66 @@ public BalanceAutoRed(Swerve swerve, ArmSub armsub, Gripper gripper) { - var thetaController = new ProfiledPIDController( - 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); - - thetaController.enableContinuousInput(-Math.PI, Math.PI); - swerveControllerCommand1 = new SwerveControllerCommand( - traj1, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand3 = new SwerveControllerCommand( - traj3, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - swerveControllerCommand4 = new SwerveControllerCommand( - traj4, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); +// var thetaController = new ProfiledPIDController( +// 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); + +// thetaController.enableContinuousInput(-Math.PI, Math.PI); +// swerveControllerCommand1 = new SwerveControllerCommand( +// traj1, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand3 = new SwerveControllerCommand( +// traj3, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); +// swerveControllerCommand4 = new SwerveControllerCommand( +// traj4, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); - swerveControllerCommand5 = new SwerveControllerCommand( - traj5, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); +// swerveControllerCommand5 = new SwerveControllerCommand( +// traj5, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); - addCommands( - new WaitCommand(0.5).raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), - swerveControllerCommand1.raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - swerveControllerCommand3.raceWith(new WaitCommand(1).andThen(new HoldArmPos(armsub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), - // swerveControllerCommand3, - swerveControllerCommand4.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), - swerveControllerCommand5.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), - new AutoBalanceStation(swerve, false, false).deadlineWith(new WaitCommand(10)) +// addCommands( +// new WaitCommand(0.5).raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), +// swerveControllerCommand1.raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// swerveControllerCommand3.raceWith(new WaitCommand(1).andThen(new HoldArmPos(armsub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), +// // swerveControllerCommand3, +// swerveControllerCommand4.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), +// swerveControllerCommand5.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), +// new AutoBalanceStation(swerve, false, false).deadlineWith(new WaitCommand(10)) - ); +// ); - } -} +// } +// } diff --git a/src/main/java/frc/robot/autos/LeftSideAuto.java b/src/main/java/frc/robot/autos/LeftSideAuto.java index 4d21b65..5f8f882 100644 --- a/src/main/java/frc/robot/autos/LeftSideAuto.java +++ b/src/main/java/frc/robot/autos/LeftSideAuto.java @@ -1,234 +1,234 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.autos; - - -import java.util.List; - -// package edu.wpi.first.math.trajectory.constraint; - -import edu.wpi.first.math.geometry.Pose2d; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; -import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.GlobalVariables.ArmPositions; -import frc.robot.commands.AutoLockToGamePiece; -import frc.robot.commands.AutoSwervePositions; -import frc.robot.commands.GripperStateCommand; -import frc.robot.commands.GroundIntake; -import frc.robot.commands.HoldArmPos; -import frc.robot.commands.TeleopSwerve; -import frc.robot.commands.Test; -import frc.robot.commands.TurnInPlace; -import frc.robot.subsystems.ArmSub; -import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html - -public class LeftSideAuto extends SequentialCommandGroup { - Swerve swerve; - ArmSub armSub; - Gripper gripper; - SwerveControllerCommand swerveControllerCommand1; - SwerveControllerCommand swerveControllerCommand2; - SwerveControllerCommand swerveControllerCommand3; - SwerveControllerCommand swerveControllerCommand4; - SwerveControllerCommand swerveControllerCommand5; - SwerveControllerCommand swerveControllerCommand6; - Joystick controller; - -MinMax minMax; - TrajectoryConstraint constraints; - - /** Creates a new AutoPlaceTest. */ - public LeftSideAuto(Swerve swerve, ArmSub armSub, Gripper gripper) { - this.swerve = swerve; - this.armSub = armSub; - this.gripper = gripper; - controller = new Joystick(1); +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. + +// package frc.robot.autos; + + +// import java.util.List; + +// // package edu.wpi.first.math.trajectory.constraint; + +// import edu.wpi.first.math.geometry.Pose2d; + +// import edu.wpi.first.math.controller.PIDController; +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; +// import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; +// import edu.wpi.first.wpilibj.Joystick; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants; +// import frc.robot.GlobalVariables.ArmPositions; +// import frc.robot.commands.AutoLockToGamePiece; +// import frc.robot.commands.AutoSwervePositions; +// import frc.robot.commands.GripperStateCommand; +// import frc.robot.commands.GroundIntake; +// import frc.robot.commands.HoldArmPos; +// import frc.robot.commands.TeleopSwerve; +// import frc.robot.commands.Test; +// import frc.robot.commands.TurnInPlace; +// import frc.robot.subsystems.ArmSub; +// import frc.robot.subsystems.Gripper; +// import frc.robot.subsystems.Swerve; +// import frc.robot.subsystems.Gripper.GripperState; + +// // NOTE: Consider using this command inline, rather than writing a subclass. For more +// // information, see: +// // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html + +// public class LeftSideAuto extends SequentialCommandGroup { +// Swerve swerve; +// ArmSub armSub; +// Gripper gripper; +// SwerveControllerCommand swerveControllerCommand1; +// SwerveControllerCommand swerveControllerCommand2; +// SwerveControllerCommand swerveControllerCommand3; +// SwerveControllerCommand swerveControllerCommand4; +// SwerveControllerCommand swerveControllerCommand5; +// SwerveControllerCommand swerveControllerCommand6; +// Joystick controller; + +// MinMax minMax; +// TrajectoryConstraint constraints; + +// /** Creates a new AutoPlaceTest. */ +// public LeftSideAuto(Swerve swerve, ArmSub armSub, Gripper gripper) { +// this.swerve = swerve; +// this.armSub = armSub; +// this.gripper = gripper; +// controller = new Joystick(1); - // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) - TrajectoryConfig config = new TrajectoryConfig( - 1.7, - 1.7) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); - config.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - TrajectoryConfig placeConfig = new TrajectoryConfig( - 0.95, - 1.3) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); - config.addConstraint(new CentripetalAccelerationConstraint(0.3)); +// // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) +// TrajectoryConfig config = new TrajectoryConfig( +// 1.7, +// 1.7) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); +// config.addConstraint(new CentripetalAccelerationConstraint(0.3)); + +// TrajectoryConfig placeConfig = new TrajectoryConfig( +// 0.95, +// 1.3) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); +// config.addConstraint(new CentripetalAccelerationConstraint(0.3)); - TrajectoryConfig configrev = new TrajectoryConfig( - 3, - 3) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); - configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - TrajectoryConfig configrevdfast = new TrajectoryConfig( - 2, - 2) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); - configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - Trajectory traj1 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(3, 5.0, new Rotation2d(Math.PI)), - new Pose2d(2.3, 5.0, new Rotation2d(Math.PI))), - placeConfig); - - Trajectory traj2 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(2.2, 5.15, new Rotation2d(Math.PI)), - new Pose2d(2.8, 5.15, new Rotation2d(Math.PI))), - configrev); - - Trajectory traj3 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(2.8, 5.15, new Rotation2d(Math.PI)), - new Pose2d(4.0, 4.75, new Rotation2d(Math.PI))), - configrevdfast); - - Trajectory traj4 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(4.0, 5.15, new Rotation2d(Math.PI)), - new Pose2d(2.8, 5.15, new Rotation2d(Math.PI))), - config); - - Trajectory traj5 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(3, 5.15, new Rotation2d(Math.PI)), - new Pose2d(1.85, 5.0, new Rotation2d(Math.PI))), - config); - Trajectory traj6 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(2.1, 5.15, new Rotation2d(Math.PI)), - new Pose2d(2.8, 4.8, new Rotation2d(Math.PI))), - configrev); +// TrajectoryConfig configrev = new TrajectoryConfig( +// 3, +// 3) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); +// configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); + +// TrajectoryConfig configrevdfast = new TrajectoryConfig( +// 2, +// 2) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); +// configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); + +// Trajectory traj1 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(3, 5.0, new Rotation2d(Math.PI)), +// new Pose2d(2.3, 5.0, new Rotation2d(Math.PI))), +// placeConfig); + +// Trajectory traj2 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(2.2, 5.15, new Rotation2d(Math.PI)), +// new Pose2d(2.8, 5.15, new Rotation2d(Math.PI))), +// configrev); + +// Trajectory traj3 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(2.8, 5.15, new Rotation2d(Math.PI)), +// new Pose2d(4.0, 4.75, new Rotation2d(Math.PI))), +// configrevdfast); + +// Trajectory traj4 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(4.0, 5.15, new Rotation2d(Math.PI)), +// new Pose2d(2.8, 5.15, new Rotation2d(Math.PI))), +// config); + +// Trajectory traj5 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(3, 5.15, new Rotation2d(Math.PI)), +// new Pose2d(1.85, 5.0, new Rotation2d(Math.PI))), +// config); +// Trajectory traj6 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(2.1, 5.15, new Rotation2d(Math.PI)), +// new Pose2d(2.8, 4.8, new Rotation2d(Math.PI))), +// configrev); - var thetaController = new ProfiledPIDController( - 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); - - thetaController.enableContinuousInput(-Math.PI, Math.PI); - - swerveControllerCommand1 = new SwerveControllerCommand( - traj1, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(2, 0, 0), - new PIDController(2, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand2 = new SwerveControllerCommand( - traj2, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(2, 0, 0), - new PIDController(2, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand3 = new SwerveControllerCommand( - traj3, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(1.7, 0, 0), - new PIDController(1.7, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - swerveControllerCommand4 = new SwerveControllerCommand( - traj4, - swerve::getPose, - Constants.Swerve.swerveKinematics, - new PIDController(1, 0, 0), - new PIDController(1, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj1); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj2); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj3); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj4); - - swerveControllerCommand5 = new SwerveControllerCommand( - traj5, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(1, 0, 0), - new PIDController(1, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand6 = new SwerveControllerCommand( - traj6, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(1, 0, 0), - new PIDController(1, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); +// var thetaController = new ProfiledPIDController( +// 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); + +// thetaController.enableContinuousInput(-Math.PI, Math.PI); + +// swerveControllerCommand1 = new SwerveControllerCommand( +// traj1, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(2, 0, 0), +// new PIDController(2, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand2 = new SwerveControllerCommand( +// traj2, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(2, 0, 0), +// new PIDController(2, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand3 = new SwerveControllerCommand( +// traj3, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(1.7, 0, 0), +// new PIDController(1.7, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); +// swerveControllerCommand4 = new SwerveControllerCommand( +// traj4, +// swerve::getPose, +// Constants.Swerve.swerveKinematics, +// new PIDController(1, 0, 0), +// new PIDController(1, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj1); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj2); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj3); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj4); + +// swerveControllerCommand5 = new SwerveControllerCommand( +// traj5, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(1, 0, 0), +// new PIDController(1, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand6 = new SwerveControllerCommand( +// traj6, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(1, 0, 0), +// new PIDController(1, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); - addCommands( +// addCommands( - new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), - swerveControllerCommand1.raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - - // swerveControllerCommand2.raceWith(new WaitCommand(0.5).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), - // new GripperStateCommand(gripper, GripperState.CONE).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - new InstantCommand(() -> swerve.normalizeOdometry()), - swerveControllerCommand3.raceWith(/* new HoldArmPos(armSub, ArmPositions.STOWED) */new WaitCommand(0.5).andThen(new HoldArmPos(armSub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), - new TurnInPlace(swerve, true , false, 0), - new AutoLockToGamePiece(swerve, 0, 0, false, false , 0).raceWith(new GroundIntake(armSub, gripper)), - new WaitCommand(0.4 ).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - new TurnInPlace(swerve, true , false, 178), - swerveControllerCommand4.raceWith(new HoldArmPos(armSub, ArmPositions.STOWED)), - new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), - swerveControllerCommand5.raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - swerveControllerCommand6.raceWith(new WaitCommand(01).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), - new GripperStateCommand(gripper, GripperState.CONE), - new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP) +// new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), +// swerveControllerCommand1.raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), + +// // swerveControllerCommand2.raceWith(new WaitCommand(0.5).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), +// // new GripperStateCommand(gripper, GripperState.CONE).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// new InstantCommand(() -> swerve.normalizeOdometry()), +// swerveControllerCommand3.raceWith(/* new HoldArmPos(armSub, ArmPositions.STOWED) */new WaitCommand(0.5).andThen(new HoldArmPos(armSub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), +// new TurnInPlace(swerve, true , false, 0), +// new AutoLockToGamePiece(swerve, 0, 0, false, false , 0).raceWith(new GroundIntake(armSub, gripper)), +// new WaitCommand(0.4 ).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// new TurnInPlace(swerve, true , false, 178), +// swerveControllerCommand4.raceWith(new HoldArmPos(armSub, ArmPositions.STOWED)), +// new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), +// swerveControllerCommand5.raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// swerveControllerCommand6.raceWith(new WaitCommand(01).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), +// new GripperStateCommand(gripper, GripperState.CONE), +// new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP) - ); +// ); - } -} +// } +// } diff --git a/src/main/java/frc/robot/autos/LeftSideAutoRed.java b/src/main/java/frc/robot/autos/LeftSideAutoRed.java index 68aed86..d0f1659 100644 --- a/src/main/java/frc/robot/autos/LeftSideAutoRed.java +++ b/src/main/java/frc/robot/autos/LeftSideAutoRed.java @@ -1,230 +1,230 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.autos; - - -import java.util.List; - -// package edu.wpi.first.math.trajectory.constraint; - -import edu.wpi.first.math.geometry.Pose2d; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; -import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.GlobalVariables.ArmPositions; -import frc.robot.commands.AutoLockToGamePiece; -import frc.robot.commands.AutoSwervePositions; -import frc.robot.commands.GripperStateCommand; -import frc.robot.commands.GroundIntake; -import frc.robot.commands.HoldArmPos; -import frc.robot.commands.TeleopSwerve; -import frc.robot.commands.Test; -import frc.robot.commands.TurnInPlace; -import frc.robot.subsystems.ArmSub; -import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html - -public class LeftSideAutoRed extends SequentialCommandGroup { - Swerve swerve; - ArmSub armSub; - Gripper gripper; - SwerveControllerCommand swerveControllerCommand1; - SwerveControllerCommand swerveControllerCommand2; - SwerveControllerCommand swerveControllerCommand3; - SwerveControllerCommand swerveControllerCommand4; - SwerveControllerCommand swerveControllerCommand5; - SwerveControllerCommand swerveControllerCommand6; - Joystick controller; - -MinMax minMax; - TrajectoryConstraint constraints; - - /** Creates a new AutoPlaceTest. */ - public LeftSideAutoRed(Swerve swerve, ArmSub armSub, Gripper gripper) { - this.swerve = swerve; - this.armSub = armSub; - this.gripper = gripper; - controller = new Joystick(1); +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. + +// package frc.robot.autos; + + +// import java.util.List; + +// // package edu.wpi.first.math.trajectory.constraint; + +// import edu.wpi.first.math.geometry.Pose2d; + +// import edu.wpi.first.math.controller.PIDController; +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; +// import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; +// import edu.wpi.first.wpilibj.Joystick; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants; +// import frc.robot.GlobalVariables.ArmPositions; +// import frc.robot.commands.AutoLockToGamePiece; +// import frc.robot.commands.AutoSwervePositions; +// import frc.robot.commands.GripperStateCommand; +// import frc.robot.commands.GroundIntake; +// import frc.robot.commands.HoldArmPos; +// import frc.robot.commands.TeleopSwerve; +// import frc.robot.commands.Test; +// import frc.robot.commands.TurnInPlace; +// import frc.robot.subsystems.ArmSub; +// import frc.robot.subsystems.Gripper; +// import frc.robot.subsystems.Swerve; +// import frc.robot.subsystems.Gripper.GripperState; + +// // NOTE: Consider using this command inline, rather than writing a subclass. For more +// // information, see: +// // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html + +// public class LeftSideAutoRed extends SequentialCommandGroup { +// Swerve swerve; +// ArmSub armSub; +// Gripper gripper; +// SwerveControllerCommand swerveControllerCommand1; +// SwerveControllerCommand swerveControllerCommand2; +// SwerveControllerCommand swerveControllerCommand3; +// SwerveControllerCommand swerveControllerCommand4; +// SwerveControllerCommand swerveControllerCommand5; +// SwerveControllerCommand swerveControllerCommand6; +// Joystick controller; + +// MinMax minMax; +// TrajectoryConstraint constraints; + +// /** Creates a new AutoPlaceTest. */ +// public LeftSideAutoRed(Swerve swerve, ArmSub armSub, Gripper gripper) { +// this.swerve = swerve; +// this.armSub = armSub; +// this.gripper = gripper; +// controller = new Joystick(1); - // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) - TrajectoryConfig config = new TrajectoryConfig( - 1.7, - 1.7) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); - config.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - TrajectoryConfig placeConfig = new TrajectoryConfig( - 0.95, - 1.3) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); - config.addConstraint(new CentripetalAccelerationConstraint(0.3)); +// // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) +// TrajectoryConfig config = new TrajectoryConfig( +// 1.7, +// 1.7) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); +// config.addConstraint(new CentripetalAccelerationConstraint(0.3)); + +// TrajectoryConfig placeConfig = new TrajectoryConfig( +// 0.95, +// 1.3) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); +// config.addConstraint(new CentripetalAccelerationConstraint(0.3)); - TrajectoryConfig configrev = new TrajectoryConfig( - 3, - 3) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); - configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - TrajectoryConfig configrevdfast = new TrajectoryConfig( - 2, - 2) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); - configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - Trajectory traj1 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(13.54, 5.0, new Rotation2d(0)), - new Pose2d(14.24, 5.0, new Rotation2d(0))), - placeConfig); - - Trajectory traj2 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(14.34, 5.15, new Rotation2d(0)), - new Pose2d(13.74, 5.15, new Rotation2d(0))), - configrev); - - Trajectory traj3 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(13.74, 5.15, new Rotation2d(0)), - new Pose2d(12.54, 4.75, new Rotation2d(0))), - configrevdfast); - - Trajectory traj4 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(12.54, 5.15, new Rotation2d(0)), - new Pose2d(13.74, 5.15, new Rotation2d(0))), - config); - - Trajectory traj5 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(13.54, 5.15, new Rotation2d(0)), - new Pose2d(14.69, 5.0, new Rotation2d(0))), - config); - Trajectory traj6 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(14.44, 5.15, new Rotation2d(0)), - new Pose2d(13.74, 4.8, new Rotation2d(0))), - configrev); +// TrajectoryConfig configrev = new TrajectoryConfig( +// 3, +// 3) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); +// configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); + +// TrajectoryConfig configrevdfast = new TrajectoryConfig( +// 2, +// 2) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); +// configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); + +// Trajectory traj1 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(13.54, 5.0, new Rotation2d(0)), +// new Pose2d(14.24, 5.0, new Rotation2d(0))), +// placeConfig); + +// Trajectory traj2 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(14.34, 5.15, new Rotation2d(0)), +// new Pose2d(13.74, 5.15, new Rotation2d(0))), +// configrev); + +// Trajectory traj3 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(13.74, 5.15, new Rotation2d(0)), +// new Pose2d(12.54, 4.75, new Rotation2d(0))), +// configrevdfast); + +// Trajectory traj4 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(12.54, 5.15, new Rotation2d(0)), +// new Pose2d(13.74, 5.15, new Rotation2d(0))), +// config); + +// Trajectory traj5 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(13.54, 5.15, new Rotation2d(0)), +// new Pose2d(14.69, 5.0, new Rotation2d(0))), +// config); +// Trajectory traj6 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(14.44, 5.15, new Rotation2d(0)), +// new Pose2d(13.74, 4.8, new Rotation2d(0))), +// configrev); - var thetaController = new ProfiledPIDController( - 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); - - thetaController.enableContinuousInput(-Math.PI, Math.PI); - - swerveControllerCommand1 = new SwerveControllerCommand( - traj1, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(2, 0, 0), - new PIDController(2, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand2 = new SwerveControllerCommand( - traj2, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(2, 0, 0), - new PIDController(2, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand3 = new SwerveControllerCommand( - traj3, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(1.7, 0, 0), - new PIDController(1.7, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - swerveControllerCommand4 = new SwerveControllerCommand( - traj4, - swerve::getPose, - Constants.Swerve.swerveKinematics, - new PIDController(1, 0, 0), - new PIDController(1, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj1); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj2); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj3); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj4); - - swerveControllerCommand5 = new SwerveControllerCommand( - traj5, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(1, 0, 0), - new PIDController(1, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand6 = new SwerveControllerCommand( - traj6, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(1, 0, 0), - new PIDController(1, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); +// var thetaController = new ProfiledPIDController( +// 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); + +// thetaController.enableContinuousInput(-Math.PI, Math.PI); + +// swerveControllerCommand1 = new SwerveControllerCommand( +// traj1, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(2, 0, 0), +// new PIDController(2, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand2 = new SwerveControllerCommand( +// traj2, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(2, 0, 0), +// new PIDController(2, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand3 = new SwerveControllerCommand( +// traj3, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(1.7, 0, 0), +// new PIDController(1.7, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); +// swerveControllerCommand4 = new SwerveControllerCommand( +// traj4, +// swerve::getPose, +// Constants.Swerve.swerveKinematics, +// new PIDController(1, 0, 0), +// new PIDController(1, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj1); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj2); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj3); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj4); + +// swerveControllerCommand5 = new SwerveControllerCommand( +// traj5, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(1, 0, 0), +// new PIDController(1, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand6 = new SwerveControllerCommand( +// traj6, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(1, 0, 0), +// new PIDController(1, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); - addCommands( - new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), - swerveControllerCommand1.raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - new InstantCommand(() -> swerve.normalizeOdometry()), - swerveControllerCommand3.raceWith(/* new HoldArmPos(armSub, ArmPositions.STOWED) */new WaitCommand(0.5).andThen(new HoldArmPos(armSub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), - new TurnInPlace(swerve, true , false, 178), - new AutoLockToGamePiece(swerve, 0, 0, false, false , 0).raceWith(new GroundIntake(armSub, gripper)), - new WaitCommand(0.4 ).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - new TurnInPlace(swerve, true , false, 0), - swerveControllerCommand4.raceWith(new HoldArmPos(armSub, ArmPositions.STOWED)), - new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), - swerveControllerCommand5.raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - swerveControllerCommand6.raceWith(new WaitCommand(1).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), - new GripperStateCommand(gripper, GripperState.CONE), - new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP) +// addCommands( +// new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), +// swerveControllerCommand1.raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// new InstantCommand(() -> swerve.normalizeOdometry()), +// swerveControllerCommand3.raceWith(/* new HoldArmPos(armSub, ArmPositions.STOWED) */new WaitCommand(0.5).andThen(new HoldArmPos(armSub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), +// new TurnInPlace(swerve, true , false, 178), +// new AutoLockToGamePiece(swerve, 0, 0, false, false , 0).raceWith(new GroundIntake(armSub, gripper)), +// new WaitCommand(0.4 ).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// new TurnInPlace(swerve, true , false, 0), +// swerveControllerCommand4.raceWith(new HoldArmPos(armSub, ArmPositions.STOWED)), +// new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), +// swerveControllerCommand5.raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// swerveControllerCommand6.raceWith(new WaitCommand(1).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), +// new GripperStateCommand(gripper, GripperState.CONE), +// new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP) - ); +// ); - } -} +// } +// } diff --git a/src/main/java/frc/robot/commands/ArmExtendTest.java b/src/main/java/frc/robot/commands/ArmExtendTest.java new file mode 100644 index 0000000..30dd2d8 --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmExtendTest.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ArmSub; + +public class ArmExtendTest extends CommandBase { + ArmSub armsub; + /** Creates a new ArmExtendTest. */ + public ArmExtendTest(ArmSub armsub) { + this.armsub = armsub; + + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + armsub.extendArmPercentOutput(0.05); + // armsub.extendArmPosition(1000); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + armsub.extendArmPercentOutput(0); + // armsub.extendArmPosition(0); + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ArmPositionCommand.java b/src/main/java/frc/robot/commands/ArmPositionCommand.java index c735966..9bfb903 100644 --- a/src/main/java/frc/robot/commands/ArmPositionCommand.java +++ b/src/main/java/frc/robot/commands/ArmPositionCommand.java @@ -32,7 +32,7 @@ public void initialize() { @Override public void execute() { timer ++; - armSub.armPresetPositions(armPos); + armSub.armRotPresetPositions(armPos); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index fc84db7..174f27e 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -32,7 +32,7 @@ public void initialize() { @Override public void execute() { timer++; - armSub.armPresetPositions(ArmPositions.GROUND_PICKUP); + armSub.armRotPresetPositions(ArmPositions.GROUND_PICKUP_ADAPTIVE); if (timer >= 20){ gripper.setClaw(GripperState.OPEN); gripper.moveGripper(1); diff --git a/src/main/java/frc/robot/commands/HoldArmPos.java b/src/main/java/frc/robot/commands/HoldArmPos.java index 1d9780e..1122ec3 100644 --- a/src/main/java/frc/robot/commands/HoldArmPos.java +++ b/src/main/java/frc/robot/commands/HoldArmPos.java @@ -33,7 +33,7 @@ public void initialize() { @Override public void execute() { timer ++; - armSub.armPresetPositions(armPos); + armSub.armRotPresetPositions(armPos); System.out.println("ARM COMMAND EXEC"); } diff --git a/src/main/java/frc/robot/subsystems/ArmSub.java b/src/main/java/frc/robot/subsystems/ArmSub.java index bb2839d..54ab95d 100644 --- a/src/main/java/frc/robot/subsystems/ArmSub.java +++ b/src/main/java/frc/robot/subsystems/ArmSub.java @@ -8,27 +8,20 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.ctre.phoenix.motorcontrol.TalonSRXFeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonFX; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.AbsoluteSensorRange; import com.ctre.phoenix.sensors.CANCoder; -import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkMax; -import com.revrobotics.ColorSensorV3; -import com.revrobotics.EncoderType; -import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxAbsoluteEncoder; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.SparkMaxRelativeEncoder.Type; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.math.Conversions; import frc.robot.Constants; @@ -42,6 +35,7 @@ public class ArmSub extends SubsystemBase { public CANSparkMax gripperJointEncoder; public CANSparkMax gripperJointNeo; + public SparkMaxPIDController gripperJointPID; public SparkMaxAbsoluteEncoder encoder; public PIDController armPID; public PIDController jointPID; @@ -67,12 +61,19 @@ public ArmSub() { rotMaxSpeedFor = 0.4; rotMaxSpeedRev = 0.65; - gripperJointEncoder = new CANSparkMax(55, MotorType.kBrushless); - encoder = gripperJointEncoder.getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type.kDutyCycle); + // gripperJointEncoder = new CANSparkMax(55, MotorType.kBrushless); + // encoder = gripperJointEncoder.getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type.kDutyCycle); gripperJointNeo = new CANSparkMax(56, MotorType.kBrushless); gripperJointNeo.clearFaults(); gripperJointNeo.restoreFactoryDefaults(); + gripperJointNeo.getEncoder().setPosition(0); + gripperJointNeo.setIdleMode(IdleMode.kBrake); + gripperJointPID = gripperJointNeo.getPIDController(); + gripperJointPID.setP(1); + gripperJointPID.setI(0); + gripperJointPID.setD(0); + gripperJointPID.setFeedbackDevice(gripperJointNeo.getEncoder()); // pidLoop = new PIDController(0.006, 0.003, 0.001); @@ -119,18 +120,26 @@ public ArmSub() { telescopeArm.configPeakOutputReverse(-telMaxSpeedRev); telescopeArm.setSensorPhase(true); telescopeArm.setInverted(InvertType.None); - telescopeArm.config_kP(0, 0.05); + telescopeArm.config_kP(0, 0.4); telescopeArm.config_kI(0, 0.0); telescopeArm.config_kD(0, 0.0); - telescopeArm.config_kF(0, 0.06); + telescopeArm.config_kF(0, 0.0); telescopeArm.setNeutralMode(NeutralMode.Brake); + telescopeArm.configNeutralDeadband(0.00001); + telescopeArm.configMotionAcceleration(6000); + telescopeArm.configMotionCruiseVelocity(10000); + telescopeArm.setSelectedSensorPosition(0); } - + public void moveRotArmMotionMagic(double degrees){ + // TODO degrees to falcon readout + double setpoint = degrees *3; + leftArm.set(TalonFXControlMode.MotionMagic, setpoint); + } /** * Moves the arm by percent output. * @param percent 0.0 - 1.0. @@ -156,7 +165,16 @@ public void extendArmPercentOutput(double percent){ } public void extendArmPosition(double position){ - telescopeArm.set(TalonFXControlMode.Position, position); + // telescopeArm.set(TalonFXControlMode.Position, position); + telescopeArm.set(TalonFXControlMode.MotionMagic, position); +} + +public void gripperJointPosition(double position){ + gripperJointPID.setReference(position, ControlType.kPosition); +} + +public double getTelescopePos(){ + return telescopeArm.getSelectedSensorPosition(); } /** @@ -170,92 +188,217 @@ public void moveArmPosition(double position){ * Moves the arm to a dedicated preset positions * @param position The dedicated position enum, current options are GROUND_PICKUP, STOWED, GROUND_SCORE, MID_SCORE, HIGH_SCORE, and SHELF_PICKUP */ - public void armPresetPositions(GlobalVariables.ArmPositions position){ + public void armRotPresetPositions(GlobalVariables.ArmPositions position){ switch(position){ - case GROUND_PICKUP: - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.grndIntakePosValue)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.grndIntakePosValue)); + case GROUND_PICKUP_ADAPTIVE: + if(GlobalVariables.gamePiece == 0){ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForConePickup); + } + else{ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForCubePickup); + } break; - case STOWED: - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.stowedPosValue)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.stowedPosValue)); + case STOWED_ADAPTIVE: + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotStow); + break; + case GROUND_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForLowCone); + } + else{ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForLowCube); + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevLowCone); + } + else{ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevLowCube); + } + } break; - case GROUND_SCORE: - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.grndScorePosValueCone)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.grndScorePosValue)); + case SHELF_PICKUP_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForShelfCone); + } + else{ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForShelfCube); + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevShelfCone); + } + else{ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevShelfCube); + } + } break; - case MID_SCORE_CONE: - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.midScorePosValueCone)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.midScorePosValue)); + case MID_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForMidCone); + } + else{ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForMidCube); + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevMidCone); + } + else{ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevMidCube); + } + } + break; + case HIGH_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForHighCone); + } + else{ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForHighCube); + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + moveRotArmMotionMagic( Constants.ArmRotationValues.armRotRevHighCone); + } + else{ + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevHighCube); + } + } break; - case HIGH_SCORE_CONE: - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.highScorePosValueCone)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.highScorePosValue)); + + default: + moveRotArmMotionMagic(Constants.ArmRotationValues.armRotStow); break; - case MID_SCORE_CUBE: - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.midScorePosValueCube)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.midScorePosValue)); + } + } - break; + public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ + switch(position){ - case HIGH_SCORE_CUBE: - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.highScorePosValueCube)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.highScorePosValue)); + case GROUND_PICKUP_ADAPTIVE: + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.ArmExtendValues.armExtendConePickup); + } + else{ + extendArmPosition(Constants.ArmExtendValues.armExtendCubePickup); + } break; - case SHELF_PICKUP: - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.shelfIntakePosValue)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.shelfIntakePosValue)); + case STOWED_ADAPTIVE: + extendArmPosition(Constants.ArmExtendValues.armExtendStow); break; - case MID_SCORE_ADAPTIVE: - if(GlobalVariables.gamePiece == 0){ - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.midScorePosValueCone)); + case GROUND_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.ArmExtendValues.armExtendForLowCone); + } + else{ + extendArmPosition(Constants.ArmExtendValues.armExtendForLowCube); + } } else{ - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.midScorePosValueCube)); + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.ArmExtendValues.armExtendRevLowCone); + } + else{ + extendArmPosition(Constants.ArmExtendValues.armExtendRevLowCube); + } } - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.shelfIntakePosValue)); - + break; - case HIGH_SCORE_ADAPTIVE: - if(GlobalVariables.gamePiece == 0){ - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.highScorePosValueCone)); + + case SHELF_PICKUP_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.ArmExtendValues.armExtendForShelfCone); + } + else{ + extendArmPosition(Constants.ArmExtendValues.armExtendForShelfCube); + } } else{ - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.highScorePosValueCube)); + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.ArmExtendValues.armExtendRevShelfCone); + } + else{ + extendArmPosition(Constants.ArmExtendValues.armExtendRevShelfCube); + + } + } + break; + case MID_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.ArmExtendValues.armExtendForMidCone); + } + else{ + extendArmPosition(Constants.ArmExtendValues.armExtendForMidCube); + } } - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.shelfIntakePosValue)); + else{ + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.ArmExtendValues.armExtendRevMidCone); + } + else{ + extendArmPosition(Constants.ArmExtendValues.armExtendRevMidCube); + } + } + break; - case CONE_UPRIGHT: - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.coneUprightPosValue)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.highScorePosValue)); + case HIGH_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.ArmExtendValues.armExtendForHighCone); + } + else{ + extendArmPosition(Constants.ArmExtendValues.armExtendForHighCube); + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.ArmExtendValues.armExtendRevHighCone); + } + else{ + extendArmPosition(Constants.ArmExtendValues.armExtendRevHighCube); + } + } + break; + default: - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(getArmPosition(), Constants.shelfIntakePosValue)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.shelfIntakePosValue)); + extendArmPosition(Constants.ArmExtendValues.armExtendStow); break; } } + /** * Returns current arm position * @return CANcoder arm position in degrees. @@ -263,6 +406,11 @@ public void armPresetPositions(GlobalVariables.ArmPositions position){ public double getArmPosition(){ return testCanCoder.getAbsolutePosition(); } + + public double getRotArmPos(){ + // TODO make this gearbox to degrees + return leftArm.getSelectedSensorPosition() /3; + } /** * Returns current arm motor position. * @return Current arm falcon position unfiltered. @@ -289,5 +437,7 @@ public void periodic() { SmartDashboard.putNumber("Arm Motor Position", getArmMotorPos()); SmartDashboard.putNumber("Arm Motor 1 Current", leftArm.getStatorCurrent()); SmartDashboard.putNumber("Arm Motor 2 Current", rightArm.getStatorCurrent()); + SmartDashboard.putNumber("Telescope Position", telescopeArm.getSelectedSensorPosition()); + SmartDashboard.putNumber("Telescope Speed?", telescopeArm.getSupplyCurrent()); } } diff --git a/src/main/java/frc/robot/subsystems/Gripper.java b/src/main/java/frc/robot/subsystems/Gripper.java index ae714a7..6acf84c 100644 --- a/src/main/java/frc/robot/subsystems/Gripper.java +++ b/src/main/java/frc/robot/subsystems/Gripper.java @@ -28,6 +28,7 @@ public class Gripper extends SubsystemBase { DoubleSolenoid cubeSolenoid; Solenoid coneSolenoid; SparkMaxPIDController gripperPID; + public PneumaticsControlModule module; Compressor compressor; public final I2C.Port i2cPort = I2C.Port.kMXP; @@ -40,6 +41,8 @@ public enum GripperState{ OPEN, CONE, CUBE } public GripperState gripperState = GripperState.OPEN; + + /** Creates a new Gripper. */ public Gripper() { gripperMotor = new CANSparkMax(42, MotorType.kBrushless); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f5d8c09..dcc4491 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -22,6 +22,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -151,6 +153,25 @@ public void updateOdometry() { } + public void updateRobotDirection(){ + if (DriverStation.getAlliance() == Alliance.Blue){ + if(gyro.getYaw()>=90 || gyro.getYaw() <= -90){ + GlobalVariables.robotDirection = true; + } + else{ + GlobalVariables.robotDirection = false; + } + } + else{ + if(gyro.getYaw()>=90 || gyro.getYaw() <= -90){ + GlobalVariables.robotDirection = false; + } + else{ + GlobalVariables.robotDirection = true; + } + } + } + public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { SwerveModuleState[] swerveModuleStates = diff --git a/vendordeps/REV2mDistanceSensor.json b/vendordeps/REV2mDistanceSensor.json new file mode 100644 index 0000000..ecf0636 --- /dev/null +++ b/vendordeps/REV2mDistanceSensor.json @@ -0,0 +1,55 @@ +{ + "fileName": "REV2mDistanceSensor.json", + "name": "REV2mDistanceSensor", + "version": "0.4.0", + "uuid": "9e352acd-4eec-40f7-8490-3357b5ed65ae", + "mavenUrls": [ + "https://www.revrobotics.com/content/sw/max/sdk/maven/" + ], + "jsonUrl": "https://www.revrobotics.com/content/sw/max/sdk/Rev2mDistanceSensor.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-java", + "version": "0.4.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-driver", + "version": "0.4.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "linuxathena" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-cpp", + "version": "0.4.0", + "libName": "libDistanceSensor", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-driver", + "version": "0.4.0", + "libName": "libDistanceSensorDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + } + ] +} \ No newline at end of file From 966d786b39a37893746a116076d9e83b77479771 Mon Sep 17 00:00:00 2001 From: Oscar Date: Mon, 20 Mar 2023 20:16:24 -0700 Subject: [PATCH 04/12] Idk --- .../java/frc/robot/commands/LockRotation.java | 32 +++++++ .../frc/robot/commands/LockToGamePiece.java | 90 ------------------- 2 files changed, 32 insertions(+), 90 deletions(-) create mode 100644 src/main/java/frc/robot/commands/LockRotation.java delete mode 100644 src/main/java/frc/robot/commands/LockToGamePiece.java diff --git a/src/main/java/frc/robot/commands/LockRotation.java b/src/main/java/frc/robot/commands/LockRotation.java new file mode 100644 index 0000000..3dfdc49 --- /dev/null +++ b/src/main/java/frc/robot/commands/LockRotation.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class LockRotation extends CommandBase { + /** Creates a new LockRotation. */ + public LockRotation() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/LockToGamePiece.java b/src/main/java/frc/robot/commands/LockToGamePiece.java deleted file mode 100644 index cd5c92f..0000000 --- a/src/main/java/frc/robot/commands/LockToGamePiece.java +++ /dev/null @@ -1,90 +0,0 @@ -package frc.robot.commands; - -import frc.robot.Constants; -import frc.robot.GlobalVariables; -import frc.robot.subsystems.Swerve; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.CommandBase; - - -public class LockToGamePiece extends CommandBase { - - private double rotation; - private Translation2d translation; - private boolean fieldRelative; - private boolean openLoop; - - private Swerve s_Swerve; - private Joystick controller; - private int translationAxis; - private int strafeAxis; - - private int pipeline; - private double mapdouble(double x, double in_min, double in_max, double out_min, double out_max){ - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; - } - - /** - * Driver control - */ - public LockToGamePiece(Swerve s_Swerve, Joystick controller, int translationAxis, int strafeAxis, boolean fieldRelative, boolean openLoop, int pipeline) { - this.s_Swerve = s_Swerve; - addRequirements(s_Swerve); - - this.controller = controller; - this.translationAxis = translationAxis; - this.strafeAxis = strafeAxis; - this.fieldRelative = fieldRelative; - this.openLoop = openLoop; - this.pipeline = pipeline; - } - - @Override - public void execute() { - double yAxis = -controller.getRawAxis(translationAxis); - double xAxis = controller.getRawAxis(strafeAxis); - double rAxis = s_Swerve.frontCamOffset(pipeline); - - - /* Deadbands */ - - if (Math.abs(yAxis) > Constants.stickDeadband) { - if (yAxis > 0){ - yAxis = mapdouble(yAxis, Constants.stickDeadband, 1, 0, 1); - } else { - yAxis = mapdouble(yAxis, -Constants.stickDeadband, -1, 0, -1); - } - } - else{ - yAxis = 0; - } - - if (Math.abs(xAxis) > Constants.stickDeadband) { - if (xAxis > 0){ - xAxis = mapdouble(xAxis, Constants.stickDeadband, 1, 0, 1); - } else { - xAxis = mapdouble(xAxis, -Constants.stickDeadband, -1, 0, -1); - } - } - else{ - xAxis = 0; - } - - if (Math.abs(rAxis) > Constants.stickDeadband) { - if (rAxis > 0){ - rAxis = mapdouble(rAxis, Constants.stickDeadband, 1, 0, 1); - } else { - rAxis = mapdouble(rAxis, -Constants.stickDeadband, -1, 0, -1); - } - } - else{ - rAxis = 0; - } - - translation = new Translation2d(yAxis, xAxis).times(Constants.Swerve.maxSpeed); - rotation = rAxis * Constants.Swerve.maxAngularVelocity; - s_Swerve.drive(translation, rotation, fieldRelative, openLoop); - } -} From be7f162610572f9782f0019092278a56ac7fe936 Mon Sep 17 00:00:00 2001 From: Oscar Date: Wed, 22 Mar 2023 20:28:43 -0700 Subject: [PATCH 05/12] Gripper Tested --- src/main/java/frc/robot/Constants.java | 3 +- .../java/frc/robot/PhotonCameraWrapper.java | 110 +++++++++++---- src/main/java/frc/robot/RobotContainer.java | 35 +++-- .../robot/commands/AutoLockToGamePiece.java | 90 ------------- .../robot/commands/AutoSwervePositions.java | 1 - .../frc/robot/commands/GripperCommand.java | 9 +- .../robot/commands/GripperStateCommand.java | 49 ------- .../java/frc/robot/commands/GroundIntake.java | 5 +- .../java/frc/robot/commands/LockRotation.java | 124 +++++++++++++---- .../java/frc/robot/commands/ShelfIntake.java | 52 -------- src/main/java/frc/robot/commands/Test.java | 1 - src/main/java/frc/robot/commands/TestRed.java | 1 - .../java/frc/robot/subsystems/ArmSub.java | 125 ++++++++++++++++++ .../java/frc/robot/subsystems/Gripper.java | 120 ++++------------- .../java/frc/robot/subsystems/Swerve.java | 88 +++++++----- 15 files changed, 412 insertions(+), 401 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/AutoLockToGamePiece.java delete mode 100644 src/main/java/frc/robot/commands/GripperStateCommand.java delete mode 100644 src/main/java/frc/robot/commands/ShelfIntake.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e58c37b..dc4d137 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -112,7 +112,8 @@ public static final class ArmExtendValues { } public static final class JointRotationValues { - public static final double JointRotStow = 0; + public static final double JointRotStowCone = 0; + public static final double JointRotStowCube = 0; public static final double JointRotConePickup = 0; public static final double JointRotCubePickup = 0; diff --git a/src/main/java/frc/robot/PhotonCameraWrapper.java b/src/main/java/frc/robot/PhotonCameraWrapper.java index 3d64c41..b5d1277 100644 --- a/src/main/java/frc/robot/PhotonCameraWrapper.java +++ b/src/main/java/frc/robot/PhotonCameraWrapper.java @@ -17,9 +17,8 @@ /** Add your docs here. */ public class PhotonCameraWrapper { - public PhotonCamera camera1; - public PhotonCamera camera2; public PhotonCamera frontCam; + public PhotonCamera rearCam; public PhotonPoseEstimator photonPoseEstimator; public PhotonPoseEstimator photonPoseEstimator2; @@ -28,52 +27,107 @@ public PhotonCameraWrapper() { AprilTagFieldLayout aprilTagFieldLayout; try { aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile); - camera1 = new PhotonCamera(VisionConstants.camera1Name); - camera2 = new PhotonCamera(VisionConstants.camera2Name); + frontCam = new PhotonCamera(VisionConstants.camera1Name); + rearCam = new PhotonCamera(VisionConstants.camera2Name); - frontCam = new PhotonCamera(VisionConstants.frontCamName); - photonPoseEstimator2 = new PhotonPoseEstimator(aprilTagFieldLayout, org.photonvision.PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, camera2, VisionConstants.robotToCam1); - photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, org.photonvision.PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, camera1, VisionConstants.robotToCam2); + photonPoseEstimator2 = new PhotonPoseEstimator(aprilTagFieldLayout, org.photonvision.PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, frontCam, VisionConstants.robotToCam1); + photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, org.photonvision.PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, rearCam, VisionConstants.robotToCam2); } catch (Exception e) { System.out.println(e); } } - public void frontCamPipeline(int pipelineIndex){ - frontCam.setPipelineIndex(pipelineIndex);; - } - public double getYOffset(){ - var result = frontCam.getLatestResult(); +// public void frontCamPipeline(int pipelineIndex){ +// frontCam.setPipelineIndex(pipelineIndex);; +// } + +public void cameraPipelines(int pipeline){ + frontCam.setPipelineIndex(pipeline); + rearCam.setPipelineIndex(pipeline); +} + +public double getFrontCamOffset(){ + var result = frontCam.getLatestResult(); + + if (!result.hasTargets()) + return 0; + + + return result.getBestTarget().getYaw(); + +} + +public double getRearCamOffset(){ + var result = frontCam.getLatestResult(); + + if (!result.hasTargets()) + return 0; + + + return result.getBestTarget().getYaw(); + +} + +public double getFrontCamDistance(){ + var result = frontCam.getLatestResult(); + + if (!result.hasTargets()) + return 0; + + + return result.getBestTarget().getBestCameraToTarget().getX(); + +} - if (!result.hasTargets()) - return 0; +public double getRearCamDistance(){ + var result = frontCam.getLatestResult(); + + if (!result.hasTargets()) + return 0; + + + return result.getBestTarget().getBestCameraToTarget().getX(); + +} + + + + + + + + // public double getYOffset(){ + // var result = frontCam.getLatestResult(); + + // if (!result.hasTargets()) + // return 0; - return result.getBestTarget().getYaw(); + // return result.getBestTarget().getYaw(); - } + // } - public double AutoGetYOffset(){ - var result = frontCam.getLatestResult(); + // public double AutoGetYOffset(){ + // var result = frontCam.getLatestResult(); - if (!result.hasTargets()) - return 0.5; + // if (!result.hasTargets()) + // return 0.5; - return result.getBestTarget().getYaw(); + // return result.getBestTarget().getYaw(); - } + // } - public double AutoTurnTOCOne(){ - var result = frontCam.getLatestResult(); + // public double AutoTurnTOCOne(){ + // var result = frontCam.getLatestResult(); - if (!result.hasTargets()) - return 0.5; + // if (!result.hasTargets()) + // return 0.5; - return result.getBestTarget().getYaw(); + // return result.getBestTarget().getYaw(); - } + // } public Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) { if (photonPoseEstimator == null || photonPoseEstimator2 == null) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9d7e645..8840b5b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,7 +27,6 @@ import frc.robot.autos.*; import frc.robot.commands.*; import frc.robot.subsystems.*; -import frc.robot.subsystems.Gripper.GripperState; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -69,6 +68,12 @@ public class RobotContainer { private final Trigger operatorExtraRight = new JoystickButton(operatorPanel, 15); private final Trigger xboxRightJoyButton = new JoystickButton(driver, 10); + private final Trigger panelRollers = new JoystickButton(operatorPanel, 10); + private final Trigger panelShelf = new JoystickButton(operatorPanel, 9); + + + + // private final Trigger TopLeftSwitch = new JoystickButton(operatorPanel) public final Swerve s_Swerve = new Swerve(); public final ArmSub armSub = new ArmSub(); @@ -105,7 +110,7 @@ public RobotContainer() { s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, joystickPanel, translationAxis, strafeAxis, rotationAxis, fieldRelative, openLoop). withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - armSub.setDefaultCommand(new HoldArmPos(armSub, ArmPositions.STOWED_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // armSub.setDefaultCommand(new HoldArmPos(armSub, ArmPositions.STOWED_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); //candle.setDefaultCommand(new InstantCommand(() -> candle.setColor(operatorExtraRight.getAsBoolean()))); candle.setDefaultCommand(new SetLedCommand(candle)); configureButtonBindings(); @@ -118,36 +123,38 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - operatorExtraRight.whileTrue(new ConeVCube(0)); - operatorExtraRight.whileFalse(new ConeVCube(1)); + panelRollers.whileTrue(new GripperCommand(gripper, 0.5)); //Push OUT + panelShelf.whileTrue(new GripperCommand(gripper, -0.5)); // Take IN + + // operatorExtraRight.whileTrue(new ConeVCube(0)); + // operatorExtraRight.whileFalse(new ConeVCube(1)); // operatorIntakeUp.whileTrue(new HoldArmPos(armSub, ArmPositions.STOWED).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); // operatorIntakeUp.whileFalse(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); // operatorIntakeDown.whileTrue(new HoldArmPos(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); // operatorIntakeDown.whileFalse(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - operatorLatch.toggleOnTrue(new InstantCommand(() -> gripper.setClaw(GripperState.CONE)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); // operatorHook.toggleOnTrue(new InstantCommand(() -> gripper.setClaw(GripperState.OPEN)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - operatorSpinup.whileTrue(new LockToGamePiece(s_Swerve, joystickPanel, translationAxis, strafeAxis, true, false, GlobalVariables.gamePiece).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - operatorLeftEmpty.toggleOnTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // operatorSpinup.whileTrue(new LockToGamePiece(s_Swerve, joystickPanel, translationAxis, strafeAxis, true, false, GlobalVariables.gamePiece).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // operatorLeftEmpty.toggleOnTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - operatorHook.whileTrue(new ArmExtendTest(armSub)); - rightJoy.onTrue(new InstantCommand(() -> s_Swerve.zeroGyro())); - leftJoy.whileTrue(new LockToGamePiece(s_Swerve, joystickPanel, translationAxis, strafeAxis, true, true, GlobalVariables.gamePiece).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // operatorHook.whileTrue(new ArmExtendTest(armSub)); + // rightJoy.onTrue(new InstantCommand(() -> s_Swerve.zeroGyro())); + // leftJoy.whileTrue(new LockToGamePiece(s_Swerve, joystickPanel, translationAxis, strafeAxis, true, true, GlobalVariables.gamePiece).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); // buttonX.toggleOnTrue(new HoldArmPos(armSub, ArmPositions.STOWED).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); // buttonY.whileTrue(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); // buttonB.whileTrue(new HoldArmPos(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - buttonA.whileTrue(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - xboxRightJoyButton.whileTrue(new HoldArmPos(armSub, ArmPositions.CONE_UPRIGHT).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // buttonA.whileTrue(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // xboxRightJoyButton.whileTrue(new HoldArmPos(armSub, ArmPositions.CONE_UPRIGHT).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); // leftBumper.toggleOnTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); // leftBumper.whileTrue(new HoldArmPos(armSub, ArmPositions.SHELF_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - rightBumper.toggleOnTrue(new ShelfIntake(gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // rightBumper.toggleOnTrue(new ShelfIntake(gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); // operatorDeploy.onTrue(new TurnInPlace(s_Swerve, true, false, 0)); - operatorDeploy.onTrue(new CustomSwerveTrajectory(s_Swerve, 10, 4, 0, 3, 10)); + // operatorDeploy.onTrue(new CustomSwerveTrajectory(s_Swerve, 10, 4, 0, 3, 10)); // operatorRightEmpty.onTrue(new BalanceAuto(s_Swerve)); // TODO RED SIDE // LeftLittleButton.onTrue(new AutoPlaceRed(s_Swerve, armSub, gripper)) ; diff --git a/src/main/java/frc/robot/commands/AutoLockToGamePiece.java b/src/main/java/frc/robot/commands/AutoLockToGamePiece.java deleted file mode 100644 index f8adb7e..0000000 --- a/src/main/java/frc/robot/commands/AutoLockToGamePiece.java +++ /dev/null @@ -1,90 +0,0 @@ -package frc.robot.commands; - -import frc.robot.Constants; -import frc.robot.GlobalVariables; -import frc.robot.subsystems.Swerve; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.CommandBase; - - -public class AutoLockToGamePiece extends CommandBase { - - private double rotation; - private Translation2d translation; - private boolean fieldRelative; - private boolean openLoop; - - private Swerve s_Swerve; - // private Joystick controller; - private int translationAxis; - private int strafeAxis; - - private int pipeline; - private double mapdouble(double x, double in_min, double in_max, double out_min, double out_max){ - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; - } - - /** - * Driver control - */ - public AutoLockToGamePiece(Swerve s_Swerve, int translationAxis, int strafeAxis, boolean fieldRelative, boolean openLoop, int pipeline) { - this.s_Swerve = s_Swerve; - addRequirements(s_Swerve); - - // this.controller = controller; - this.translationAxis = translationAxis; - this.strafeAxis = strafeAxis; - this.fieldRelative = fieldRelative; - this.openLoop = openLoop; - this.pipeline = pipeline; - } - - @Override - public void execute() { - double yAxis = 0.4; - double xAxis = 0; - double rAxis = s_Swerve.frontCamOffset(GlobalVariables.gamePiece); - - - /* Deadbands */ - - if (Math.abs(yAxis) > Constants.stickDeadband) { - if (yAxis > 0){ - yAxis = mapdouble(yAxis, Constants.stickDeadband, 1, 0, 1); - } else { - yAxis = mapdouble(yAxis, -Constants.stickDeadband, -1, 0, -1); - } - } - else{ - yAxis = 0; - } - - if (Math.abs(xAxis) > Constants.stickDeadband) { - if (xAxis > 0){ - xAxis = mapdouble(xAxis, Constants.stickDeadband, 1, 0, 1); - } else { - xAxis = mapdouble(xAxis, -Constants.stickDeadband, -1, 0, -1); - } - } - else{ - xAxis = 0; - } - - if (Math.abs(rAxis) > 0.01) { - if (rAxis > 0){ - rAxis = mapdouble(rAxis, 0.01, 1, 0, 1); - } else { - rAxis = mapdouble(rAxis, -0.01, -1, 0, -1); - } - } - else{ - rAxis = 0; - } - - translation = new Translation2d(yAxis, xAxis).times(Constants.Swerve.maxSpeed); - rotation = rAxis * Constants.Swerve.maxAngularVelocity; - s_Swerve.drive(translation, rotation, false, false); - } -} diff --git a/src/main/java/frc/robot/commands/AutoSwervePositions.java b/src/main/java/frc/robot/commands/AutoSwervePositions.java index c2ad2d9..6078b8a 100644 --- a/src/main/java/frc/robot/commands/AutoSwervePositions.java +++ b/src/main/java/frc/robot/commands/AutoSwervePositions.java @@ -19,7 +19,6 @@ import frc.robot.subsystems.Gripper; import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; public class AutoSwervePositions extends CommandBase { Swerve swerve; diff --git a/src/main/java/frc/robot/commands/GripperCommand.java b/src/main/java/frc/robot/commands/GripperCommand.java index 70a9d99..602c474 100644 --- a/src/main/java/frc/robot/commands/GripperCommand.java +++ b/src/main/java/frc/robot/commands/GripperCommand.java @@ -25,15 +25,16 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // gripper.moveGripper(percent); - gripper.moveGripperPos(20); + gripper.moveGripper(percent); + // gripper.moveGripperPos(20); + } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - // gripper.moveGripper(0); - gripper.moveGripperPos(0); + gripper.moveGripper(0); + // gripper.moveGripperPos(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/GripperStateCommand.java b/src/main/java/frc/robot/commands/GripperStateCommand.java deleted file mode 100644 index a4c1a78..0000000 --- a/src/main/java/frc/robot/commands/GripperStateCommand.java +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Gripper.GripperState; - -public class GripperStateCommand extends CommandBase { - Gripper gripper; - GripperState state; - int timer; - /** Creates a new GripperStateCommand. */ - public GripperStateCommand(Gripper gripper, GripperState state) { - this.gripper = gripper; - this.state = state; - addRequirements(gripper); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - timer = 0; - gripper.setClaw(state); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - timer ++; - System.out.println("GRIPPER COMMAND EXEC"); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - System.out.println("GRIPPER COMMAND END"); - - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return timer > 7; - } -} diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index 174f27e..c35a53c 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -8,7 +8,6 @@ import frc.robot.GlobalVariables.ArmPositions; import frc.robot.subsystems.ArmSub; import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Gripper.GripperState; public class GroundIntake extends CommandBase { @@ -34,7 +33,6 @@ public void execute() { timer++; armSub.armRotPresetPositions(ArmPositions.GROUND_PICKUP_ADAPTIVE); if (timer >= 20){ - gripper.setClaw(GripperState.OPEN); gripper.moveGripper(1); } @@ -44,14 +42,13 @@ public void execute() { @Override public void end(boolean interrupted) { - gripper.setClaw(GripperState.CONE); gripper.moveGripper(0); System.out.println("END"); } @Override public boolean isFinished() { - if(gripper.colorSensorDistance() > 150){ + if(gripper.getDistanceSensorDist() > 150){ return true; } diff --git a/src/main/java/frc/robot/commands/LockRotation.java b/src/main/java/frc/robot/commands/LockRotation.java index 3dfdc49..0e00c0a 100644 --- a/src/main/java/frc/robot/commands/LockRotation.java +++ b/src/main/java/frc/robot/commands/LockRotation.java @@ -1,32 +1,104 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot.commands; +import frc.robot.Constants; +import frc.robot.GlobalVariables; +import frc.robot.subsystems.Swerve; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; + public class LockRotation extends CommandBase { - /** Creates a new LockRotation. */ - public LockRotation() { - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } + + private double rotation; + private Translation2d translation; + private boolean fieldRelative; + private boolean openLoop; + + private Swerve s_Swerve; + private Joystick controller; + private int translationAxis; + private int strafeAxis; + private int rotationAxis; + private double goal; + PIDController pidController; + + + + private double mapdouble(double x, double in_min, double in_max, double out_min, double out_max){ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; + } + + /** + * Driver control + */ + public LockRotation(Swerve s_Swerve, Joystick controller, int translationAxis, int strafeAxis, int rotationAxis, boolean fieldRelative, boolean openLoop) { + this.s_Swerve = s_Swerve; + addRequirements(s_Swerve); + + this.controller = controller; + this.translationAxis = translationAxis; + this.strafeAxis = strafeAxis; + this.rotationAxis = rotationAxis; + this.fieldRelative = fieldRelative; + this.openLoop = openLoop; + pidController = new PIDController(0.003 , 0.00, 0); + pidController.setTolerance(1); + pidController.enableContinuousInput(-180, 180); + + + } + + @Override + public void execute() { + if(GlobalVariables.robotDirection){ + goal = 180; + } + else{ + goal = 0; + } + double yAxis = -controller.getRawAxis(translationAxis); + double xAxis = controller.getRawAxis(strafeAxis); + double rAxis = pidController.calculate(s_Swerve.getYaw().getDegrees(), goal); + + /* Deadbands */ + + if (Math.abs(yAxis) > Constants.stickDeadband) { + if (yAxis > 0){ + yAxis = mapdouble(yAxis, Constants.stickDeadband, 1, 0, 1); + } else { + yAxis = mapdouble(yAxis, -Constants.stickDeadband, -1, 0, -1); + } + } + else{ + yAxis = 0; + } + + if (Math.abs(xAxis) > Constants.stickDeadband) { + if (xAxis > 0){ + xAxis = mapdouble(xAxis, Constants.stickDeadband, 1, 0, 1); + } else { + xAxis = mapdouble(xAxis, -Constants.stickDeadband, -1, 0, -1); + } + } + else{ + xAxis = 0; + } + + // if (Math.abs(rAxis) > Constants.stickDeadband) { + // if (rAxis > 0){ + // rAxis = mapdouble(rAxis, Constants.stickDeadband, 1, 0, 1); + // } else { + // rAxis = mapdouble(rAxis, -Constants.stickDeadband, -1, 0, -1); + // } + // } + // else{ + // rAxis = 0; + // } + + translation = new Translation2d(yAxis, xAxis).times(Constants.Swerve.maxSpeed); + rotation = rAxis * Constants.Swerve.maxAngularVelocity; + s_Swerve.drive(translation, rotation, fieldRelative, openLoop); + } } diff --git a/src/main/java/frc/robot/commands/ShelfIntake.java b/src/main/java/frc/robot/commands/ShelfIntake.java deleted file mode 100644 index 82a0432..0000000 --- a/src/main/java/frc/robot/commands/ShelfIntake.java +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.GlobalVariables.ArmPositions; -import frc.robot.subsystems.ArmSub; -import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Gripper.GripperState; - -public class ShelfIntake extends CommandBase { - - /** Creates a new GroundIntake. */ - Gripper gripper; - - public ShelfIntake( Gripper gripper) { - this.gripper = gripper; - addRequirements( gripper); - - } - - @Override - public void initialize() { - gripper.setClaw(GripperState.OPEN); - } - - @Override - public void execute() { - gripper.moveGripper(0.8); - - } - - @Override - public void end(boolean interrupted) { - gripper.setClaw(GripperState.CONE); - gripper.moveGripper(0); - System.out.println("END"); - } - - @Override - public boolean isFinished() { - if(gripper.colorSensorDistance() > 200){ - return true; - - } - else{ - return false; - } - } -} diff --git a/src/main/java/frc/robot/commands/Test.java b/src/main/java/frc/robot/commands/Test.java index 6bfb59d..0b8007d 100644 --- a/src/main/java/frc/robot/commands/Test.java +++ b/src/main/java/frc/robot/commands/Test.java @@ -19,7 +19,6 @@ import frc.robot.subsystems.Gripper; import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; public class Test extends CommandBase { Swerve swerve; diff --git a/src/main/java/frc/robot/commands/TestRed.java b/src/main/java/frc/robot/commands/TestRed.java index abb991a..9496424 100644 --- a/src/main/java/frc/robot/commands/TestRed.java +++ b/src/main/java/frc/robot/commands/TestRed.java @@ -19,7 +19,6 @@ import frc.robot.subsystems.Gripper; import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; public class TestRed extends CommandBase { Swerve swerve; diff --git a/src/main/java/frc/robot/subsystems/ArmSub.java b/src/main/java/frc/robot/subsystems/ArmSub.java index 54ab95d..c1370fd 100644 --- a/src/main/java/frc/robot/subsystems/ArmSub.java +++ b/src/main/java/frc/robot/subsystems/ArmSub.java @@ -177,6 +177,11 @@ public double getTelescopePos(){ return telescopeArm.getSelectedSensorPosition(); } +public void homeRotArmPos(){ + // TODO THING + leftArm.setSelectedSensorPosition(testCanCoder.getPosition() /30); +} + /** * Moves the arm to a dedicated degree. * @param position Desired arm position in degrees @@ -398,6 +403,126 @@ public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ break; } } + + public void jointRotPresetPositions(GlobalVariables.ArmPositions position){ + switch(position){ + + case GROUND_PICKUP_ADAPTIVE: + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.JointRotationValues.JointRotConePickup); + } + else{ + extendArmPosition(Constants.JointRotationValues.JointRotCubePickup); + + } + break; + + case STOWED_ADAPTIVE: + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.JointRotationValues.JointRotStowCone); + } + else{ + extendArmPosition(Constants.JointRotationValues.JointRotStowCube); + + } + break; + + case GROUND_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.JointRotationValues.JointRotForLowCone); + } + else{ + extendArmPosition(Constants.JointRotationValues.JointRotForLowCube); + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.JointRotationValues.JointRotRevLowCone); + } + else{ + extendArmPosition(Constants.JointRotationValues.JointRotRevLowCube); + + } + } + + break; + + case SHELF_PICKUP_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.JointRotationValues.JointRotForShelfCone); + } + else{ + extendArmPosition(Constants.JointRotationValues.JointRotForShelfCube); + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.JointRotationValues.JointRotRevShelfCone); + } + else{ + extendArmPosition(Constants.JointRotationValues.JointRotRevShelfCube); + + } + } + break; + + case MID_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.JointRotationValues.JointRotForMidCone); + } + else{ + extendArmPosition(Constants.JointRotationValues.JointRotForMidCube); + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.JointRotationValues.JointRotRevMidCone); + } + else{ + extendArmPosition(Constants.JointRotationValues.JointRotRevMidCube); + + } + } + + break; + + case HIGH_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.JointRotationValues.JointRotForHighCone); + } + else{ + extendArmPosition(Constants.JointRotationValues.JointRotForHighCube); + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.JointRotationValues.JointRotRevHighCone); + } + else{ + extendArmPosition(Constants.JointRotationValues.JointRotRevHighCube); + + } + } + + break; + + + + default: + if(GlobalVariables.gamePiece == 0){ + extendArmPosition(Constants.JointRotationValues.JointRotStowCone); + } + else{ + extendArmPosition(Constants.JointRotationValues.JointRotStowCube); + + } + break; + } + } /** * Returns current arm position diff --git a/src/main/java/frc/robot/subsystems/Gripper.java b/src/main/java/frc/robot/subsystems/Gripper.java index 6acf84c..4893b24 100644 --- a/src/main/java/frc/robot/subsystems/Gripper.java +++ b/src/main/java/frc/robot/subsystems/Gripper.java @@ -4,128 +4,58 @@ package frc.robot.subsystems; -import com.revrobotics.CANSparkMax; -import com.revrobotics.ColorSensorV3; -import com.revrobotics.SparkMaxPIDController; -import com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj.Compressor; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.PneumaticsControlModule; -import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.Solenoid; -import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; + +import com.revrobotics.Rev2mDistanceSensor; + +import com.revrobotics.Rev2mDistanceSensor.Port; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Gripper extends SubsystemBase { - CANSparkMax gripperMotor; - DoubleSolenoid cubeSolenoid; - Solenoid coneSolenoid; - SparkMaxPIDController gripperPID; - - public PneumaticsControlModule module; - Compressor compressor; - public final I2C.Port i2cPort = I2C.Port.kMXP; + public TalonFX gripperFalcon; - public final ColorSensorV3 colorSensor = new ColorSensorV3(i2cPort); -; - public Color detectedColor; + - public enum GripperState{ - OPEN, CONE, CUBE - } - public GripperState gripperState = GripperState.OPEN; + private Rev2mDistanceSensor distanceSensor; +; + /** Creates a new Gripper. */ public Gripper() { - gripperMotor = new CANSparkMax(42, MotorType.kBrushless); - gripperMotor.restoreFactoryDefaults(); - gripperMotor.setIdleMode(IdleMode.kBrake); - gripperMotor.getEncoder().setPosition(0); - - gripperPID = gripperMotor.getPIDController(); - gripperPID.setP(1); - gripperPID.setI(0); - gripperPID.setD(0); - gripperPID.setFeedbackDevice(gripperMotor.getEncoder()); - compressor = new Compressor(PneumaticsModuleType.REVPH); - compressor.enableDigital(); - - cubeSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, 15, 14); - // coneSolenoid = new DoubleS/olenoid(PneumaticsModuleType.REVPH, 1, 3); - coneSolenoid = new Solenoid(PneumaticsModuleType.REVPH, 1); - - module = new PneumaticsControlModule(1); - module.clearAllStickyFaults(); - - detectedColor = colorSensor.getColor(); + gripperFalcon = new TalonFX(35, "Karen"); + gripperFalcon.configFactoryDefault(); + gripperFalcon.setNeutralMode(NeutralMode.Brake); - } - public void getColorSensor(){ - detectedColor = colorSensor.getColor(); - } - public double getRed(){ - getColorSensor(); - return detectedColor.red; - } + distanceSensor = new Rev2mDistanceSensor(Port.kMXP); + distanceSensor.setAutomaticMode(true); - public double getBlue(){ - getColorSensor(); - return detectedColor.blue; } - public double getGreen(){ - getColorSensor(); - return detectedColor.green; + public double getDistanceSensorDist(){ + return distanceSensor.getRange(); } - public double colorSensorDistance(){ - return colorSensor.getProximity(); - } + public void moveGripper(double percent ){ - gripperMotor.set(percent); + gripperFalcon.set(TalonFXControlMode.PercentOutput, percent); } - public void moveGripperPos(double position){ - gripperPID.setReference(position, ControlType.kPosition); - } - public void setClaw(GripperState state){ - switch(state){ - case OPEN: - cubeSolenoid.set(Value.kReverse); - // coneSolenoid.set(false); - break; - - case CONE: - cubeSolenoid.set(Value.kForward); - // coneSolenoid.set(false); - break; - - case CUBE: - // cubeSolenoid.set(Value.kOff); - // coneSolenoid.set(true); - break; - } -} + @Override public void periodic() { - SmartDashboard.putNumber("RED", colorSensor.getRed()); - SmartDashboard.putNumber("BLUE", colorSensor.getBlue()); - SmartDashboard.putNumber("GREEN", colorSensor.getGreen()); - SmartDashboard.putNumber("Distance", colorSensor.getProximity()); - SmartDashboard.putNumber("SPARKMAX POS", gripperMotor.getEncoder().getPosition()); + SmartDashboard.putNumber("Distance Sensor", getDistanceSensorDist()); + } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index dcc4491..e009cb3 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -36,6 +36,7 @@ public class Swerve extends SubsystemBase { public PhotonCameraWrapper pcw; public AHRS gyro; public ProfiledPIDController rotatePID; + public PIDController aimPID; private SwerveModule m_frontLeft = new SwerveModule(0, Constants.Swerve.Mod0.constants); private SwerveModule m_frontRight = new SwerveModule(1, Constants.Swerve.Mod1.constants); @@ -79,6 +80,8 @@ public Swerve() { // zeroGyro(); SmartDashboard.putData("Field", m_fieldSim); + aimPID = new PIDController(1, 0, 0); + aimPID.setTolerance(0.001); rotatePID = new ProfiledPIDController( 0.010, @@ -96,7 +99,7 @@ public Swerve() { swerveOdometry = new SwerveDriveOdometry(Constants.Swerve.swerveKinematics, getYaw(), mSwerveModulePositions); - + m_poseEstimator = new SwerveDrivePoseEstimator( Constants.Swerve.swerveKinematics, getYaw(), new SwerveModulePosition[] { m_frontRight.getPosition(), m_frontLeft.getPosition(), @@ -273,46 +276,61 @@ public double getPitch(){ return gyro.getRoll(); } + public double getCamOffset(){ + pcw.cameraPipelines(GlobalVariables.gamePiece); + double headingError; + if(GlobalVariables.robotDirection){ + headingError = pcw.getFrontCamOffset(); + return aimPID.calculate(headingError, 30); + } + else{ + headingError = pcw.getRearCamOffset(); + return aimPID.calculate(headingError, -30); - - public double frontCamOffset(int pipeline){ - pcw.frontCamPipeline(GlobalVariables.gamePiece); - - double headingError = pcw.getYOffset(); - /* double steering_adjust; - steering_adjust = 0; - if (headingError > 0.05) - { - steering_adjust = 0.09; } - else if (headingError < -0.05) - { - steering_adjust = -0.09; - } */ - return (rotatePID.calculate(headingError, 0)/* +steering_adjust */); - } + } - - public double AutoFrontCamOffset(int pipeline){ - pcw.frontCamPipeline(GlobalVariables.gamePiece); - double headingError = pcw.AutoGetYOffset(); - /* double steering_adjust; - steering_adjust = 0; - if (headingError > 0.05) - { - steering_adjust = 0.09; - } - else if (headingError < -0.05) - { - steering_adjust = -0.09; - } */ - + // public double frontCamOffset(int pipeline){ + // pcw.frontCamPipeline(GlobalVariables.gamePiece); + + // double headingError = pcw.getYOffset(); + // /* double steering_adjust; + // steering_adjust = 0; + // if (headingError > 0.05) + // { + // steering_adjust = 0.09; + // } + // else if (headingError < -0.05) + // { + // steering_adjust = -0.09; + // } */ + + + // return (rotatePID.calculate(headingError, 0)/* +steering_adjust */); + // } - return (rotatePID.calculate(headingError, 0)/* +steering_adjust */); - } + + // public double AutoFrontCamOffset(int pipeline){ + // pcw.frontCamPipeline(GlobalVariables.gamePiece); + + // double headingError = pcw.AutoGetYOffset(); + // /* double steering_adjust; + // steering_adjust = 0; + // if (headingError > 0.05) + // { + // steering_adjust = 0.09; + // } + // else if (headingError < -0.05) + // { + // steering_adjust = -0.09; + // } */ + + + // return (rotatePID.calculate(headingError, 0)/* +steering_adjust */); + // } @@ -343,7 +361,7 @@ public void periodic(){ SmartDashboard.putNumber("Odometry X", getPose().getX()); SmartDashboard.putNumber("Odometry Y", getPose().getY()); - SmartDashboard.putNumber("CONE OFFSET ", frontCamOffset(1)); + // SmartDashboard.putNumber("CONE OFFSET ", frontCamOffset(1)); From 6eb9928cef8d14b113d051f1d1afd8a9739b2cdc Mon Sep 17 00:00:00 2001 From: Oscar Date: Thu, 23 Mar 2023 20:50:11 -0700 Subject: [PATCH 06/12] Gripper Working --- src/main/java/frc/robot/RobotContainer.java | 1 + .../frc/robot/commands/MoveJointTemp.java | 41 +++++++++++++++++++ .../java/frc/robot/subsystems/ArmSub.java | 21 ++++++++-- 3 files changed, 60 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/commands/MoveJointTemp.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8840b5b..d1bf0af 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -112,6 +112,7 @@ public RobotContainer() { withInterruptBehavior(InterruptionBehavior.kCancelSelf)); // armSub.setDefaultCommand(new HoldArmPos(armSub, ArmPositions.STOWED_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); //candle.setDefaultCommand(new InstantCommand(() -> candle.setColor(operatorExtraRight.getAsBoolean()))); + armSub.setDefaultCommand(new MoveJointTemp(armSub, operatorPanel)); candle.setDefaultCommand(new SetLedCommand(candle)); configureButtonBindings(); } diff --git a/src/main/java/frc/robot/commands/MoveJointTemp.java b/src/main/java/frc/robot/commands/MoveJointTemp.java new file mode 100644 index 0000000..a643a0a --- /dev/null +++ b/src/main/java/frc/robot/commands/MoveJointTemp.java @@ -0,0 +1,41 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ArmSub; + +public class MoveJointTemp extends CommandBase { + ArmSub armsub; + Joystick controller; + /** Creates a new MoveJointTemp. */ + public MoveJointTemp(ArmSub armSub, Joystick controller) { + this.armsub = armSub; + this.controller = controller; + addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + armsub.manualJointControl(controller.getRawAxis(0)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/ArmSub.java b/src/main/java/frc/robot/subsystems/ArmSub.java index c1370fd..b72949d 100644 --- a/src/main/java/frc/robot/subsystems/ArmSub.java +++ b/src/main/java/frc/robot/subsystems/ArmSub.java @@ -14,6 +14,7 @@ import com.ctre.phoenix.sensors.AbsoluteSensorRange; import com.ctre.phoenix.sensors.CANCoder; import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxAbsoluteEncoder; import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANSparkMax.ControlType; @@ -33,10 +34,10 @@ public class ArmSub extends SubsystemBase { public TalonFX telescopeArm; - public CANSparkMax gripperJointEncoder; public CANSparkMax gripperJointNeo; public SparkMaxPIDController gripperJointPID; - public SparkMaxAbsoluteEncoder encoder; + // public SparkMaxAbsoluteEncoder encoder; + public RelativeEncoder jointEncoder; public PIDController armPID; public PIDController jointPID; public PIDController telescopePID; @@ -63,11 +64,14 @@ public ArmSub() { // gripperJointEncoder = new CANSparkMax(55, MotorType.kBrushless); // encoder = gripperJointEncoder.getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type.kDutyCycle); - gripperJointNeo = new CANSparkMax(56, MotorType.kBrushless); + gripperJointNeo = new CANSparkMax(45, MotorType.kBrushless); gripperJointNeo.clearFaults(); gripperJointNeo.restoreFactoryDefaults(); gripperJointNeo.getEncoder().setPosition(0); gripperJointNeo.setIdleMode(IdleMode.kBrake); + gripperJointNeo.setSmartCurrentLimit(30); + + jointEncoder = gripperJointNeo.getEncoder(); gripperJointPID = gripperJointNeo.getPIDController(); gripperJointPID.setP(1); @@ -173,6 +177,11 @@ public void gripperJointPosition(double position){ gripperJointPID.setReference(position, ControlType.kPosition); } +public void manualJointControl(double percent ){ + gripperJointNeo.set(percent); + +} + public double getTelescopePos(){ return telescopeArm.getSelectedSensorPosition(); } @@ -553,6 +562,10 @@ public double getArmMotorPos(){ return test; } + public double getJointPos(){ + return jointEncoder.getPosition(); + } + @Override public void periodic() { @@ -564,5 +577,7 @@ public void periodic() { SmartDashboard.putNumber("Arm Motor 2 Current", rightArm.getStatorCurrent()); SmartDashboard.putNumber("Telescope Position", telescopeArm.getSelectedSensorPosition()); SmartDashboard.putNumber("Telescope Speed?", telescopeArm.getSupplyCurrent()); + SmartDashboard.putNumber("Joint Position ", getJointPos() ); + SmartDashboard.putNumber("Joint Motor Current ", gripperJointNeo.getOutputCurrent()); } } From 4318f7a976a446d09da30d4375420453187634a3 Mon Sep 17 00:00:00 2001 From: Oscar Date: Tue, 28 Mar 2023 20:11:34 -0700 Subject: [PATCH 07/12] Arm Testing --- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/GlobalVariables.java | 3 +- src/main/java/frc/robot/Robot.java | 22 +++- src/main/java/frc/robot/RobotContainer.java | 14 ++- .../robot/commands/ArmExtendPositionTest.java | 44 +++++++ .../frc/robot/commands/ArmExtendTest.java | 14 ++- .../frc/robot/commands/ArmRotationTest.java | 40 +++++++ .../java/frc/robot/subsystems/ArmSub.java | 110 ++++++++++++++++-- .../java/frc/robot/subsystems/CANdleSub.java | 2 +- 9 files changed, 233 insertions(+), 20 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ArmExtendPositionTest.java create mode 100644 src/main/java/frc/robot/commands/ArmRotationTest.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dc4d137..9693dff 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -161,7 +161,7 @@ public static final class Swerve { public static final double wheelDiameter = Units.inchesToMeters(3.94); public static final double wheelCircumference = wheelDiameter * Math.PI; - public static final double openLoopRamp = 0.2; + public static final double openLoopRamp = 0.0; public static final double closedLoopRamp = 0.61 ; public static final double driveGearRatio = (6.12 / 1.0); //8:14:1 @@ -202,7 +202,7 @@ public static final class Swerve { public static final double driveKA = (0.27 / 12); /* Swerve Profiling Values */ - public static final double maxSpeed = 4.5; //meters per second + public static final double maxSpeed = 10; //meters per second public static final double maxAngularVelocity = 11.5; /* Neutral Modes */ diff --git a/src/main/java/frc/robot/GlobalVariables.java b/src/main/java/frc/robot/GlobalVariables.java index d57e4d7..2eb6107 100644 --- a/src/main/java/frc/robot/GlobalVariables.java +++ b/src/main/java/frc/robot/GlobalVariables.java @@ -13,7 +13,8 @@ public class GlobalVariables { public static int height; public static boolean robotDirection; public static double gripperConePlacement; - + public static double armRotGoal; + public static double armExtendGoal; public static enum StationSelect{ CLOSE_RAMP, LEFT_SHELF, LEFT_RAMP, RIGHT_SHELF, RIGHT_RAMP } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index efc2e56..f5f9820 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,12 +6,17 @@ +import com.ctre.phoenix.motorcontrol.NeutralMode; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -33,6 +38,10 @@ public class Robot extends TimedRobot { private CANdleSub candlesub; + private PowerDistribution pdh; + DigitalInput input = new DigitalInput(0); + + // PhotonCamera camera = new PhotonCamera("photonvision"); @@ -44,6 +53,7 @@ public class Robot extends TimedRobot { public void robotInit() { ctreConfigs = new CTREConfigs(); candlesub = new CANdleSub(); + pdh = new PowerDistribution(1, ModuleType.kRev); // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); @@ -68,7 +78,15 @@ public void disabledInit() {} @Override public void disabledPeriodic() { + if(input.get()){ + m_robotContainer.armSub.ArmBrakeMode(NeutralMode.Coast); + } + else{ + m_robotContainer.armSub.ArmBrakeMode(NeutralMode.Brake); + + } candlesub.disabledLed(); + pdh.setSwitchableChannel(false); } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @@ -81,6 +99,8 @@ public void autonomousInit() { if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } + m_robotContainer.armSub.ArmBrakeMode(NeutralMode.Brake); + } /** This function is called periodically during autonomous. */ @@ -111,7 +131,7 @@ public void teleopPeriodic() { Alliance alliance; alliance = DriverStation.getAlliance(); m_robotContainer.s_Swerve.updateOdometry(); - +pdh.setSwitchableChannel(true); // System.out.println(alliance); // candlesub.enabledLed(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d1bf0af..908a33c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -70,6 +70,9 @@ public class RobotContainer { private final Trigger panelRollers = new JoystickButton(operatorPanel, 10); private final Trigger panelShelf = new JoystickButton(operatorPanel, 9); + private final Trigger panelHigh = new JoystickButton(operatorPanel, 13); + private final Trigger panelMid = new JoystickButton(operatorPanel, 11); + private final Trigger panelLow = new JoystickButton(operatorPanel, 3); @@ -112,7 +115,8 @@ public RobotContainer() { withInterruptBehavior(InterruptionBehavior.kCancelSelf)); // armSub.setDefaultCommand(new HoldArmPos(armSub, ArmPositions.STOWED_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); //candle.setDefaultCommand(new InstantCommand(() -> candle.setColor(operatorExtraRight.getAsBoolean()))); - armSub.setDefaultCommand(new MoveJointTemp(armSub, operatorPanel)); + // armSub.setDefaultCommand(new MoveJointTemp(armSub, operatorPanel)); + armSub.setDefaultCommand(new ArmExtendTest(armSub, operatorPanel)); candle.setDefaultCommand(new SetLedCommand(candle)); configureButtonBindings(); } @@ -126,6 +130,12 @@ public RobotContainer() { private void configureButtonBindings() { panelRollers.whileTrue(new GripperCommand(gripper, 0.5)); //Push OUT panelShelf.whileTrue(new GripperCommand(gripper, -0.5)); // Take IN + panelHigh.onTrue(new InstantCommand(() -> armSub.homeTelescopePosition())); + panelMid.whileTrue(new ArmExtendPositionTest(armSub, 35000)); + panelMid.whileFalse(new ArmExtendPositionTest(armSub, 0)); + + panelLow.whileTrue(new ArmRotationTest(armSub, 220)); + panelLow.whileFalse(new ArmRotationTest(armSub, 10)); // operatorExtraRight.whileTrue(new ConeVCube(0)); // operatorExtraRight.whileFalse(new ConeVCube(1)); @@ -139,7 +149,7 @@ private void configureButtonBindings() { // operatorLeftEmpty.toggleOnTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); // operatorHook.whileTrue(new ArmExtendTest(armSub)); - // rightJoy.onTrue(new InstantCommand(() -> s_Swerve.zeroGyro())); + rightJoy.onTrue(new InstantCommand(() -> s_Swerve.zeroGyro())); // leftJoy.whileTrue(new LockToGamePiece(s_Swerve, joystickPanel, translationAxis, strafeAxis, true, true, GlobalVariables.gamePiece).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); // buttonX.toggleOnTrue(new HoldArmPos(armSub, ArmPositions.STOWED).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); diff --git a/src/main/java/frc/robot/commands/ArmExtendPositionTest.java b/src/main/java/frc/robot/commands/ArmExtendPositionTest.java new file mode 100644 index 0000000..3c4b42d --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmExtendPositionTest.java @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ArmSub; + +public class ArmExtendPositionTest extends CommandBase { + ArmSub armSub; + double position; + /** Creates a new ArmExtendPositionTest. */ + public ArmExtendPositionTest(ArmSub armSub, double position) { + this.armSub = armSub; + this.position = position; + // addRequirements(armSub); + + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + armSub.extendArmPosition(position); + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + armSub.extendArmPosition(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ArmExtendTest.java b/src/main/java/frc/robot/commands/ArmExtendTest.java index 30dd2d8..d81a86b 100644 --- a/src/main/java/frc/robot/commands/ArmExtendTest.java +++ b/src/main/java/frc/robot/commands/ArmExtendTest.java @@ -4,14 +4,18 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ArmSub; public class ArmExtendTest extends CommandBase { ArmSub armsub; + Joystick controller; /** Creates a new ArmExtendTest. */ - public ArmExtendTest(ArmSub armsub) { + public ArmExtendTest(ArmSub armsub, Joystick controller) { this.armsub = armsub; + this.controller = controller; + addRequirements(armsub); // Use addRequirements() here to declare subsystem dependencies. } @@ -23,13 +27,19 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - armsub.extendArmPercentOutput(0.05); + if(controller.getRawButton(17)){ + armsub.moveArmPercentOutput(controller.getRawAxis(0)); + } + else{ + armsub.extendArmPercentOutput(controller.getRawAxis(0)); + } // armsub.extendArmPosition(1000); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + armsub.moveArmPercentOutput(0); armsub.extendArmPercentOutput(0); // armsub.extendArmPosition(0); diff --git a/src/main/java/frc/robot/commands/ArmRotationTest.java b/src/main/java/frc/robot/commands/ArmRotationTest.java new file mode 100644 index 0000000..1b63886 --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmRotationTest.java @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ArmSub; + +public class ArmRotationTest extends CommandBase { + ArmSub armSub; + double position; + /** Creates a new ArmRotationTest. */ + public ArmRotationTest(ArmSub armSub, double position) { + this.position = position; + this.armSub = armSub; + // addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + armSub.moveArmPosition(position); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/ArmSub.java b/src/main/java/frc/robot/subsystems/ArmSub.java index b72949d..0bbb7ed 100644 --- a/src/main/java/frc/robot/subsystems/ArmSub.java +++ b/src/main/java/frc/robot/subsystems/ArmSub.java @@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; @@ -43,6 +44,8 @@ public class ArmSub extends SubsystemBase { public PIDController telescopePID; public CANCoder testCanCoder; + private SupplyCurrentLimitConfiguration limit; + public double rotMaxSpeedFor; public double rotMaxSpeedRev; @@ -59,8 +62,10 @@ public ArmSub() { testCanCoder.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360); - rotMaxSpeedFor = 0.4; - rotMaxSpeedRev = 0.65; + rotMaxSpeedFor = 0.7; + rotMaxSpeedRev = 1; + + limit = new SupplyCurrentLimitConfiguration(true, 20, 20, 0); // gripperJointEncoder = new CANSparkMax(55, MotorType.kBrushless); // encoder = gripperJointEncoder.getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type.kDutyCycle); @@ -68,7 +73,7 @@ public ArmSub() { gripperJointNeo.clearFaults(); gripperJointNeo.restoreFactoryDefaults(); gripperJointNeo.getEncoder().setPosition(0); - gripperJointNeo.setIdleMode(IdleMode.kBrake); + gripperJointNeo.setIdleMode(IdleMode.kBrake); gripperJointNeo.setSmartCurrentLimit(30); jointEncoder = gripperJointNeo.getEncoder(); @@ -81,8 +86,9 @@ public ArmSub() { // pidLoop = new PIDController(0.006, 0.003, 0.001); - armPID = new PIDController(0.014, 0.002, 0.00); + armPID = new PIDController(0.015, 0.00, 0.00); armPID.setTolerance(0.5); + armPID.enableContinuousInput(0, 360); // pidLoop. leftArm = new WPI_TalonFX(20, "Karen"); @@ -116,33 +122,43 @@ public ArmSub() { rightArm.setNeutralMode(NeutralMode.Brake); rightArm.follow(leftArm); - telMaxSpeedFor = 0.5; - telMaxSpeedRev = 0.5; + telMaxSpeedFor = 1; + telMaxSpeedRev = 1; telescopeArm.configFactoryDefault(); telescopeArm.configPeakOutputForward(telMaxSpeedFor); telescopeArm.configPeakOutputReverse(-telMaxSpeedRev); telescopeArm.setSensorPhase(true); telescopeArm.setInverted(InvertType.None); - telescopeArm.config_kP(0, 0.4); + telescopeArm.config_kP(0, 0.5); telescopeArm.config_kI(0, 0.0); telescopeArm.config_kD(0, 0.0); telescopeArm.config_kF(0, 0.0); telescopeArm.setNeutralMode(NeutralMode.Brake); - telescopeArm.configNeutralDeadband(0.00001); - telescopeArm.configMotionAcceleration(6000); + telescopeArm.configNeutralDeadband(0.1); + telescopeArm.configMotionAcceleration(100000); telescopeArm.configMotionCruiseVelocity(10000); telescopeArm.setSelectedSensorPosition(0); + telescopeArm.configSupplyCurrentLimit(limit); + } + + public void ArmBrakeMode(NeutralMode mode){ + telescopeArm.setNeutralMode(mode); + rightArm.setNeutralMode(mode); + leftArm.setNeutralMode(mode); + } public void moveRotArmMotionMagic(double degrees){ // TODO degrees to falcon readout - double setpoint = degrees *3; - leftArm.set(TalonFXControlMode.MotionMagic, setpoint); + // double setpoint = degrees *3; + // leftArm.set(TalonFXControlMode.MotionMagic, setpoint); + leftArm.set(ControlMode.PercentOutput ,armPID.calculate(testCanCoder.getAbsolutePosition(), degrees)); + } /** * Moves the arm by percent output. @@ -185,6 +201,9 @@ public void manualJointControl(double percent ){ public double getTelescopePos(){ return telescopeArm.getSelectedSensorPosition(); } +public void homeTelescopePosition(){ + telescopeArm.setSelectedSensorPosition(0); +} public void homeRotArmPos(){ // TODO THING @@ -208,31 +227,44 @@ public void armRotPresetPositions(GlobalVariables.ArmPositions position){ case GROUND_PICKUP_ADAPTIVE: if(GlobalVariables.gamePiece == 0){ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForConePickup); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForConePickup; } else{ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForCubePickup); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForCubePickup; + } break; case STOWED_ADAPTIVE: moveRotArmMotionMagic(Constants.ArmRotationValues.armRotStow); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotStow; + break; case GROUND_SCORE_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForLowCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForLowCone; + } else{ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForLowCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForLowCube; + } } else{ if(GlobalVariables.gamePiece == 0){ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevLowCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevLowCone; + } else{ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevLowCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevLowCube; + } } break; @@ -242,17 +274,25 @@ public void armRotPresetPositions(GlobalVariables.ArmPositions position){ if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForShelfCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForShelfCone; + } else{ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForShelfCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForShelfCube; + } } else{ if(GlobalVariables.gamePiece == 0){ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevShelfCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevShelfCone; + } else{ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevShelfCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevShelfCube; + } } break; @@ -261,17 +301,25 @@ public void armRotPresetPositions(GlobalVariables.ArmPositions position){ if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForMidCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForMidCone; + } else{ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForMidCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForMidCube; + } } else{ if(GlobalVariables.gamePiece == 0){ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevMidCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevMidCone; + } else{ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevMidCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevMidCube; + } } break; @@ -280,17 +328,25 @@ public void armRotPresetPositions(GlobalVariables.ArmPositions position){ if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForHighCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForHighCone; + } else{ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForHighCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForHighCube; + } } else{ if(GlobalVariables.gamePiece == 0){ moveRotArmMotionMagic( Constants.ArmRotationValues.armRotRevHighCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevHighCone; + } else{ moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevHighCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevHighCube; + } } break; @@ -299,6 +355,8 @@ public void armRotPresetPositions(GlobalVariables.ArmPositions position){ default: moveRotArmMotionMagic(Constants.ArmRotationValues.armRotStow); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotStow; + break; } } @@ -309,15 +367,19 @@ public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ case GROUND_PICKUP_ADAPTIVE: if(GlobalVariables.gamePiece == 0){ extendArmPosition(Constants.ArmExtendValues.armExtendConePickup); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendConePickup; } else{ extendArmPosition(Constants.ArmExtendValues.armExtendCubePickup); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendCubePickup; + } break; case STOWED_ADAPTIVE: extendArmPosition(Constants.ArmExtendValues.armExtendStow); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendStow; break; @@ -325,17 +387,25 @@ public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ extendArmPosition(Constants.ArmExtendValues.armExtendForLowCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForLowCone; + } else{ extendArmPosition(Constants.ArmExtendValues.armExtendForLowCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForLowCube; + } } else{ if(GlobalVariables.gamePiece == 0){ extendArmPosition(Constants.ArmExtendValues.armExtendRevLowCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevLowCone; + } else{ extendArmPosition(Constants.ArmExtendValues.armExtendRevLowCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevLowCube; + } } @@ -346,17 +416,25 @@ public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ extendArmPosition(Constants.ArmExtendValues.armExtendForShelfCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForShelfCone; + } else{ extendArmPosition(Constants.ArmExtendValues.armExtendForShelfCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForShelfCube; + } } else{ if(GlobalVariables.gamePiece == 0){ extendArmPosition(Constants.ArmExtendValues.armExtendRevShelfCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevShelfCone; + } else{ extendArmPosition(Constants.ArmExtendValues.armExtendRevShelfCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevShelfCube; + } } @@ -366,17 +444,25 @@ public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ extendArmPosition(Constants.ArmExtendValues.armExtendForMidCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForMidCone; + } else{ extendArmPosition(Constants.ArmExtendValues.armExtendForMidCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForMidCube; + } } else{ if(GlobalVariables.gamePiece == 0){ extendArmPosition(Constants.ArmExtendValues.armExtendRevMidCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevMidCone; + } else{ extendArmPosition(Constants.ArmExtendValues.armExtendRevMidCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevMidCube; + } } @@ -408,6 +494,8 @@ public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ default: extendArmPosition(Constants.ArmExtendValues.armExtendStow); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendStow; + break; } diff --git a/src/main/java/frc/robot/subsystems/CANdleSub.java b/src/main/java/frc/robot/subsystems/CANdleSub.java index 001594f..175b7ac 100644 --- a/src/main/java/frc/robot/subsystems/CANdleSub.java +++ b/src/main/java/frc/robot/subsystems/CANdleSub.java @@ -14,7 +14,7 @@ public class CANdleSub extends SubsystemBase { private final CANdle m_candle = new CANdle(Constants.CANdle.id, "rio"); - private final int numLed = 48 + 32 + 32 + 48; + private final int numLed = /* 48 + 32 + 32 + 48 */200; private final int ledOffset = 8; public CANdleSub() { From 0e45fd432160fe2d335feaee96624251e54250b5 Mon Sep 17 00:00:00 2001 From: Oscar Date: Thu, 30 Mar 2023 20:22:40 -0700 Subject: [PATCH 08/12] Switched TO gripper facon --- src/main/java/frc/robot/Robot.java | 11 ++++ src/main/java/frc/robot/RobotContainer.java | 52 +++++++++++++--- .../frc/robot/commands/FullArmPosition.java | 57 +++++++++++++++++ .../java/frc/robot/commands/GroundIntake.java | 40 ++++++------ .../java/frc/robot/commands/HoldJointPos.java | 40 ++++++++++++ .../java/frc/robot/subsystems/ArmSub.java | 61 ++++++++++++++++--- .../java/frc/robot/subsystems/CANdleSub.java | 2 +- .../java/frc/robot/subsystems/Swerve.java | 2 + 8 files changed, 223 insertions(+), 42 deletions(-) create mode 100644 src/main/java/frc/robot/commands/FullArmPosition.java create mode 100644 src/main/java/frc/robot/commands/HoldJointPos.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f5f9820..2601180 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -41,6 +42,8 @@ public class Robot extends TimedRobot { private PowerDistribution pdh; DigitalInput input = new DigitalInput(0); + private Joystick panel = new Joystick(2); + // PhotonCamera camera = new PhotonCamera("photonvision"); @@ -85,6 +88,8 @@ public void disabledPeriodic() { m_robotContainer.armSub.ArmBrakeMode(NeutralMode.Brake); } + + candlesub.disabledLed(); pdh.setSwitchableChannel(false); } @@ -128,6 +133,12 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() { + if(panel.getRawButton(17)){ + GlobalVariables.gamePiece = 0; + } + else{ + GlobalVariables.gamePiece = 1; + } Alliance alliance; alliance = DriverStation.getAlliance(); m_robotContainer.s_Swerve.updateOdometry(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 908a33c..6340bd1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -73,6 +73,10 @@ public class RobotContainer { private final Trigger panelHigh = new JoystickButton(operatorPanel, 13); private final Trigger panelMid = new JoystickButton(operatorPanel, 11); private final Trigger panelLow = new JoystickButton(operatorPanel, 3); + private final Trigger panelStow = new JoystickButton(operatorPanel, 5); + private final Trigger panelGround = new JoystickButton(operatorPanel, 16); + private final Trigger panelGPSwitch = new JoystickButton(joystickPanel, 17); + @@ -115,10 +119,11 @@ public RobotContainer() { withInterruptBehavior(InterruptionBehavior.kCancelSelf)); // armSub.setDefaultCommand(new HoldArmPos(armSub, ArmPositions.STOWED_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); //candle.setDefaultCommand(new InstantCommand(() -> candle.setColor(operatorExtraRight.getAsBoolean()))); - // armSub.setDefaultCommand(new MoveJointTemp(armSub, operatorPanel)); - armSub.setDefaultCommand(new ArmExtendTest(armSub, operatorPanel)); + armSub.setDefaultCommand(new MoveJointTemp(armSub, operatorPanel)); + // armSub.setDefaultCommand(new ArmExtendTest(armSub, operatorPanel)); candle.setDefaultCommand(new SetLedCommand(candle)); configureButtonBindings(); + } /** @@ -128,17 +133,44 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - panelRollers.whileTrue(new GripperCommand(gripper, 0.5)); //Push OUT - panelShelf.whileTrue(new GripperCommand(gripper, -0.5)); // Take IN - panelHigh.onTrue(new InstantCommand(() -> armSub.homeTelescopePosition())); - panelMid.whileTrue(new ArmExtendPositionTest(armSub, 35000)); + panelRollers.whileTrue(new GripperCommand(gripper, 0.7)); //Push OUT + panelShelf.whileTrue(new GripperCommand(gripper, -1)); // Take IN + panelHigh.onTrue(new InstantCommand(() -> armSub.homeJointPos())); + + // panelStow.whileTrue(new FullArmPosition(armSub, 150, 50000, true)); + // panelStow.whileFalse(new FullArmPosition(armSub, 0, 0, false)); + + panelMid.whileTrue(new ArmExtendPositionTest(armSub, 50000)); panelMid.whileFalse(new ArmExtendPositionTest(armSub, 0)); - panelLow.whileTrue(new ArmRotationTest(armSub, 220)); - panelLow.whileFalse(new ArmRotationTest(armSub, 10)); + panelLow.whileTrue(new ArmRotationTest(armSub, 130)); + panelLow.whileFalse(new ArmRotationTest(armSub, 0)); + + // panelLow.onTrue(new InstantCommand(() -> armSub.homeArmRotPos())); + + // panelStow.whileTrue(new HoldJointPos(armSub, 19).alongWith(new ArmExtendPositionTest(armSub, 5000)).alongWith(new ArmRotationTest(armSub, 0))); + // panelStow.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5))); + + + + // panelGround.whileTrue(new HoldJointPos(armSub, 19).alongWith(new ArmExtendPositionTest(armSub, 7000)).alongWith(new ArmRotationTest(armSub, 1))); + // panelGround.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + + + // panelStow.whileTrue(new HoldJointPos(armSub, 15).alongWith(new ArmExtendPositionTest(armSub, 7000)).alongWith(new ArmRotationTest(armSub, 0))); + // panelStow.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + + // panelGround.whileTrue(new GroundIntake(armSub, gripper)); + // panelGround.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + + // panelStow.whileTrue(new HoldJointPos(armSub, 22)); + // panelStow.whileFalse(new HoldJointPos(armSub, 0)); + // panelStow.whileTrue(new HoldJointPos(armSub, 30).alongWith(new ArmExtendPositionTest(armSub, 5000)).alongWith(new ArmRotationTest(armSub, 37))); + // panelStow.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5))); + - // operatorExtraRight.whileTrue(new ConeVCube(0)); - // operatorExtraRight.whileFalse(new ConeVCube(1)); + // panelGPSwitch.whileTrue(new ConeVCube(0)); + // panelGPSwitch.whileFalse(new ConeVCube(1)); // operatorIntakeUp.whileTrue(new HoldArmPos(armSub, ArmPositions.STOWED).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); // operatorIntakeUp.whileFalse(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); diff --git a/src/main/java/frc/robot/commands/FullArmPosition.java b/src/main/java/frc/robot/commands/FullArmPosition.java new file mode 100644 index 0000000..b97443a --- /dev/null +++ b/src/main/java/frc/robot/commands/FullArmPosition.java @@ -0,0 +1,57 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ArmSub; + +public class FullArmPosition extends CommandBase { + ArmSub armSub; + double armRotPos; + double armExtendPos; + boolean direction; + + + + /** Creates a new FullArmPosition. */ + public FullArmPosition(ArmSub armSub, double armRotPos, double armExtendPos, boolean direction) { + this.armSub = armSub; + this.armRotPos = armRotPos; + this.armExtendPos = armExtendPos; + this.direction = direction; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(direction){ + armSub.moveRotArmMotionMagic(armRotPos); + if(armSub.getArmPosition() >= armRotPos*0.75){ + armSub.extendArmPosition(armExtendPos); + } + } + else{ + armSub.extendArmPosition(armExtendPos); + if(armSub.getTelescopePos() <= 30000){ + armSub.moveRotArmMotionMagic(armRotPos); + } + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index c35a53c..0e60280 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -5,55 +5,53 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.GlobalVariables.ArmPositions; +import frc.robot.GlobalVariables; import frc.robot.subsystems.ArmSub; import frc.robot.subsystems.Gripper; public class GroundIntake extends CommandBase { - /** Creates a new GroundIntake. */ ArmSub armSub; Gripper gripper; - double timer; - public GroundIntake(ArmSub armSub, Gripper gripper) { - this.gripper = gripper; this.armSub = armSub; - addRequirements(armSub, gripper); - + this.gripper = gripper; + // Use addRequirements() here to declare subsystem dependencies. } + // Called when the command is initially scheduled. @Override - public void initialize() { - timer = 0; - } + public void initialize() {} + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - timer++; - armSub.armRotPresetPositions(ArmPositions.GROUND_PICKUP_ADAPTIVE); - if (timer >= 20){ - gripper.moveGripper(1); + if(GlobalVariables.gamePiece == 0){ + armSub.gripperJointPosition(19); + armSub.extendArmPosition(7000); + armSub.moveRotArmMotionMagic(1); + gripper.moveGripper(-1); } + else{ + armSub.gripperJointPosition(15); + armSub.extendArmPosition(7000); + armSub.moveRotArmMotionMagic(1); + gripper.moveGripper(0.7); + } } + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { gripper.moveGripper(0); - System.out.println("END"); } + // Returns true when the command should end. @Override public boolean isFinished() { - if(gripper.getDistanceSensorDist() > 150){ - return true; - - } - else{ return false; - } } } diff --git a/src/main/java/frc/robot/commands/HoldJointPos.java b/src/main/java/frc/robot/commands/HoldJointPos.java new file mode 100644 index 0000000..72fd749 --- /dev/null +++ b/src/main/java/frc/robot/commands/HoldJointPos.java @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ArmSub; + +public class HoldJointPos extends CommandBase { + /** Creates a new HoldJointPos. */ + ArmSub armSub; + double position; + + public HoldJointPos(ArmSub armSub, double position) { + this.armSub = armSub; + this.position = position; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + armSub.gripperJointPosition(position); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/ArmSub.java b/src/main/java/frc/robot/subsystems/ArmSub.java index 0bbb7ed..1c47d8d 100644 --- a/src/main/java/frc/robot/subsystems/ArmSub.java +++ b/src/main/java/frc/robot/subsystems/ArmSub.java @@ -35,6 +35,8 @@ public class ArmSub extends SubsystemBase { public TalonFX telescopeArm; + public TalonFX gripperJointFalcon; + public CANSparkMax gripperJointNeo; public SparkMaxPIDController gripperJointPID; // public SparkMaxAbsoluteEncoder encoder; @@ -60,14 +62,17 @@ public ArmSub() { testCanCoder = new CANCoder(14, "Karen"); testCanCoder.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360); + testCanCoder.configSensorDirection(true); + // testCanCoder.setPosition(0); + testCanCoder.configMagnetOffset(-12); - rotMaxSpeedFor = 0.7; - rotMaxSpeedRev = 1; + rotMaxSpeedFor = 1; + rotMaxSpeedRev = 0.7; limit = new SupplyCurrentLimitConfiguration(true, 20, 20, 0); - // gripperJointEncoder = new CANSparkMax(55, MotorType.kBrushless); + // gripperJointEncoder = new CANSparkMax(55, MotorType .kBrushless); // encoder = gripperJointEncoder.getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type.kDutyCycle); gripperJointNeo = new CANSparkMax(45, MotorType.kBrushless); gripperJointNeo.clearFaults(); @@ -75,25 +80,33 @@ public ArmSub() { gripperJointNeo.getEncoder().setPosition(0); gripperJointNeo.setIdleMode(IdleMode.kBrake); gripperJointNeo.setSmartCurrentLimit(30); + gripperJointNeo.setInverted(true); + // gripperJointNeo.getEncoder().setInverted(true); jointEncoder = gripperJointNeo.getEncoder(); gripperJointPID = gripperJointNeo.getPIDController(); - gripperJointPID.setP(1); + gripperJointPID.setP(0.03); gripperJointPID.setI(0); gripperJointPID.setD(0); gripperJointPID.setFeedbackDevice(gripperJointNeo.getEncoder()); + gripperJointPID.setSmartMotionMaxAccel(5, 1); + + armPID = new PIDController(0.012, 0.0001, 0.0007); + // armPID = new PIDController(0.012, 0.000, 0.000); - // pidLoop = new PIDController(0.006, 0.003, 0.001); - armPID = new PIDController(0.015, 0.00, 0.00); armPID.setTolerance(0.5); armPID.enableContinuousInput(0, 360); + + // armPID.setIntegratorRange(1, 0.1); + // pidLoop. leftArm = new WPI_TalonFX(20, "Karen"); rightArm = new WPI_TalonFX(21, "Karen"); telescopeArm = new WPI_TalonFX(22, "Karen"); + gripperJointFalcon = new WPI_TalonFX(40, "Karen"); leftArm.configFactoryDefault(); leftArm.setNeutralMode(NeutralMode.Brake); @@ -104,7 +117,7 @@ public ArmSub() { leftArm.config_kD(0, 0.0); leftArm.config_kF(0, 0.06); leftArm.setSensorPhase(true); - leftArm.setInverted(InvertType.None); + leftArm.setInverted(InvertType.InvertMotorOutput); leftArm.configRemoteFeedbackFilter(testCanCoder, 0); leftArm.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); @@ -140,6 +153,22 @@ public ArmSub() { telescopeArm.configMotionCruiseVelocity(10000); telescopeArm.setSelectedSensorPosition(0); telescopeArm.configSupplyCurrentLimit(limit); + + gripperJointFalcon.configFactoryDefault(); + gripperJointFalcon.configPeakOutputForward(telMaxSpeedFor); + gripperJointFalcon.configPeakOutputReverse(-telMaxSpeedRev); + gripperJointFalcon.setSensorPhase(true); + gripperJointFalcon.setInverted(InvertType.None); + gripperJointFalcon.config_kP(0, 0.01); + gripperJointFalcon.config_kI(0, 0.0); + gripperJointFalcon.config_kD(0, 0.0); + gripperJointFalcon.config_kF(0, 0.0); + gripperJointFalcon.setNeutralMode(NeutralMode.Brake); + gripperJointFalcon.configNeutralDeadband(0.1); + gripperJointFalcon.configMotionAcceleration(100000); + gripperJointFalcon.configMotionCruiseVelocity(10000); + gripperJointFalcon.setSelectedSensorPosition(0); + gripperJointFalcon.configSupplyCurrentLimit(limit); @@ -160,6 +189,11 @@ public void moveRotArmMotionMagic(double degrees){ leftArm.set(ControlMode.PercentOutput ,armPID.calculate(testCanCoder.getAbsolutePosition(), degrees)); } + +// public void homeArmRotPos(){ +// testCanCoder.setPosition(0); + +// } /** * Moves the arm by percent output. * @param percent 0.0 - 1.0. @@ -190,11 +224,12 @@ public void extendArmPosition(double position){ } public void gripperJointPosition(double position){ - gripperJointPID.setReference(position, ControlType.kPosition); + gripperJointFalcon.set(TalonFXControlMode.MotionMagic, position); } public void manualJointControl(double percent ){ - gripperJointNeo.set(percent); + // gripperJointNeo.set(percent); + gripperJointFalcon.set(TalonFXControlMode.PercentOutput, percent); } @@ -210,6 +245,11 @@ public void homeRotArmPos(){ leftArm.setSelectedSensorPosition(testCanCoder.getPosition() /30); } +public void homeJointPos(){ + // gripperJointNeo.getEncoder().setPosition(0); + gripperJointFalcon.setSelectedSensorPosition(0); +} + /** * Moves the arm to a dedicated degree. * @param position Desired arm position in degrees @@ -651,7 +691,8 @@ public double getArmMotorPos(){ } public double getJointPos(){ - return jointEncoder.getPosition(); + // return jointEncoder.getPosition(); + return gripperJointFalcon.getSelectedSensorPosition(); } diff --git a/src/main/java/frc/robot/subsystems/CANdleSub.java b/src/main/java/frc/robot/subsystems/CANdleSub.java index 175b7ac..b445508 100644 --- a/src/main/java/frc/robot/subsystems/CANdleSub.java +++ b/src/main/java/frc/robot/subsystems/CANdleSub.java @@ -30,7 +30,7 @@ public CANdleSub() { } public void disabledLed(){ - m_candle.animate(new SingleFadeAnimation(252, 40, 101, 0, 0.6, numLed, ledOffset)); + m_candle.animate(new SingleFadeAnimation(255, 0, 0, 0, 0.6, numLed, ledOffset)); } public void enabledLed(){ diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index e009cb3..f7a1a25 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -339,6 +339,7 @@ public double getCamOffset(){ @Override public void periodic(){ + updateRobotDirection(); // pcw.frontCamPipeline(2); AprilTagGrid.setDefaultOption("Left Tag", gridTag1); @@ -367,6 +368,7 @@ public void periodic(){ SmartDashboard.putNumber("Gyro Yaw ", gyro.getYaw()); SmartDashboard.putNumber("AprilTag Yaw ", getAprilTagEstPosition().getRotation().getDegrees()); + SmartDashboard.putBoolean("robot direction ", GlobalVariables.robotDirection); kyslol(); From 17db3beca9b484cfb88b3cd6d2658503528d0232 Mon Sep 17 00:00:00 2001 From: Oscar Date: Fri, 31 Mar 2023 22:36:33 -0700 Subject: [PATCH 09/12] Arm Tuning --- .pathplanner/settings.json | 7 + src/main/deploy/pathplanner/FullAuto.path | 74 ++++++ src/main/java/frc/robot/Constants.java | 112 ++++----- .../java/frc/robot/PhotonCameraWrapper.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 94 ++++--- .../robot/commands/AdaptiveArmMovement.java | 60 +++++ .../robot/commands/ArmExtendPositionTest.java | 4 +- .../frc/robot/commands/ArmExtendTest.java | 8 +- .../robot/commands/ArmPercentageCommand.java | 2 +- .../frc/robot/commands/ArmRotationTest.java | 2 +- .../frc/robot/commands/FullArmPosition.java | 16 +- .../java/frc/robot/commands/GroundIntake.java | 31 ++- .../frc/robot/commands/HoldArmPosAuto.java | 2 +- .../java/frc/robot/commands/HoldJointPos.java | 2 +- .../frc/robot/commands/MoveJointTemp.java | 2 +- .../java/frc/robot/subsystems/ArmSub.java | 236 +++++++----------- .../java/frc/robot/subsystems/CANdleSub.java | 2 +- .../java/frc/robot/subsystems/Gripper.java | 12 - .../java/frc/robot/subsystems/Swerve.java | 18 +- vendordeps/PathplannerLib.json | 35 +++ 20 files changed, 442 insertions(+), 281 deletions(-) create mode 100644 .pathplanner/settings.json create mode 100644 src/main/deploy/pathplanner/FullAuto.path create mode 100644 src/main/java/frc/robot/commands/AdaptiveArmMovement.java create mode 100644 vendordeps/PathplannerLib.json diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..a9266fc --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,7 @@ +{ + "robotWidth": 0.61, + "robotLength": 0.64, + "holonomicMode": true, + "generateJSON": false, + "generateCSV": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/FullAuto.path b/src/main/deploy/pathplanner/FullAuto.path new file mode 100644 index 0000000..9410d25 --- /dev/null +++ b/src/main/deploy/pathplanner/FullAuto.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.0011332330356972, + "y": 4.156577202043294 + }, + "prevControl": null, + "nextControl": { + "x": 3.0011332330356977, + "y": 4.156577202043294 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.1985003115305375, + "y": 6.760147537389092 + }, + "prevControl": { + "x": 5.1985003115305375, + "y": 5.760147537389092 + }, + "nextControl": { + "x": 5.1985003115305375, + "y": 5.760147537389092 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.60305799244077, + "y": 4.681858936367445 + }, + "prevControl": { + "x": 5.60305799244077, + "y": 4.681858936367445 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9693dff..c650934 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,110 +41,110 @@ public final class Constants { public static final class ArmRotationValues { - public static final double armRotStow = 0; + public static final double armRotStow = 5; - public static final double armRotForHighCone = 0; - public static final double armRotForHighCube = 0; + public static final double armRotForHighCone = 50; + public static final double armRotForHighCube = 50; - public static final double armRotForMidCone = 0; - public static final double armRotForMidCube = 0; + public static final double armRotForMidCone = 50; + public static final double armRotForMidCube = 50; - public static final double armRotForLowCone = 0; - public static final double armRotForLowCube = 0; + public static final double armRotForLowCone = 5; + public static final double armRotForLowCube = 5; - public static final double armRotForConePickup = 0; - public static final double armRotForCubePickup = 0; + public static final double armRotForConePickup = 3; + public static final double armRotForCubePickup = 3; - public static final double armRotForShelfCone = 0; - public static final double armRotForShelfCube = 0; + public static final double armRotForShelfCone = 70; + public static final double armRotForShelfCube = 70; - public static final double armRotRevHighCone = 0; - public static final double armRotRevHighCube = 0; + public static final double armRotRevHighCone = 138; + public static final double armRotRevHighCube = 130; - public static final double armRotRevMidCone = 0; - public static final double armRotRevMidCube = 0; + public static final double armRotRevMidCone = 130; + public static final double armRotRevMidCube = 130; - public static final double armRotRevLowCone = 0; - public static final double armRotRevLowCube = 0; + public static final double armRotRevLowCone = 5; + public static final double armRotRevLowCube = 90; - public static final double armRotRevConePickup = 0; - public static final double armRotRevCubePickup = 0; + public static final double armRotRevConePickup = 180; + public static final double armRotRevCubePickup = 170; - public static final double armRotRevShelfCone = 0; - public static final double armRotRevShelfCube = 0; + public static final double armRotRevShelfCone = 120; + public static final double armRotRevShelfCube = 120; } public static final class ArmExtendValues { public static final double armExtendStow = 0; - public static final double armExtendConePickup = 0; - public static final double armExtendCubePickup = 0; + public static final double armExtendConePickup = 7000; + public static final double armExtendCubePickup = 7000; - public static final double armExtendForHighCone = 0; - public static final double armExtendForHighCube = 0; + public static final double armExtendForHighCone = 50000; + public static final double armExtendForHighCube = 50000; - public static final double armExtendForMidCone = 0; - public static final double armExtendForMidCube = 0; + public static final double armExtendForMidCone = 30000; + public static final double armExtendForMidCube = 30000; public static final double armExtendForLowCone = 0; public static final double armExtendForLowCube = 0; - public static final double armExtendForConePickup = 0; - public static final double armExtendForCubePickup = 0; + public static final double armExtendForConePickup = 7000; + public static final double armExtendForCubePickup = 7000; - public static final double armExtendForShelfCone = 0; - public static final double armExtendForShelfCube = 0; + public static final double armExtendForShelfCone = 50000; + public static final double armExtendForShelfCube = 50000; - public static final double armExtendRevHighCone = 0; - public static final double armExtendRevHighCube = 0; + public static final double armExtendRevHighCone = 50000; + public static final double armExtendRevHighCube = 50000; - public static final double armExtendRevMidCone = 0; - public static final double armExtendRevMidCube = 0; + public static final double armExtendRevMidCone = 30000; + public static final double armExtendRevMidCube = 30000; public static final double armExtendRevLowCone = 0; public static final double armExtendRevLowCube = 0; - public static final double armExtendRevShelfCone = 0; - public static final double armExtendRevShelfCube = 0; + public static final double armExtendRevShelfCone = 50000; + public static final double armExtendRevShelfCube = 50000; } public static final class JointRotationValues { - public static final double JointRotStowCone = 0; - public static final double JointRotStowCube = 0; + public static final double JointRotStowCone = 500; + public static final double JointRotStowCube = 1000; - public static final double JointRotConePickup = 0; - public static final double JointRotCubePickup = 0; + public static final double JointRotConePickup = 11000; + public static final double JointRotCubePickup = 9000; - public static final double JointRotForHighCone = 0; - public static final double JointRotForHighCube = 0; + public static final double JointRotForHighCone = 16000; + public static final double JointRotForHighCube = 9000; - public static final double JointRotForMidCone = 0; - public static final double JointRotForMidCube = 0; + public static final double JointRotForMidCone = 16000; + public static final double JointRotForMidCube = 9000; - public static final double JointRotForLowCone = 0; - public static final double JointRotForLowCube = 0; + public static final double JointRotForLowCone = 9000; + public static final double JointRotForLowCube = 4000; - public static final double JointRotForConePickup = 0; - public static final double JointRotForCubePickup = 0; + public static final double JointRotForConePickup = 11000; + public static final double JointRotForCubePickup = 9000; - public static final double JointRotForShelfCone = 0; - public static final double JointRotForShelfCube = 0; + public static final double JointRotForShelfCone = 16000; + public static final double JointRotForShelfCube = 9000; - public static final double JointRotRevHighCone = 0; - public static final double JointRotRevHighCube = 0; + public static final double JointRotRevHighCone = 6600; + public static final double JointRotRevHighCube = 4000; - public static final double JointRotRevMidCone = 0; - public static final double JointRotRevMidCube = 0; + public static final double JointRotRevMidCone = 11000; + public static final double JointRotRevMidCube = 4000; public static final double JointRotRevLowCone = 0; public static final double JointRotRevLowCube = 0; - public static final double JointRotRevShelfCone = 0; + public static final double JointRotRevShelfCone = 4000; public static final double JointRotRevShelfCube = 0; } diff --git a/src/main/java/frc/robot/PhotonCameraWrapper.java b/src/main/java/frc/robot/PhotonCameraWrapper.java index b5d1277..0ab2f7a 100644 --- a/src/main/java/frc/robot/PhotonCameraWrapper.java +++ b/src/main/java/frc/robot/PhotonCameraWrapper.java @@ -30,8 +30,8 @@ public PhotonCameraWrapper() { frontCam = new PhotonCamera(VisionConstants.camera1Name); rearCam = new PhotonCamera(VisionConstants.camera2Name); - photonPoseEstimator2 = new PhotonPoseEstimator(aprilTagFieldLayout, org.photonvision.PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, frontCam, VisionConstants.robotToCam1); - photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, org.photonvision.PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, rearCam, VisionConstants.robotToCam2); + // photonPoseEstimator2 = new PhotonPoseEstimator(aprilTagFieldLayout, org.photonvision.PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, frontCam, VisionConstants.robotToCam1); + // photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, org.photonvision.PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, rearCam, VisionConstants.robotToCam2); } catch (Exception e) { System.out.println(e); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6340bd1..2ba9bb5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,16 @@ package frc.robot; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; + +import com.pathplanner.lib.PathConstraints; +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import com.pathplanner.lib.auto.PIDConstants; +import com.pathplanner.lib.auto.SwerveAutoBuilder; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -18,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; @@ -46,27 +57,10 @@ public class RobotContainer { private final int rotationAxis = 2; /* Driver Buttons */ - private final Trigger LeftLittleButton = new JoystickButton(driver, 7); - private final Trigger RightLittleButton = new JoystickButton(driver, 8); - private final Trigger buttonX = new JoystickButton(driver, 3); - private final Trigger buttonY = new JoystickButton(driver, 4); - private final Trigger buttonB = new JoystickButton(driver, 2); - private final Trigger buttonA = new JoystickButton(driver, 1); - private final Trigger leftBumper = new JoystickButton(driver, 5); - private final Trigger rightBumper = new JoystickButton(driver, 6); - private final Trigger leftJoy = new JoystickButton(joystickPanel, 1); + private final Trigger rightJoy = new JoystickButton(joystickPanel, 2); public final Trigger operatorDeploy = new JoystickButton(operatorPanel, 11); - private final Trigger operatorShoot = new JoystickButton(operatorPanel, 3); - private final Trigger operatorLeftEmpty = new JoystickButton(operatorPanel, 9); - private final Trigger operatorRightEmpty = new JoystickButton(operatorPanel, 12); - private final Trigger operatorLatch = new JoystickButton(operatorPanel, 7); - private final Trigger operatorHook = new JoystickButton(operatorPanel, 8); - private final Trigger operatorIntakeUp = new JoystickButton(operatorPanel, 13); - private final Trigger operatorIntakeDown = new JoystickButton(operatorPanel, 14); - private final Trigger operatorSpinup = new JoystickButton(operatorPanel, 1); - private final Trigger operatorExtraRight = new JoystickButton(operatorPanel, 15); - private final Trigger xboxRightJoyButton = new JoystickButton(driver, 10); + private final Trigger panelRollers = new JoystickButton(operatorPanel, 10); private final Trigger panelShelf = new JoystickButton(operatorPanel, 9); @@ -119,8 +113,10 @@ public RobotContainer() { withInterruptBehavior(InterruptionBehavior.kCancelSelf)); // armSub.setDefaultCommand(new HoldArmPos(armSub, ArmPositions.STOWED_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); //candle.setDefaultCommand(new InstantCommand(() -> candle.setColor(operatorExtraRight.getAsBoolean()))); - armSub.setDefaultCommand(new MoveJointTemp(armSub, operatorPanel)); + // armSub.setDefaultCommand(new MoveJointTemp(armSub, operatorPanel)); // armSub.setDefaultCommand(new ArmExtendTest(armSub, operatorPanel)); + // armSub.setDefaultCommand(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + armSub.setDefaultCommand(new AdaptiveArmMovement(armSub, ArmPositions.STOWED_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); candle.setDefaultCommand(new SetLedCommand(candle)); configureButtonBindings(); @@ -135,16 +131,24 @@ public RobotContainer() { private void configureButtonBindings() { panelRollers.whileTrue(new GripperCommand(gripper, 0.7)); //Push OUT panelShelf.whileTrue(new GripperCommand(gripper, -1)); // Take IN - panelHigh.onTrue(new InstantCommand(() -> armSub.homeJointPos())); + panelStow.onTrue(new InstantCommand(() -> armSub.homeGripperJointPos())); + panelHigh.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.HIGH_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelMid.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelLow.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.GROUND_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + + // panelStow.whileTrue(new FullArmPosition(armSub, 150, 50000, 9000, true)); + // panelStow.whileFalse(new FullArmPosition(armSub, 0, 0, 0, false)); - // panelStow.whileTrue(new FullArmPosition(armSub, 150, 50000, true)); - // panelStow.whileFalse(new FullArmPosition(armSub, 0, 0, false)); + // panelMid.whileTrue(new ArmExtendPositionTest(armSub, 50000)); + // panelMid.whileFalse(new ArmExtendPositionTest(armSub, 0)); + + // panelLow.whileTrue(new ArmRotationTest(armSub, 130).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // panelLow.whileFalse(new ArmRotationTest(armSub, 5).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + + // panelGround.whileTrue(new HoldJointPos(armSub, 11000).alongWith(new ArmExtendPositionTest(armSub, 7000)).alongWith(new ArmRotationTest(armSub, 0)).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // panelGround.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 2)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - panelMid.whileTrue(new ArmExtendPositionTest(armSub, 50000)); - panelMid.whileFalse(new ArmExtendPositionTest(armSub, 0)); - panelLow.whileTrue(new ArmRotationTest(armSub, 130)); - panelLow.whileFalse(new ArmRotationTest(armSub, 0)); // panelLow.onTrue(new InstantCommand(() -> armSub.homeArmRotPos())); @@ -153,17 +157,14 @@ private void configureButtonBindings() { - // panelGround.whileTrue(new HoldJointPos(armSub, 19).alongWith(new ArmExtendPositionTest(armSub, 7000)).alongWith(new ArmRotationTest(armSub, 1))); - // panelGround.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - - + // panelStow.whileTrue(new HoldJointPos(armSub, 15).alongWith(new ArmExtendPositionTest(armSub, 7000)).alongWith(new ArmRotationTest(armSub, 0))); // panelStow.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - // panelGround.whileTrue(new GroundIntake(armSub, gripper)); - // panelGround.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + panelGround.whileTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelGround.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - // panelStow.whileTrue(new HoldJointPos(armSub, 22)); + // panelStow.whileTrue(new HoldJointPos(armSub, 11000)); // panelStow.whileFalse(new HoldJointPos(armSub, 0)); // panelStow.whileTrue(new HoldJointPos(armSub, 30).alongWith(new ArmExtendPositionTest(armSub, 5000)).alongWith(new ArmRotationTest(armSub, 37))); // panelStow.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5))); @@ -236,11 +237,34 @@ private void configureButtonBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { + + List pathGroup = PathPlanner.loadPathGroup("FullAuto", PathPlanner.getConstraintsFromPath("FullAuto")); + +// This is just an example event map. It would be better to have a constant, global event map +// in your code that will be used by all path following commands. +HashMap eventMap = new HashMap<>(); +eventMap.put("marker1", new PrintCommand("Passed marker 1")); + +// Create the AutoBuilder. This only needs to be created once when robot code starts, not every time you want to create an auto command. A good place to put this is in RobotContainer along with your subsystems. +SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder( + s_Swerve::getPose, // Pose2d supplier + s_Swerve::resetOdometry, // Pose2d consumer, used to reset odometry at the beginning of auto + Constants.Swerve.swerveKinematics, // SwerveDriveKinematics + new PIDConstants(5.0, 0.0, 0.0), // PID constants to correct for translation error (used to create the X and Y PID controllers) + new PIDConstants(0.5, 0.0, 0.0), // PID constants to correct for rotation error (used to create the rotation controller) + s_Swerve::setModuleStates, // Module states consumer used to output to the drive subsystem + eventMap, + true, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true + s_Swerve // The drive subsystem. Used to properly set the requirements of path following commands +); + +Command fullAuto = autoBuilder.fullAuto(pathGroup); + // An ExampleCommand will run in autonomous // return new BalanceAuto(s_Swerve, armSub, gripper); // return new BalanceAutoRed(s_Swerve, armSub, gripper); // return m_chooser.getSelected(); - return new ButterAutoTest(s_Swerve); + return fullAuto;//new ButterAutoTest(s_Swerve); // return new LeftSideAuto(s_Swerve, armSub, gripper); // return new LeftSideAutoRed(s_Swerve, armSub, gripper); } diff --git a/src/main/java/frc/robot/commands/AdaptiveArmMovement.java b/src/main/java/frc/robot/commands/AdaptiveArmMovement.java new file mode 100644 index 0000000..42cbeb9 --- /dev/null +++ b/src/main/java/frc/robot/commands/AdaptiveArmMovement.java @@ -0,0 +1,60 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.GlobalVariables.ArmPositions; +import frc.robot.subsystems.ArmSub; + +public class AdaptiveArmMovement extends CommandBase { + /** Creates a new AdaptiveArmMovement. */ + ArmSub armSub; + GlobalVariables.ArmPositions armPosition; + public AdaptiveArmMovement(ArmSub armSub, GlobalVariables.ArmPositions armPosition) { + this.armSub = armSub; + this.armPosition = armPosition; + addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(armPosition == ArmPositions.STOWED_ADAPTIVE){ + armSub.armExtendPresetPositions(armPosition); + if(armSub.getTelescopePos() <= 10000){ + armSub.armRotPresetPositions(armPosition); + armSub.jointRotPresetPositions(armPosition); + + } + } + else{ + + armSub.armRotPresetPositions(armPosition); + armSub.jointRotPresetPositions(armPosition); + + if(armSub.getArmEncoderPosition() >= GlobalVariables.armRotGoal*0.75){ + armSub.armExtendPresetPositions(armPosition); + } + + } + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ArmExtendPositionTest.java b/src/main/java/frc/robot/commands/ArmExtendPositionTest.java index 3c4b42d..5744fb4 100644 --- a/src/main/java/frc/robot/commands/ArmExtendPositionTest.java +++ b/src/main/java/frc/robot/commands/ArmExtendPositionTest.java @@ -26,14 +26,14 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - armSub.extendArmPosition(position); + armSub.moveTelescopeArmPosition(position); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - armSub.extendArmPosition(0); + armSub.moveTelescopeArmPosition(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/ArmExtendTest.java b/src/main/java/frc/robot/commands/ArmExtendTest.java index d81a86b..48f6672 100644 --- a/src/main/java/frc/robot/commands/ArmExtendTest.java +++ b/src/main/java/frc/robot/commands/ArmExtendTest.java @@ -28,10 +28,10 @@ public void initialize() {} @Override public void execute() { if(controller.getRawButton(17)){ - armsub.moveArmPercentOutput(controller.getRawAxis(0)); + armsub.moveRotArmPercentOutput(controller.getRawAxis(0)); } else{ - armsub.extendArmPercentOutput(controller.getRawAxis(0)); + armsub.moveTelescopeArmPercentOutput(controller.getRawAxis(0)); } // armsub.extendArmPosition(1000); } @@ -39,8 +39,8 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - armsub.moveArmPercentOutput(0); - armsub.extendArmPercentOutput(0); + armsub.moveRotArmPercentOutput(0); + armsub.moveTelescopeArmPercentOutput(0); // armsub.extendArmPosition(0); } diff --git a/src/main/java/frc/robot/commands/ArmPercentageCommand.java b/src/main/java/frc/robot/commands/ArmPercentageCommand.java index c5866d4..1b192fd 100644 --- a/src/main/java/frc/robot/commands/ArmPercentageCommand.java +++ b/src/main/java/frc/robot/commands/ArmPercentageCommand.java @@ -31,7 +31,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - armSub.moveArmPercentOutput(controller.getRawAxis(axis1) - controller.getRawAxis(axis2) ); + armSub.moveRotArmPercentOutput(controller.getRawAxis(axis1) - controller.getRawAxis(axis2) ); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/ArmRotationTest.java b/src/main/java/frc/robot/commands/ArmRotationTest.java index 1b63886..fd68f53 100644 --- a/src/main/java/frc/robot/commands/ArmRotationTest.java +++ b/src/main/java/frc/robot/commands/ArmRotationTest.java @@ -25,7 +25,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - armSub.moveArmPosition(position); + armSub.moveRotArmPosition(position); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/FullArmPosition.java b/src/main/java/frc/robot/commands/FullArmPosition.java index b97443a..66147af 100644 --- a/src/main/java/frc/robot/commands/FullArmPosition.java +++ b/src/main/java/frc/robot/commands/FullArmPosition.java @@ -11,15 +11,17 @@ public class FullArmPosition extends CommandBase { ArmSub armSub; double armRotPos; double armExtendPos; + double armJointPos; boolean direction; /** Creates a new FullArmPosition. */ - public FullArmPosition(ArmSub armSub, double armRotPos, double armExtendPos, boolean direction) { + public FullArmPosition(ArmSub armSub, double armRotPos, double armExtendPos, double armJointPos, boolean direction) { this.armSub = armSub; this.armRotPos = armRotPos; this.armExtendPos = armExtendPos; + this.armJointPos = armJointPos; this.direction = direction; // Use addRequirements() here to declare subsystem dependencies. } @@ -32,15 +34,17 @@ public void initialize() {} @Override public void execute() { if(direction){ - armSub.moveRotArmMotionMagic(armRotPos); - if(armSub.getArmPosition() >= armRotPos*0.75){ - armSub.extendArmPosition(armExtendPos); + armSub.moveRotArmPosition(armRotPos); + if(armSub.getArmEncoderPosition() >= armRotPos*0.75){ + armSub.moveTelescopeArmPosition(armExtendPos); + armSub.moveGripperJointPosition(armJointPos); } } else{ - armSub.extendArmPosition(armExtendPos); + armSub.moveTelescopeArmPosition(armExtendPos); + armSub.moveGripperJointPosition(armJointPos); if(armSub.getTelescopePos() <= 30000){ - armSub.moveRotArmMotionMagic(armRotPos); + armSub.moveRotArmPosition(armRotPos); } } } diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index 0e60280..6f2d27d 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -16,6 +16,7 @@ public class GroundIntake extends CommandBase { public GroundIntake(ArmSub armSub, Gripper gripper) { this.armSub = armSub; this.gripper = gripper; + addRequirements(armSub); // Use addRequirements() here to declare subsystem dependencies. } @@ -27,17 +28,31 @@ public void initialize() {} @Override public void execute() { if(GlobalVariables.gamePiece == 0){ - armSub.gripperJointPosition(19); - armSub.extendArmPosition(7000); - armSub.moveRotArmMotionMagic(1); - gripper.moveGripper(-1); + armSub.moveGripperJointPosition(13000); + armSub.moveTelescopeArmPosition(8000); + armSub.moveRotArmPosition(1); + if((gripper.getDistanceSensorDist() <= 13)){ + gripper.moveGripper(0); + } + else{ + gripper.moveGripper(-1); + + } } + else{ - armSub.gripperJointPosition(15); - armSub.extendArmPosition(7000); - armSub.moveRotArmMotionMagic(1); - gripper.moveGripper(0.7); + armSub.moveGripperJointPosition(8000); + armSub.moveTelescopeArmPosition(8000); + armSub.moveRotArmPosition(1); + if((gripper.getDistanceSensorDist() <= 13)){ + gripper.moveGripper(0); + + } + else{ + gripper.moveGripper(0.8); + + } } diff --git a/src/main/java/frc/robot/commands/HoldArmPosAuto.java b/src/main/java/frc/robot/commands/HoldArmPosAuto.java index 05106bf..6a08152 100644 --- a/src/main/java/frc/robot/commands/HoldArmPosAuto.java +++ b/src/main/java/frc/robot/commands/HoldArmPosAuto.java @@ -34,7 +34,7 @@ public void initialize() { public void execute() { timer ++; double armpos = Constants.ArmPositions[GlobalVariables.gamePiece][GlobalVariables.height]; - armSub.moveArmPosition(armpos); + armSub.moveRotArmPosition(armpos); System.out.println("ARM COMMAND EXEC"); } diff --git a/src/main/java/frc/robot/commands/HoldJointPos.java b/src/main/java/frc/robot/commands/HoldJointPos.java index 72fd749..e33c882 100644 --- a/src/main/java/frc/robot/commands/HoldJointPos.java +++ b/src/main/java/frc/robot/commands/HoldJointPos.java @@ -25,7 +25,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - armSub.gripperJointPosition(position); + armSub.moveGripperJointPosition(position); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/MoveJointTemp.java b/src/main/java/frc/robot/commands/MoveJointTemp.java index a643a0a..2abbb8c 100644 --- a/src/main/java/frc/robot/commands/MoveJointTemp.java +++ b/src/main/java/frc/robot/commands/MoveJointTemp.java @@ -26,7 +26,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - armsub.manualJointControl(controller.getRawAxis(0)); + armsub.moveGripperJointPercentOutput(controller.getRawAxis(0)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/ArmSub.java b/src/main/java/frc/robot/subsystems/ArmSub.java index 1c47d8d..b4cf672 100644 --- a/src/main/java/frc/robot/subsystems/ArmSub.java +++ b/src/main/java/frc/robot/subsystems/ArmSub.java @@ -14,13 +14,6 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.AbsoluteSensorRange; import com.ctre.phoenix.sensors.CANCoder; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkMaxAbsoluteEncoder; -import com.revrobotics.SparkMaxPIDController; -import com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -37,10 +30,7 @@ public class ArmSub extends SubsystemBase { public TalonFX gripperJointFalcon; - public CANSparkMax gripperJointNeo; - public SparkMaxPIDController gripperJointPID; - // public SparkMaxAbsoluteEncoder encoder; - public RelativeEncoder jointEncoder; + public PIDController armPID; public PIDController jointPID; public PIDController telescopePID; @@ -63,45 +53,21 @@ public ArmSub() { testCanCoder = new CANCoder(14, "Karen"); testCanCoder.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360); testCanCoder.configSensorDirection(true); - // testCanCoder.setPosition(0); testCanCoder.configMagnetOffset(-12); - rotMaxSpeedFor = 1; - rotMaxSpeedRev = 0.7; - - limit = new SupplyCurrentLimitConfiguration(true, 20, 20, 0); - - // gripperJointEncoder = new CANSparkMax(55, MotorType .kBrushless); - // encoder = gripperJointEncoder.getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type.kDutyCycle); - gripperJointNeo = new CANSparkMax(45, MotorType.kBrushless); - gripperJointNeo.clearFaults(); - gripperJointNeo.restoreFactoryDefaults(); - gripperJointNeo.getEncoder().setPosition(0); - gripperJointNeo.setIdleMode(IdleMode.kBrake); - gripperJointNeo.setSmartCurrentLimit(30); - gripperJointNeo.setInverted(true); - // gripperJointNeo.getEncoder().setInverted(true); + rotMaxSpeedFor = 0.3; + rotMaxSpeedRev = 0.3; - jointEncoder = gripperJointNeo.getEncoder(); + limit = new SupplyCurrentLimitConfiguration(true, 10, 10, 0); - gripperJointPID = gripperJointNeo.getPIDController(); - gripperJointPID.setP(0.03); - gripperJointPID.setI(0); - gripperJointPID.setD(0); - gripperJointPID.setFeedbackDevice(gripperJointNeo.getEncoder()); - gripperJointPID.setSmartMotionMaxAccel(5, 1); - - - armPID = new PIDController(0.012, 0.0001, 0.0007); - // armPID = new PIDController(0.012, 0.000, 0.000); + - armPID.setTolerance(0.5); + armPID = new PIDController(0.024, 0.000, 0.000); + armPID.setTolerance(0.1); armPID.enableContinuousInput(0, 360); - // armPID.setIntegratorRange(1, 0.1); - // pidLoop. leftArm = new WPI_TalonFX(20, "Karen"); rightArm = new WPI_TalonFX(21, "Karen"); @@ -143,13 +109,13 @@ public ArmSub() { telescopeArm.configPeakOutputReverse(-telMaxSpeedRev); telescopeArm.setSensorPhase(true); telescopeArm.setInverted(InvertType.None); - telescopeArm.config_kP(0, 0.5); + telescopeArm.config_kP(0, 0.3); telescopeArm.config_kI(0, 0.0); telescopeArm.config_kD(0, 0.0); telescopeArm.config_kF(0, 0.0); telescopeArm.setNeutralMode(NeutralMode.Brake); telescopeArm.configNeutralDeadband(0.1); - telescopeArm.configMotionAcceleration(100000); + telescopeArm.configMotionAcceleration(10000); telescopeArm.configMotionCruiseVelocity(10000); telescopeArm.setSelectedSensorPosition(0); telescopeArm.configSupplyCurrentLimit(limit); @@ -158,17 +124,20 @@ public ArmSub() { gripperJointFalcon.configPeakOutputForward(telMaxSpeedFor); gripperJointFalcon.configPeakOutputReverse(-telMaxSpeedRev); gripperJointFalcon.setSensorPhase(true); - gripperJointFalcon.setInverted(InvertType.None); - gripperJointFalcon.config_kP(0, 0.01); + gripperJointFalcon.setInverted(InvertType.InvertMotorOutput); + gripperJointFalcon.config_kP(0, 0.3); gripperJointFalcon.config_kI(0, 0.0); gripperJointFalcon.config_kD(0, 0.0); gripperJointFalcon.config_kF(0, 0.0); gripperJointFalcon.setNeutralMode(NeutralMode.Brake); - gripperJointFalcon.configNeutralDeadband(0.1); - gripperJointFalcon.configMotionAcceleration(100000); + gripperJointFalcon.configNeutralDeadband(0.001); + gripperJointFalcon.configMotionAcceleration(5000); gripperJointFalcon.configMotionCruiseVelocity(10000); gripperJointFalcon.setSelectedSensorPosition(0); + gripperJointFalcon.configAllowableClosedloopError(0, 10); + gripperJointFalcon.configSupplyCurrentLimit(limit); + // gripperJointFalcon.setSensorPhase(false); @@ -182,52 +151,39 @@ public void ArmBrakeMode(NeutralMode mode){ } - public void moveRotArmMotionMagic(double degrees){ - // TODO degrees to falcon readout - // double setpoint = degrees *3; - // leftArm.set(TalonFXControlMode.MotionMagic, setpoint); - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(testCanCoder.getAbsolutePosition(), degrees)); + public void moveRotArmPosition(double degrees){ + leftArm.set(ControlMode.PercentOutput, armPID.calculate(testCanCoder.getAbsolutePosition(), degrees)); } -// public void homeArmRotPos(){ -// testCanCoder.setPosition(0); - -// } + /** * Moves the arm by percent output. * @param percent 0.0 - 1.0. */ - public void moveArmPercentOutput(double percent){ + public void moveRotArmPercentOutput(double percent){ leftArm.set(TalonFXControlMode.PercentOutput, percent); } - /** - * Sets the encoders within the arm Falcons to the current CANCoder readout. - * - */ - private void resetToAbsolute(){ - double absolutePosition = Conversions.degreesToFalcon(testCanCoder.getAbsolutePosition(), (56)); - leftArm.setSelectedSensorPosition(absolutePosition); -} + /** * Moves the arm telescope using percent control. * @param percent */ -public void extendArmPercentOutput(double percent){ +public void moveTelescopeArmPercentOutput(double percent){ telescopeArm.set(TalonFXControlMode.PercentOutput, percent); } -public void extendArmPosition(double position){ +public void moveTelescopeArmPosition(double position){ // telescopeArm.set(TalonFXControlMode.Position, position); telescopeArm.set(TalonFXControlMode.MotionMagic, position); } -public void gripperJointPosition(double position){ +public void moveGripperJointPosition(double position){ gripperJointFalcon.set(TalonFXControlMode.MotionMagic, position); } -public void manualJointControl(double percent ){ +public void moveGripperJointPercentOutput(double percent ){ // gripperJointNeo.set(percent); gripperJointFalcon.set(TalonFXControlMode.PercentOutput, percent); @@ -245,7 +201,7 @@ public void homeRotArmPos(){ leftArm.setSelectedSensorPosition(testCanCoder.getPosition() /30); } -public void homeJointPos(){ +public void homeGripperJointPos(){ // gripperJointNeo.getEncoder().setPosition(0); gripperJointFalcon.setSelectedSensorPosition(0); } @@ -254,9 +210,7 @@ public void homeJointPos(){ * Moves the arm to a dedicated degree. * @param position Desired arm position in degrees */ - public void moveArmPosition(double position){ - leftArm.set(ControlMode.PercentOutput ,armPID.calculate(testCanCoder.getAbsolutePosition(), position)); - } + /** * Moves the arm to a dedicated preset positions * @param position The dedicated position enum, current options are GROUND_PICKUP, STOWED, GROUND_SCORE, MID_SCORE, HIGH_SCORE, and SHELF_PICKUP @@ -266,18 +220,18 @@ public void armRotPresetPositions(GlobalVariables.ArmPositions position){ case GROUND_PICKUP_ADAPTIVE: if(GlobalVariables.gamePiece == 0){ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForConePickup); + moveRotArmPosition(Constants.ArmRotationValues.armRotForConePickup); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForConePickup; } else{ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForCubePickup); + moveRotArmPosition(Constants.ArmRotationValues.armRotForCubePickup); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForCubePickup; } break; case STOWED_ADAPTIVE: - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotStow); + moveRotArmPosition(Constants.ArmRotationValues.armRotStow); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotStow; break; @@ -285,24 +239,24 @@ public void armRotPresetPositions(GlobalVariables.ArmPositions position){ case GROUND_SCORE_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForLowCone); + moveRotArmPosition(Constants.ArmRotationValues.armRotForLowCone); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForLowCone; } else{ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForLowCube); + moveRotArmPosition(Constants.ArmRotationValues.armRotForLowCube); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForLowCube; } } else{ if(GlobalVariables.gamePiece == 0){ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevLowCone); + moveRotArmPosition(Constants.ArmRotationValues.armRotRevLowCone); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevLowCone; } else{ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevLowCube); + moveRotArmPosition(Constants.ArmRotationValues.armRotRevLowCube); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevLowCube; } @@ -313,24 +267,24 @@ public void armRotPresetPositions(GlobalVariables.ArmPositions position){ if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForShelfCone); + moveRotArmPosition(Constants.ArmRotationValues.armRotForShelfCone); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForShelfCone; } else{ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForShelfCube); + moveRotArmPosition(Constants.ArmRotationValues.armRotForShelfCube); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForShelfCube; } } else{ if(GlobalVariables.gamePiece == 0){ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevShelfCone); + moveRotArmPosition(Constants.ArmRotationValues.armRotRevShelfCone); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevShelfCone; } else{ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevShelfCube); + moveRotArmPosition(Constants.ArmRotationValues.armRotRevShelfCube); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevShelfCube; } @@ -340,24 +294,24 @@ public void armRotPresetPositions(GlobalVariables.ArmPositions position){ case MID_SCORE_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForMidCone); + moveRotArmPosition(Constants.ArmRotationValues.armRotForMidCone); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForMidCone; } else{ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForMidCube); + moveRotArmPosition(Constants.ArmRotationValues.armRotForMidCube); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForMidCube; } } else{ if(GlobalVariables.gamePiece == 0){ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevMidCone); + moveRotArmPosition(Constants.ArmRotationValues.armRotRevMidCone); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevMidCone; } else{ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevMidCube); + moveRotArmPosition(Constants.ArmRotationValues.armRotRevMidCube); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevMidCube; } @@ -367,24 +321,24 @@ public void armRotPresetPositions(GlobalVariables.ArmPositions position){ case HIGH_SCORE_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForHighCone); + moveRotArmPosition(Constants.ArmRotationValues.armRotForHighCone); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForHighCone; } else{ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotForHighCube); + moveRotArmPosition(Constants.ArmRotationValues.armRotForHighCube); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForHighCube; } } else{ if(GlobalVariables.gamePiece == 0){ - moveRotArmMotionMagic( Constants.ArmRotationValues.armRotRevHighCone); + moveRotArmPosition( Constants.ArmRotationValues.armRotRevHighCone); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevHighCone; } else{ - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotRevHighCube); + moveRotArmPosition(Constants.ArmRotationValues.armRotRevHighCube); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevHighCube; } @@ -394,7 +348,7 @@ public void armRotPresetPositions(GlobalVariables.ArmPositions position){ default: - moveRotArmMotionMagic(Constants.ArmRotationValues.armRotStow); + moveRotArmPosition(Constants.ArmRotationValues.armRotStow); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotStow; break; @@ -406,11 +360,11 @@ public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ case GROUND_PICKUP_ADAPTIVE: if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.ArmExtendValues.armExtendConePickup); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendConePickup); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendConePickup; } else{ - extendArmPosition(Constants.ArmExtendValues.armExtendCubePickup); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendCubePickup); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendCubePickup; @@ -418,7 +372,7 @@ public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ break; case STOWED_ADAPTIVE: - extendArmPosition(Constants.ArmExtendValues.armExtendStow); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendStow); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendStow; break; @@ -426,24 +380,24 @@ public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ case GROUND_SCORE_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.ArmExtendValues.armExtendForLowCone); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForLowCone); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForLowCone; } else{ - extendArmPosition(Constants.ArmExtendValues.armExtendForLowCube); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForLowCube); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForLowCube; } } else{ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.ArmExtendValues.armExtendRevLowCone); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevLowCone); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevLowCone; } else{ - extendArmPosition(Constants.ArmExtendValues.armExtendRevLowCube); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevLowCube); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevLowCube; @@ -455,24 +409,24 @@ public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ case SHELF_PICKUP_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.ArmExtendValues.armExtendForShelfCone); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForShelfCone); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForShelfCone; } else{ - extendArmPosition(Constants.ArmExtendValues.armExtendForShelfCube); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForShelfCube); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForShelfCube; } } else{ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.ArmExtendValues.armExtendRevShelfCone); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevShelfCone); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevShelfCone; } else{ - extendArmPosition(Constants.ArmExtendValues.armExtendRevShelfCube); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevShelfCube); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevShelfCube; @@ -483,24 +437,24 @@ public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ case MID_SCORE_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.ArmExtendValues.armExtendForMidCone); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForMidCone); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForMidCone; } else{ - extendArmPosition(Constants.ArmExtendValues.armExtendForMidCube); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForMidCube); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForMidCube; } } else{ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.ArmExtendValues.armExtendRevMidCone); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevMidCone); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevMidCone; } else{ - extendArmPosition(Constants.ArmExtendValues.armExtendRevMidCube); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevMidCube); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevMidCube; @@ -512,18 +466,18 @@ public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ case HIGH_SCORE_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.ArmExtendValues.armExtendForHighCone); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForHighCone); } else{ - extendArmPosition(Constants.ArmExtendValues.armExtendForHighCube); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForHighCube); } } else{ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.ArmExtendValues.armExtendRevHighCone); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevHighCone); } else{ - extendArmPosition(Constants.ArmExtendValues.armExtendRevHighCube); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevHighCube); } } @@ -533,7 +487,7 @@ public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ default: - extendArmPosition(Constants.ArmExtendValues.armExtendStow); + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendStow); GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendStow; @@ -546,20 +500,20 @@ public void jointRotPresetPositions(GlobalVariables.ArmPositions position){ case GROUND_PICKUP_ADAPTIVE: if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.JointRotationValues.JointRotConePickup); + moveGripperJointPosition(Constants.JointRotationValues.JointRotConePickup); } else{ - extendArmPosition(Constants.JointRotationValues.JointRotCubePickup); + moveGripperJointPosition(Constants.JointRotationValues.JointRotCubePickup); } break; case STOWED_ADAPTIVE: if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.JointRotationValues.JointRotStowCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCone); } else{ - extendArmPosition(Constants.JointRotationValues.JointRotStowCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCube); } break; @@ -567,18 +521,18 @@ public void jointRotPresetPositions(GlobalVariables.ArmPositions position){ case GROUND_SCORE_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.JointRotationValues.JointRotForLowCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForLowCone); } else{ - extendArmPosition(Constants.JointRotationValues.JointRotForLowCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForLowCube); } } else{ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.JointRotationValues.JointRotRevLowCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevLowCone); } else{ - extendArmPosition(Constants.JointRotationValues.JointRotRevLowCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevLowCube); } } @@ -588,18 +542,18 @@ public void jointRotPresetPositions(GlobalVariables.ArmPositions position){ case SHELF_PICKUP_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.JointRotationValues.JointRotForShelfCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForShelfCone); } else{ - extendArmPosition(Constants.JointRotationValues.JointRotForShelfCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForShelfCube); } } else{ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.JointRotationValues.JointRotRevShelfCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevShelfCone); } else{ - extendArmPosition(Constants.JointRotationValues.JointRotRevShelfCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevShelfCube); } } @@ -608,18 +562,18 @@ public void jointRotPresetPositions(GlobalVariables.ArmPositions position){ case MID_SCORE_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.JointRotationValues.JointRotForMidCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForMidCone); } else{ - extendArmPosition(Constants.JointRotationValues.JointRotForMidCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForMidCube); } } else{ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.JointRotationValues.JointRotRevMidCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevMidCone); } else{ - extendArmPosition(Constants.JointRotationValues.JointRotRevMidCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevMidCube); } } @@ -629,18 +583,18 @@ public void jointRotPresetPositions(GlobalVariables.ArmPositions position){ case HIGH_SCORE_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.JointRotationValues.JointRotForHighCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForHighCone); } else{ - extendArmPosition(Constants.JointRotationValues.JointRotForHighCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForHighCube); } } else{ if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.JointRotationValues.JointRotRevHighCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevHighCone); } else{ - extendArmPosition(Constants.JointRotationValues.JointRotRevHighCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevHighCube); } } @@ -651,10 +605,10 @@ public void jointRotPresetPositions(GlobalVariables.ArmPositions position){ default: if(GlobalVariables.gamePiece == 0){ - extendArmPosition(Constants.JointRotationValues.JointRotStowCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCone); } else{ - extendArmPosition(Constants.JointRotationValues.JointRotStowCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCube); } break; @@ -665,7 +619,7 @@ public void jointRotPresetPositions(GlobalVariables.ArmPositions position){ * Returns current arm position * @return CANcoder arm position in degrees. */ - public double getArmPosition(){ + public double getArmEncoderPosition(){ return testCanCoder.getAbsolutePosition(); } @@ -690,7 +644,7 @@ public double getArmMotorPos(){ return test; } - public double getJointPos(){ + public double getGripperJointPos(){ // return jointEncoder.getPosition(); return gripperJointFalcon.getSelectedSensorPosition(); } @@ -699,14 +653,14 @@ public double getJointPos(){ @Override public void periodic() { SmartDashboard.putNumber("PID OUTPUT ", leftArm.getMotorOutputPercent()); - SmartDashboard.putNumber("PID ERROR ", armPID.getPositionError()); - SmartDashboard.putNumber("Arm Encoder Position", getArmPosition() ); + SmartDashboard.putData("PID ERROR ", armPID); + SmartDashboard.putNumber("Arm Encoder Position", getArmEncoderPosition() ); SmartDashboard.putNumber("Arm Motor Position", getArmMotorPos()); SmartDashboard.putNumber("Arm Motor 1 Current", leftArm.getStatorCurrent()); SmartDashboard.putNumber("Arm Motor 2 Current", rightArm.getStatorCurrent()); SmartDashboard.putNumber("Telescope Position", telescopeArm.getSelectedSensorPosition()); SmartDashboard.putNumber("Telescope Speed?", telescopeArm.getSupplyCurrent()); - SmartDashboard.putNumber("Joint Position ", getJointPos() ); - SmartDashboard.putNumber("Joint Motor Current ", gripperJointNeo.getOutputCurrent()); + SmartDashboard.putNumber("Joint Position ", getGripperJointPos() ); + SmartDashboard.putNumber("Joint Speed ", gripperJointFalcon.getSelectedSensorVelocity()); } } diff --git a/src/main/java/frc/robot/subsystems/CANdleSub.java b/src/main/java/frc/robot/subsystems/CANdleSub.java index b445508..b25fb18 100644 --- a/src/main/java/frc/robot/subsystems/CANdleSub.java +++ b/src/main/java/frc/robot/subsystems/CANdleSub.java @@ -40,7 +40,7 @@ public void enabledLed(){ public void setColor(boolean cone) { m_candle.clearAnimation(0); if (cone) { - m_candle.setLEDs(252, 186, 3, 0, ledOffset, numLed); + m_candle.setLEDs(255, 45, 0, 0, ledOffset, numLed); return; } m_candle.setLEDs(111, 3, 252, 0, ledOffset, numLed); diff --git a/src/main/java/frc/robot/subsystems/Gripper.java b/src/main/java/frc/robot/subsystems/Gripper.java index 4893b24..ad4223e 100644 --- a/src/main/java/frc/robot/subsystems/Gripper.java +++ b/src/main/java/frc/robot/subsystems/Gripper.java @@ -19,13 +19,8 @@ public class Gripper extends SubsystemBase { public TalonFX gripperFalcon; - - private Rev2mDistanceSensor distanceSensor; -; - - /** Creates a new Gripper. */ public Gripper() { gripperFalcon = new TalonFX(35, "Karen"); @@ -42,17 +37,10 @@ public double getDistanceSensorDist(){ return distanceSensor.getRange(); } - - - public void moveGripper(double percent ){ gripperFalcon.set(TalonFXControlMode.PercentOutput, percent); } - - - - @Override public void periodic() { SmartDashboard.putNumber("Distance Sensor", getDistanceSensorDist()); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f7a1a25..28119e5 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -121,24 +121,24 @@ public void kyslol(){ public void updateOdometry() { - Optional result = pcw.getEstimatedGlobalPose(m_poseEstimator.getEstimatedPosition()); + // Optional result = pcw.getEstimatedGlobalPose(m_poseEstimator.getEstimatedPosition()); m_poseEstimator.update(getYaw(), new SwerveModulePosition[] { m_frontRight.getPosition(), m_frontLeft.getPosition(), m_backRight.getPosition(), m_backLeft.getPosition()}); - if (result.isPresent()) { + // if (result.isPresent()) { - EstimatedRobotPose camPose = result.get(); + // EstimatedRobotPose camPose = result.get(); - m_poseEstimator.addVisionMeasurement( - camPose.estimatedPose.toPose2d(), camPose.timestampSeconds); + // m_poseEstimator.addVisionMeasurement( + // camPose.estimatedPose.toPose2d(), camPose.timestampSeconds); // m_fieldSim.getObject("Cam Est Pos").setPose( m_poseEstimator.getEstimatedPosition() ); - } else { - // m_fieldSim.getObject("Cam Est Pos").setPose(m_poseEstimator.getEstimatedPosition()); - } + // } else { + // // m_fieldSim.getObject("Cam Est Pos").setPose(m_poseEstimator.getEstimatedPosition()); + // } m_poseEstimator.update(getYaw(), new SwerveModulePosition[] { m_frontRight.getPosition(), @@ -149,7 +149,7 @@ public void updateOdometry() { m_fieldSim.setRobotPose(getPose()); SmartDashboard.putNumber("lmao X", m_poseEstimator.getEstimatedPosition().getX()); SmartDashboard.putNumber("lmao Y", m_poseEstimator.getEstimatedPosition().getY()); - SmartDashboard.putBoolean("AprilTag in View", result.isPresent()); + // SmartDashboard.putBoolean("AprilTag in View", result.isPresent()); // m_fieldSim.setRobotPose(getPose()); // m_fieldSim.getObject("Actual Pos").setPose(getPose()); // m_fieldSim.setRobotPose(m_poseEstimator.getEstimatedPosition()); diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 0000000..69fddcd --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,35 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2023.4.3", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2023.4.3" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2023.4.3", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena" + ] + } + ] +} \ No newline at end of file From 989b199827fa9ad502a3c2d5ff427231053df59c Mon Sep 17 00:00:00 2001 From: Oscar Date: Wed, 5 Apr 2023 20:29:58 -0700 Subject: [PATCH 10/12] Arm Stuff Done(almost ready yay) --- src/main/deploy/pathplanner/FullAuto.path | 74 +++++++++---- src/main/deploy/pathplanner/Patha.path | 74 +++++++++++++ src/main/java/frc/robot/Constants.java | 75 ++++++------- .../java/frc/robot/PhotonCameraWrapper.java | 10 +- src/main/java/frc/robot/Robot.java | 2 + src/main/java/frc/robot/RobotContainer.java | 59 ++++++++-- .../robot/commands/AdaptiveArmMovement.java | 3 +- .../frc/robot/commands/AdaptiveOuttake.java | 46 ++++++++ .../commands/AutoAdaptiveArmMovement.java | 71 ++++++++++++ .../frc/robot/commands/AutoGroundIntake.java | 85 ++++++++++++++ .../robot/commands/AutoPlaceGamePiece.java | 89 +++++++++++++++ .../java/frc/robot/commands/ConeVCube.java | 6 +- .../java/frc/robot/commands/GroundIntake.java | 8 +- .../java/frc/robot/commands/LockToGrid.java | 104 ++++++++++++++++++ .../frc/robot/commands/ManualRollers.java | 46 ++++++++ .../java/frc/robot/commands/ShelfIntake.java | 77 +++++++++++++ .../commands/SingleSubstationIntake.java | 72 ++++++++++++ .../frc/robot/commands/UprightConeIntake.java | 60 ++++++++++ .../java/frc/robot/subsystems/ArmSub.java | 18 ++- .../java/frc/robot/subsystems/Swerve.java | 11 +- 20 files changed, 900 insertions(+), 90 deletions(-) create mode 100644 src/main/deploy/pathplanner/Patha.path create mode 100644 src/main/java/frc/robot/commands/AdaptiveOuttake.java create mode 100644 src/main/java/frc/robot/commands/AutoAdaptiveArmMovement.java create mode 100644 src/main/java/frc/robot/commands/AutoGroundIntake.java create mode 100644 src/main/java/frc/robot/commands/AutoPlaceGamePiece.java create mode 100644 src/main/java/frc/robot/commands/LockToGrid.java create mode 100644 src/main/java/frc/robot/commands/ManualRollers.java create mode 100644 src/main/java/frc/robot/commands/ShelfIntake.java create mode 100644 src/main/java/frc/robot/commands/SingleSubstationIntake.java create mode 100644 src/main/java/frc/robot/commands/UprightConeIntake.java diff --git a/src/main/deploy/pathplanner/FullAuto.path b/src/main/deploy/pathplanner/FullAuto.path index 9410d25..842e8d2 100644 --- a/src/main/deploy/pathplanner/FullAuto.path +++ b/src/main/deploy/pathplanner/FullAuto.path @@ -2,13 +2,13 @@ "waypoints": [ { "anchorPoint": { - "x": 2.0011332330356972, - "y": 4.156577202043294 + "x": 1.761330702148584, + "y": 5.138625661866708 }, "prevControl": null, "nextControl": { - "x": 3.0011332330356977, - "y": 4.156577202043294 + "x": 2.761330702148583, + "y": 5.138625661866708 }, "holonomicAngle": 0, "isReversal": false, @@ -16,45 +16,52 @@ "isLocked": false, "isStopPoint": false, "stopEvent": { - "names": [], - "executionBehavior": "parallel", + "names": [ + "switchcone", + "placehigh", + "stowarm", + "switchcube" + ], + "executionBehavior": "sequential", "waitBehavior": "none", "waitTime": 0 } }, { "anchorPoint": { - "x": 5.1985003115305375, - "y": 6.760147537389092 + "x": 6.831441355190402, + "y": 4.613343927542556 }, "prevControl": { - "x": 5.1985003115305375, - "y": 5.760147537389092 + "x": 5.015793621330831, + "y": 4.601924759405074 }, "nextControl": { - "x": 5.1985003115305375, - "y": 5.760147537389092 + "x": 5.015793621330831, + "y": 4.601924759405074 }, - "holonomicAngle": 0, + "holonomicAngle": -3.8140748342904334, "isReversal": true, "velOverride": null, "isLocked": false, - "isStopPoint": false, + "isStopPoint": true, "stopEvent": { - "names": [], - "executionBehavior": "parallel", + "names": [ + "stowarm" + ], + "executionBehavior": "sequential", "waitBehavior": "none", "waitTime": 0 } }, { "anchorPoint": { - "x": 6.60305799244077, - "y": 4.681858936367445 + "x": 1.6813965251862129, + "y": 4.56766725499263 }, "prevControl": { - "x": 5.60305799244077, - "y": 4.681858936367445 + "x": 2.2180974276478453, + "y": 4.56766725499263 }, "nextControl": null, "holonomicAngle": 0, @@ -63,12 +70,33 @@ "isLocked": false, "isStopPoint": false, "stopEvent": { - "names": [], - "executionBehavior": "parallel", + "names": [ + "placemid" + ], + "executionBehavior": "sequential", "waitBehavior": "none", "waitTime": 0 } } ], - "markers": [] + "markers": [ + { + "position": 0.7563636363635975, + "names": [ + "cubepickup" + ] + }, + { + "position": 1.01, + "names": [ + "holdstow" + ] + }, + { + "position": 0.01, + "names": [ + "holdstow" + ] + } + ] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Patha.path b/src/main/deploy/pathplanner/Patha.path new file mode 100644 index 0000000..74c48a8 --- /dev/null +++ b/src/main/deploy/pathplanner/Patha.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c650934..e35ea2c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -43,11 +43,11 @@ public static final class ArmRotationValues { public static final double armRotStow = 5; - public static final double armRotForHighCone = 50; + public static final double armRotForHighCone = 70; public static final double armRotForHighCube = 50; - public static final double armRotForMidCone = 50; - public static final double armRotForMidCube = 50; + public static final double armRotForMidCone = 65; + public static final double armRotForMidCube = 40; public static final double armRotForLowCone = 5; public static final double armRotForLowCube = 5; @@ -55,22 +55,22 @@ public static final class ArmRotationValues { public static final double armRotForConePickup = 3; public static final double armRotForCubePickup = 3; - public static final double armRotForShelfCone = 70; - public static final double armRotForShelfCube = 70; + public static final double armRotForShelfCone = 120; + public static final double armRotForShelfCube = 110; public static final double armRotRevHighCone = 138; - public static final double armRotRevHighCube = 130; + public static final double armRotRevHighCube = 145; - public static final double armRotRevMidCone = 130; - public static final double armRotRevMidCube = 130; + public static final double armRotRevMidCone = 135; + public static final double armRotRevMidCube = 150; - public static final double armRotRevLowCone = 5; + public static final double armRotRevLowCone = 30; public static final double armRotRevLowCube = 90; public static final double armRotRevConePickup = 180; public static final double armRotRevCubePickup = 170; - public static final double armRotRevShelfCone = 120; + public static final double armRotRevShelfCone = 81; public static final double armRotRevShelfCube = 120; } @@ -82,11 +82,11 @@ public static final class ArmExtendValues { public static final double armExtendCubePickup = 7000; - public static final double armExtendForHighCone = 50000; - public static final double armExtendForHighCube = 50000; + public static final double armExtendForHighCone = 48000; + public static final double armExtendForHighCube = 48000; - public static final double armExtendForMidCone = 30000; - public static final double armExtendForMidCube = 30000; + public static final double armExtendForMidCone = 35000; + public static final double armExtendForMidCube = 20000; public static final double armExtendForLowCone = 0; public static final double armExtendForLowCube = 0; @@ -94,21 +94,21 @@ public static final class ArmExtendValues { public static final double armExtendForConePickup = 7000; public static final double armExtendForCubePickup = 7000; - public static final double armExtendForShelfCone = 50000; - public static final double armExtendForShelfCube = 50000; + public static final double armExtendForShelfCone = 45000; + public static final double armExtendForShelfCube = 45000; - public static final double armExtendRevHighCone = 50000; - public static final double armExtendRevHighCube = 50000; + public static final double armExtendRevHighCone = 48000; + public static final double armExtendRevHighCube = 35000; public static final double armExtendRevMidCone = 30000; - public static final double armExtendRevMidCube = 30000; + public static final double armExtendRevMidCube = 0; public static final double armExtendRevLowCone = 0; public static final double armExtendRevLowCube = 0; - public static final double armExtendRevShelfCone = 50000; - public static final double armExtendRevShelfCube = 50000; + public static final double armExtendRevShelfCone = 48000; + public static final double armExtendRevShelfCube = 48000; } public static final class JointRotationValues { @@ -120,31 +120,32 @@ public static final class JointRotationValues { public static final double JointRotForHighCone = 16000; - public static final double JointRotForHighCube = 9000; + public static final double JointRotForHighCube = 17000; - public static final double JointRotForMidCone = 16000; - public static final double JointRotForMidCube = 9000; + public static final double JointRotForMidCone = 45000; + public static final double JointRotForMidCube = 17000 + ; - public static final double JointRotForLowCone = 9000; - public static final double JointRotForLowCube = 4000; + public static final double JointRotForLowCone = 17000; + public static final double JointRotForLowCube = 6000; public static final double JointRotForConePickup = 11000; public static final double JointRotForCubePickup = 9000; - public static final double JointRotForShelfCone = 16000; + public static final double JointRotForShelfCone = 2000; public static final double JointRotForShelfCube = 9000; - public static final double JointRotRevHighCone = 6600; + public static final double JointRotRevHighCone = 13500; public static final double JointRotRevHighCube = 4000; - public static final double JointRotRevMidCone = 11000; + public static final double JointRotRevMidCone = 6500; public static final double JointRotRevMidCube = 4000; - public static final double JointRotRevLowCone = 0; + public static final double JointRotRevLowCone = 2000; public static final double JointRotRevLowCube = 0; - public static final double JointRotRevShelfCone = 4000; + public static final double JointRotRevShelfCone = 48000; public static final double JointRotRevShelfCube = 0; } @@ -222,7 +223,7 @@ public static final class Mod0 { public static final int driveMotorID = /* 2 */5; public static final int angleMotorID = /* 1 */4; public static final int canCoderID = /* 3 */6; - public static final double angleOffset = /* 274; */ 13; + public static final double angleOffset = /* 274; */ 15; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -232,7 +233,7 @@ public static final class Mod1 { public static final int driveMotorID = /* 11 */11; public static final int angleMotorID = /* 10 */10; public static final int canCoderID = /* 12 */12; - public static final double angleOffset = /* 240 ; */ 218; + public static final double angleOffset = /* 240 ; */ 218.5; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -242,7 +243,7 @@ public static final class Mod2 { public static final int driveMotorID = /* 5 */2; public static final int angleMotorID = /* 4 */1; public static final int canCoderID = /* 6 */3; - public static final double angleOffset = /* 319; */ 310; + public static final double angleOffset = /* 319; */ 311; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -252,7 +253,7 @@ public static final class Mod3 { public static final int driveMotorID = /* 8 */8; public static final int angleMotorID = /* 7 */7; public static final int canCoderID = /* 9 */9; - public static final double angleOffset = /* 305 ; */ 288; + public static final double angleOffset = /* 305 ; */ 286; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -342,8 +343,8 @@ static class VisionConstants { new Rotation3d( 0, 0, Math.toRadians(45))); - static final String camera1Name = "OV9281-01"; - static final String camera2Name = "Cam_2"; + static final String camera1Name = "RearCam"; + static final String camera2Name = "FrontCam"; static final String frontCamName = "HD_USB_Camera"; } diff --git a/src/main/java/frc/robot/PhotonCameraWrapper.java b/src/main/java/frc/robot/PhotonCameraWrapper.java index 0ab2f7a..4fe8956 100644 --- a/src/main/java/frc/robot/PhotonCameraWrapper.java +++ b/src/main/java/frc/robot/PhotonCameraWrapper.java @@ -50,7 +50,7 @@ public double getFrontCamOffset(){ var result = frontCam.getLatestResult(); if (!result.hasTargets()) - return 0; + return 9; return result.getBestTarget().getYaw(); @@ -60,12 +60,14 @@ public double getFrontCamOffset(){ public double getRearCamOffset(){ var result = frontCam.getLatestResult(); - if (!result.hasTargets()) - return 0; + if (!result.hasTargets()){ + return 9; + } + else{ return result.getBestTarget().getYaw(); - + } } public double getFrontCamDistance(){ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2601180..57cca93 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -117,6 +117,8 @@ public void autonomousPeriodic() { @Override public void teleopInit() { + m_robotContainer.armSub.ArmBrakeMode(NeutralMode.Brake); + // m_robotContainer.s_Swerve.normalizeOdometry(); // m_robotContainer.s_Swerve.autoZeroGyro(); m_robotContainer.s_Swerve.zeroGyro(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2ba9bb5..04aa766 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -57,7 +57,8 @@ public class RobotContainer { private final int rotationAxis = 2; /* Driver Buttons */ - + private final Trigger leftJoy = new JoystickButton(joystickPanel, 1); + private final Trigger rightJoy = new JoystickButton(joystickPanel, 2); public final Trigger operatorDeploy = new JoystickButton(operatorPanel, 11); @@ -70,6 +71,9 @@ public class RobotContainer { private final Trigger panelStow = new JoystickButton(operatorPanel, 5); private final Trigger panelGround = new JoystickButton(operatorPanel, 16); private final Trigger panelGPSwitch = new JoystickButton(joystickPanel, 17); + private final Trigger panelRelease = new JoystickButton(operatorPanel, 15); + private final Trigger panelEmptyRight = new JoystickButton(operatorPanel, 12); + private final Trigger panelEmptyLeft = new JoystickButton(operatorPanel, 14); @@ -101,6 +105,21 @@ public RobotContainer() { operatorPanel.setOutput(10, true ); operatorPanel.setOutput(13, true ); operatorPanel.setOutput(16, true ); + operatorPanel.setOutput(19, true ); + + operatorPanel.setOutput(3, true ); + + operatorPanel.setOutput(4, true ); + operatorPanel.setOutput(7, true ); + operatorPanel.setOutput(8, true ); + operatorPanel.setOutput(9, true ); + operatorPanel.setOutput(11, true ); + operatorPanel.setOutput(12, true ); + operatorPanel.setOutput(14, true ); + operatorPanel.setOutput(15, true ); + operatorPanel.setOutput(17, true ); + operatorPanel.setOutput(18, true ); + // m_chooser.setDefaultOption("Balance Blue", balanceBlue); // m_chooser.addOption("Balance Red", balanceRed); @@ -129,12 +148,22 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - panelRollers.whileTrue(new GripperCommand(gripper, 0.7)); //Push OUT - panelShelf.whileTrue(new GripperCommand(gripper, -1)); // Take IN - panelStow.onTrue(new InstantCommand(() -> armSub.homeGripperJointPos())); + // panelRollers.whileTrue(new GripperCommand(gripper, 0.7)); //Push OUT + // panelShelf.whileTrue(new GripperCommand(gripper, -1)); // Take IN + // panelStow.onTrue(new InstantCommand(() -> armSub.homeGripperJointPos())); + + panelHigh.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.HIGH_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelMid.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelLow.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.GROUND_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelStow.onTrue(new AutoPlaceGamePiece(armSub, gripper, ArmPositions.HIGH_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelGround.whileTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelShelf.whileTrue(new ShelfIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelRelease.whileTrue(new AdaptiveOuttake(gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelRollers.whileTrue(new ManualRollers(gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelEmptyRight.whileTrue(new UprightConeIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelEmptyLeft.whileTrue(new SingleSubstationIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // panelStow.whileTrue(new FullArmPosition(armSub, 150, 50000, 9000, true)); // panelStow.whileFalse(new FullArmPosition(armSub, 0, 0, 0, false)); @@ -161,8 +190,7 @@ private void configureButtonBindings() { // panelStow.whileTrue(new HoldJointPos(armSub, 15).alongWith(new ArmExtendPositionTest(armSub, 7000)).alongWith(new ArmRotationTest(armSub, 0))); // panelStow.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - panelGround.whileTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - panelGround.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // panelGround.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); // panelStow.whileTrue(new HoldJointPos(armSub, 11000)); // panelStow.whileFalse(new HoldJointPos(armSub, 0)); @@ -184,7 +212,7 @@ private void configureButtonBindings() { // operatorHook.whileTrue(new ArmExtendTest(armSub)); rightJoy.onTrue(new InstantCommand(() -> s_Swerve.zeroGyro())); // leftJoy.whileTrue(new LockToGamePiece(s_Swerve, joystickPanel, translationAxis, strafeAxis, true, true, GlobalVariables.gamePiece).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - + leftJoy.whileTrue(new LockToGrid(s_Swerve, joystickPanel, translationAxis, strafeAxis, rotationAxis, true, true)); // buttonX.toggleOnTrue(new HoldArmPos(armSub, ArmPositions.STOWED).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); // buttonY.whileTrue(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); // buttonB.whileTrue(new HoldArmPos(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); @@ -238,12 +266,25 @@ private void configureButtonBindings() { */ public Command getAutonomousCommand() { - List pathGroup = PathPlanner.loadPathGroup("FullAuto", PathPlanner.getConstraintsFromPath("FullAuto")); + List pathGroup = PathPlanner.loadPathGroup("FullAuto", new PathConstraints(1, 1)); // This is just an example event map. It would be better to have a constant, global event map // in your code that will be used by all path following commands. HashMap eventMap = new HashMap<>(); eventMap.put("marker1", new PrintCommand("Passed marker 1")); +eventMap.put("goarm", new AdaptiveArmMovement(armSub, ArmPositions.HIGH_SCORE_ADAPTIVE)); +eventMap.put("stowarm", new AutoAdaptiveArmMovement(armSub, ArmPositions.STOWED_ADAPTIVE)); +eventMap.put("placehigh", new AutoPlaceGamePiece(armSub, gripper, ArmPositions.HIGH_SCORE_ADAPTIVE)); +eventMap.put("switchcone", new ConeVCube(0)); +eventMap.put("switchcube", new ConeVCube(1)); +eventMap.put("cubepickup", new AutoGroundIntake(armSub, gripper, 1) ); +eventMap.put("conepickup", new AutoGroundIntake(armSub, gripper, 0) ); +eventMap.put("placemid", new AutoPlaceGamePiece(armSub, gripper, ArmPositions.MID_SCORE_ADAPTIVE)); +eventMap.put("holdstow", new AdaptiveArmMovement(armSub, ArmPositions.STOWED_ADAPTIVE)); + + + + // Create the AutoBuilder. This only needs to be created once when robot code starts, not every time you want to create an auto command. A good place to put this is in RobotContainer along with your subsystems. SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder( @@ -254,7 +295,7 @@ public Command getAutonomousCommand() { new PIDConstants(0.5, 0.0, 0.0), // PID constants to correct for rotation error (used to create the rotation controller) s_Swerve::setModuleStates, // Module states consumer used to output to the drive subsystem eventMap, - true, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true + false, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true s_Swerve // The drive subsystem. Used to properly set the requirements of path following commands ); diff --git a/src/main/java/frc/robot/commands/AdaptiveArmMovement.java b/src/main/java/frc/robot/commands/AdaptiveArmMovement.java index 42cbeb9..bb42e26 100644 --- a/src/main/java/frc/robot/commands/AdaptiveArmMovement.java +++ b/src/main/java/frc/robot/commands/AdaptiveArmMovement.java @@ -29,9 +29,10 @@ public void initialize() {} public void execute() { if(armPosition == ArmPositions.STOWED_ADAPTIVE){ armSub.armExtendPresetPositions(armPosition); + armSub.jointRotPresetPositions(armPosition); + if(armSub.getTelescopePos() <= 10000){ armSub.armRotPresetPositions(armPosition); - armSub.jointRotPresetPositions(armPosition); } } diff --git a/src/main/java/frc/robot/commands/AdaptiveOuttake.java b/src/main/java/frc/robot/commands/AdaptiveOuttake.java new file mode 100644 index 0000000..92a7a75 --- /dev/null +++ b/src/main/java/frc/robot/commands/AdaptiveOuttake.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.subsystems.Gripper; + +public class AdaptiveOuttake extends CommandBase { + /** Creates a new AdaptiveOuttake. */ + Gripper gripper; + public AdaptiveOuttake(Gripper gripper) { + this.gripper = gripper; + addRequirements(gripper); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(GlobalVariables.gamePiece == 0){ + gripper.moveGripper(1); + } + else{ + gripper.moveGripper(-1); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + gripper.moveGripper(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/AutoAdaptiveArmMovement.java b/src/main/java/frc/robot/commands/AutoAdaptiveArmMovement.java new file mode 100644 index 0000000..a9eb1b8 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoAdaptiveArmMovement.java @@ -0,0 +1,71 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.GlobalVariables.ArmPositions; +import frc.robot.subsystems.ArmSub; + +public class AutoAdaptiveArmMovement extends CommandBase { + /** Creates a new AdaptiveArmMovement. */ + ArmSub armSub; + GlobalVariables.ArmPositions armPosition; + boolean finished; + public AutoAdaptiveArmMovement(ArmSub armSub, GlobalVariables.ArmPositions armPosition) { + this.armSub = armSub; + this.armPosition = armPosition; + addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + finished = false; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(armPosition == ArmPositions.STOWED_ADAPTIVE){ + armSub.armExtendPresetPositions(armPosition); + if(armSub.getTelescopePos() <= 10000){ + armSub.armRotPresetPositions(armPosition); + armSub.jointRotPresetPositions(armPosition); + if(armSub.getArmEncoderPosition() >= (GlobalVariables.armRotGoal - 5) && + armSub.getArmEncoderPosition() <= (GlobalVariables.armRotGoal + 5) ){ + finished = true; + } + + } + } + else{ + + armSub.armRotPresetPositions(armPosition); + armSub.jointRotPresetPositions(armPosition); + + if(armSub.getArmEncoderPosition() >= GlobalVariables.armRotGoal*0.75){ + armSub.armExtendPresetPositions(armPosition); + if(armSub.getTelescopePos() >= (GlobalVariables.armExtendGoal - 2000) && + armSub.getTelescopePos() <= (GlobalVariables.armExtendGoal + 2000) ){ + finished = true; + } + } + + } + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return finished; + } +} diff --git a/src/main/java/frc/robot/commands/AutoGroundIntake.java b/src/main/java/frc/robot/commands/AutoGroundIntake.java new file mode 100644 index 0000000..da9643c --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoGroundIntake.java @@ -0,0 +1,85 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.subsystems.ArmSub; +import frc.robot.subsystems.Gripper; + +public class AutoGroundIntake extends CommandBase { + /** Creates a new GroundIntake. */ + ArmSub armSub; + Gripper gripper; + int gamePiece; + boolean finished; + /** + * For gamepiece cone = 0, and cube = 1 + * @param armSub + * @param gripper + * @param gamePiece + */ + public AutoGroundIntake(ArmSub armSub, Gripper gripper, int gamePiece) { + this.armSub = armSub; + this.gripper = gripper; + this.gamePiece = gamePiece; + addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + finished = false; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(gamePiece == 0){ + armSub.moveGripperJointPosition(33000); + armSub.moveTelescopeArmPosition(8000); + armSub.moveRotArmPosition(1); + if((gripper.getDistanceSensorDist() <= 12)){ + gripper.moveGripper(0); + finished = true; + + } + else{ + gripper.moveGripper(-1); + + } + } + + else{ + armSub.moveGripperJointPosition(24000); + armSub.moveTelescopeArmPosition(8000); + armSub.moveRotArmPosition(1); + if((gripper.getDistanceSensorDist() <= 12)){ + gripper.moveGripper(0); + finished = true; + + } + else{ + gripper.moveGripper(0.8); + + } + } + + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + gripper.moveGripper(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return finished; + } +} diff --git a/src/main/java/frc/robot/commands/AutoPlaceGamePiece.java b/src/main/java/frc/robot/commands/AutoPlaceGamePiece.java new file mode 100644 index 0000000..2c2e517 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoPlaceGamePiece.java @@ -0,0 +1,89 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.GlobalVariables.ArmPositions; +import frc.robot.subsystems.ArmSub; +import frc.robot.subsystems.Gripper; + +public class AutoPlaceGamePiece extends CommandBase { + /** Creates a new AdaptiveArmMovement. */ + ArmSub armSub; + Gripper gripper; + GlobalVariables.ArmPositions armPosition; + boolean finished; + public AutoPlaceGamePiece(ArmSub armSub, Gripper gripper, GlobalVariables.ArmPositions armPosition) { + this.armSub = armSub; + this.armPosition = armPosition; + this.gripper = gripper; + addRequirements(armSub, gripper); + finished = false; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + finished = false; + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(armPosition == ArmPositions.STOWED_ADAPTIVE){ + armSub.armExtendPresetPositions(armPosition); + if(armSub.getTelescopePos() <= 10000){ + armSub.armRotPresetPositions(armPosition); + armSub.jointRotPresetPositions(armPosition); + + } + } + else{ + + armSub.armRotPresetPositions(armPosition); + armSub.jointRotPresetPositions(armPosition); + + if(armSub.getArmEncoderPosition() >= GlobalVariables.armRotGoal*0.75){ + armSub.armExtendPresetPositions(armPosition); + if(armSub.getTelescopePos()>= (GlobalVariables.armExtendGoal -1500) && + armSub.getTelescopePos()<= (GlobalVariables.armExtendGoal + 1500)){ + if(gripper.getDistanceSensorDist() <= 12){ + if(GlobalVariables.gamePiece == 0){ + gripper.moveGripper(0.7); + } + else{ + gripper.moveGripper(-1); + + } + } + else{ + gripper.moveGripper(0); + finished = true; + } + } + else{ + finished = false; + } + } + + + } + + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return finished; + } +} diff --git a/src/main/java/frc/robot/commands/ConeVCube.java b/src/main/java/frc/robot/commands/ConeVCube.java index 6b41217..f3ccd6e 100644 --- a/src/main/java/frc/robot/commands/ConeVCube.java +++ b/src/main/java/frc/robot/commands/ConeVCube.java @@ -10,6 +10,7 @@ public class ConeVCube extends CommandBase { /** Creates a new ConeVCube. */ int gamePiece; + boolean finished; public ConeVCube(int gamePiece) { this.gamePiece = gamePiece; @@ -19,6 +20,7 @@ public ConeVCube(int gamePiece) { // Called when the command is initially scheduled. @Override public void initialize() { + finished = false; } // Called every time the scheduler runs while the command is scheduled. @@ -27,6 +29,8 @@ public void execute() { GlobalVariables.gamePiece = gamePiece; System.out.print(GlobalVariables.gamePiece); + finished=true; + } // Called once the command ends or is interrupted. @@ -36,6 +40,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return finished; } } diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index 6f2d27d..16d4921 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -28,10 +28,10 @@ public void initialize() {} @Override public void execute() { if(GlobalVariables.gamePiece == 0){ - armSub.moveGripperJointPosition(13000); + armSub.moveGripperJointPosition(33000); armSub.moveTelescopeArmPosition(8000); armSub.moveRotArmPosition(1); - if((gripper.getDistanceSensorDist() <= 13)){ + if((gripper.getDistanceSensorDist() <= 12)){ gripper.moveGripper(0); } @@ -42,10 +42,10 @@ public void execute() { } else{ - armSub.moveGripperJointPosition(8000); + armSub.moveGripperJointPosition(24000); armSub.moveTelescopeArmPosition(8000); armSub.moveRotArmPosition(1); - if((gripper.getDistanceSensorDist() <= 13)){ + if((gripper.getDistanceSensorDist() <= 12)){ gripper.moveGripper(0); } diff --git a/src/main/java/frc/robot/commands/LockToGrid.java b/src/main/java/frc/robot/commands/LockToGrid.java new file mode 100644 index 0000000..90259e7 --- /dev/null +++ b/src/main/java/frc/robot/commands/LockToGrid.java @@ -0,0 +1,104 @@ +package frc.robot.commands; + +import frc.robot.Constants; +import frc.robot.GlobalVariables; +import frc.robot.subsystems.Swerve; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; + + +public class LockToGrid extends CommandBase { + + private double rotation; + private Translation2d translation; + private boolean fieldRelative; + private boolean openLoop; + + private Swerve s_Swerve; + private Joystick controller; + private int translationAxis; + private int strafeAxis; + private int rotationAxis; + private double goal; + PIDController pidController; + + + + private double mapdouble(double x, double in_min, double in_max, double out_min, double out_max){ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; + } + + /** + * Driver control + */ + public LockToGrid(Swerve s_Swerve, Joystick controller, int translationAxis, int strafeAxis, int rotationAxis, boolean fieldRelative, boolean openLoop) { + this.s_Swerve = s_Swerve; + addRequirements(s_Swerve); + + this.controller = controller; + this.translationAxis = translationAxis; + this.strafeAxis = strafeAxis; + this.rotationAxis = rotationAxis; + this.fieldRelative = fieldRelative; + this.openLoop = openLoop; + pidController = new PIDController(0.017 , 0.00, 0); + pidController.setTolerance(1); + pidController.enableContinuousInput(-180, 180); + + + } + + @Override + public void execute() { + if(GlobalVariables.robotDirection){ + goal = 180; + } + else{ + goal = 0; + } + double yAxis = -controller.getRawAxis(translationAxis); + double xAxis = controller.getRawAxis(strafeAxis); /* -s_Swerve.getCamOffset() */; + double rAxis = pidController.calculate(s_Swerve.getYaw().getDegrees(), goal); + + /* Deadbands */ + + if (Math.abs(yAxis) > Constants.stickDeadband) { + if (yAxis > 0){ + yAxis = mapdouble(yAxis, Constants.stickDeadband, 1, 0, 1); + } else { + yAxis = mapdouble(yAxis, -Constants.stickDeadband, -1, 0, -1); + } + } + else{ + yAxis = 0; + } + + if (Math.abs(xAxis) > Constants.stickDeadband) { + if (xAxis > 0){ + xAxis = mapdouble(xAxis, Constants.stickDeadband, 1, 0, 1); + } else { + xAxis = mapdouble(xAxis, -Constants.stickDeadband, -1, 0, -1); + } + } + else{ + xAxis = 0; + } + + // if (Math.abs(rAxis) > Constants.stickDeadband) { + // if (rAxis > 0){ + // rAxis = mapdouble(rAxis, Constants.stickDeadband, 1, 0, 1); + // } else { + // rAxis = mapdouble(rAxis, -Constants.stickDeadband, -1, 0, -1); + // } + // } + // else{ + // rAxis = 0; + // } + + translation = new Translation2d(yAxis, xAxis).times(Constants.Swerve.maxSpeed); + rotation = rAxis * Constants.Swerve.maxAngularVelocity; + s_Swerve.drive(translation, rotation, fieldRelative, openLoop); + } +} diff --git a/src/main/java/frc/robot/commands/ManualRollers.java b/src/main/java/frc/robot/commands/ManualRollers.java new file mode 100644 index 0000000..d340f3e --- /dev/null +++ b/src/main/java/frc/robot/commands/ManualRollers.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.subsystems.Gripper; + +public class ManualRollers extends CommandBase { + /** Creates a new AdaptiveOuttake. */ + Gripper gripper; + public ManualRollers(Gripper gripper) { + this.gripper = gripper; + addRequirements(gripper); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(GlobalVariables.gamePiece == 0){ + gripper.moveGripper(-1); + } + else{ + gripper.moveGripper(1); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + gripper.moveGripper(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ShelfIntake.java b/src/main/java/frc/robot/commands/ShelfIntake.java new file mode 100644 index 0000000..5fafe62 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShelfIntake.java @@ -0,0 +1,77 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.GlobalVariables.ArmPositions; +import frc.robot.subsystems.ArmSub; +import frc.robot.subsystems.Gripper; + +public class ShelfIntake extends CommandBase { + /** Creates a new AdaptiveArmMovement. */ + ArmSub armSub; + Gripper gripper; + public ShelfIntake(ArmSub armSub, Gripper gripper) { + this.armSub = armSub; + this.gripper = gripper; + addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + + armSub.armRotPresetPositions(ArmPositions.SHELF_PICKUP_ADAPTIVE); + armSub.jointRotPresetPositions(ArmPositions.SHELF_PICKUP_ADAPTIVE); + + if(armSub.getArmEncoderPosition() >= GlobalVariables.armRotGoal*0.75){ + armSub.armExtendPresetPositions(ArmPositions.SHELF_PICKUP_ADAPTIVE); + } + + if(GlobalVariables.gamePiece == 0){ + if((gripper.getDistanceSensorDist() <= 12)){ + gripper.moveGripper(0); + + } + else{ + gripper.moveGripper(-1); + + } + } + else{ + if((gripper.getDistanceSensorDist() <= 12)){ + gripper.moveGripper(0); + + } + else{ + gripper.moveGripper(0.8); + + } + } + + + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + gripper.moveGripper(0); + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/SingleSubstationIntake.java b/src/main/java/frc/robot/commands/SingleSubstationIntake.java new file mode 100644 index 0000000..82f40fc --- /dev/null +++ b/src/main/java/frc/robot/commands/SingleSubstationIntake.java @@ -0,0 +1,72 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.subsystems.ArmSub; +import frc.robot.subsystems.Gripper; + +public class SingleSubstationIntake extends CommandBase { + /** Creates a new GroundIntake. */ + ArmSub armSub; + Gripper gripper; + public SingleSubstationIntake(ArmSub armSub, Gripper gripper) { + this.armSub = armSub; + this.gripper = gripper; + addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(GlobalVariables.gamePiece == 0){ + armSub.moveGripperJointPosition(27000); + armSub.moveTelescopeArmPosition(0); + armSub.moveRotArmPosition(40); + if((gripper.getDistanceSensorDist() <= 12)){ + gripper.moveGripper(0); + + } + else{ + gripper.moveGripper(-1); + + } + } + + else{ + armSub.moveGripperJointPosition(15000); + armSub.moveTelescopeArmPosition(0); + armSub.moveRotArmPosition(40); + if((gripper.getDistanceSensorDist() <= 12)){ + gripper.moveGripper(0); + + } + else{ + gripper.moveGripper(0.8); + + } + } + + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + gripper.moveGripper(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/UprightConeIntake.java b/src/main/java/frc/robot/commands/UprightConeIntake.java new file mode 100644 index 0000000..9d62aa9 --- /dev/null +++ b/src/main/java/frc/robot/commands/UprightConeIntake.java @@ -0,0 +1,60 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.subsystems.ArmSub; +import frc.robot.subsystems.Gripper; + +public class UprightConeIntake extends CommandBase { + /** Creates a new GroundIntake. */ + ArmSub armSub; + Gripper gripper; + public UprightConeIntake(ArmSub armSub, Gripper gripper) { + this.armSub = armSub; + this.gripper = gripper; + addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + armSub.moveGripperJointPosition(48000); + armSub.moveTelescopeArmPosition(0); + armSub.moveRotArmPosition(42); + + if((gripper.getDistanceSensorDist() <= 12)){ + gripper.moveGripper(0); + + } + else{ + gripper.moveGripper(-1); + + } + + + + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + gripper.moveGripper(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/ArmSub.java b/src/main/java/frc/robot/subsystems/ArmSub.java index b4cf672..19cfae8 100644 --- a/src/main/java/frc/robot/subsystems/ArmSub.java +++ b/src/main/java/frc/robot/subsystems/ArmSub.java @@ -56,7 +56,7 @@ public ArmSub() { testCanCoder.configMagnetOffset(-12); - rotMaxSpeedFor = 0.3; + rotMaxSpeedFor = 0.5; rotMaxSpeedRev = 0.3; limit = new SupplyCurrentLimitConfiguration(true, 10, 10, 0); @@ -109,7 +109,7 @@ public ArmSub() { telescopeArm.configPeakOutputReverse(-telMaxSpeedRev); telescopeArm.setSensorPhase(true); telescopeArm.setInverted(InvertType.None); - telescopeArm.config_kP(0, 0.3); + telescopeArm.config_kP(0, 0.2); telescopeArm.config_kI(0, 0.0); telescopeArm.config_kD(0, 0.0); telescopeArm.config_kF(0, 0.0); @@ -125,14 +125,14 @@ public ArmSub() { gripperJointFalcon.configPeakOutputReverse(-telMaxSpeedRev); gripperJointFalcon.setSensorPhase(true); gripperJointFalcon.setInverted(InvertType.InvertMotorOutput); - gripperJointFalcon.config_kP(0, 0.3); + gripperJointFalcon.config_kP(0, 0.4); gripperJointFalcon.config_kI(0, 0.0); gripperJointFalcon.config_kD(0, 0.0); gripperJointFalcon.config_kF(0, 0.0); gripperJointFalcon.setNeutralMode(NeutralMode.Brake); gripperJointFalcon.configNeutralDeadband(0.001); - gripperJointFalcon.configMotionAcceleration(5000); - gripperJointFalcon.configMotionCruiseVelocity(10000); + gripperJointFalcon.configMotionAcceleration(15000); + gripperJointFalcon.configMotionCruiseVelocity(15000); gripperJointFalcon.setSelectedSensorPosition(0); gripperJointFalcon.configAllowableClosedloopError(0, 10); @@ -467,17 +467,25 @@ public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForHighCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForHighCone; + } else{ moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForHighCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForHighCube; + } } else{ if(GlobalVariables.gamePiece == 0){ moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevHighCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevHighCone; + } else{ moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevHighCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevHighCube; + } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 28119e5..c112f6d 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -1,8 +1,6 @@ package frc.robot.subsystems; -import java.util.Optional; -import org.photonvision.EstimatedRobotPose; import com.kauailabs.navx.frc.AHRS; import frc.robot.SwerveModule; @@ -13,7 +11,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -80,8 +77,8 @@ public Swerve() { // zeroGyro(); SmartDashboard.putData("Field", m_fieldSim); - aimPID = new PIDController(1, 0, 0); - aimPID.setTolerance(0.001); + aimPID = new PIDController(0.05, 0, 0); + aimPID.setTolerance(0.5); rotatePID = new ProfiledPIDController( 0.010, @@ -285,7 +282,7 @@ public double getCamOffset(){ } else{ headingError = pcw.getRearCamOffset(); - return aimPID.calculate(headingError, -30); + return aimPID.calculate(headingError, 9); } @@ -341,6 +338,7 @@ public double getCamOffset(){ public void periodic(){ updateRobotDirection(); // pcw.frontCamPipeline(2); + pcw.cameraPipelines(GlobalVariables.gamePiece); AprilTagGrid.setDefaultOption("Left Tag", gridTag1); AprilTagGrid.addOption("Mid Tag", gridTag2); @@ -369,6 +367,7 @@ public void periodic(){ SmartDashboard.putNumber("Gyro Yaw ", gyro.getYaw()); SmartDashboard.putNumber("AprilTag Yaw ", getAprilTagEstPosition().getRotation().getDegrees()); SmartDashboard.putBoolean("robot direction ", GlobalVariables.robotDirection); + SmartDashboard.putNumber("Rear Cam offset ", pcw.getRearCamOffset()); kyslol(); From e83fe8f8be99909cbaab0b8acde4a4616cf09c41 Mon Sep 17 00:00:00 2001 From: Oscar Date: Fri, 7 Apr 2023 20:32:33 -0700 Subject: [PATCH 11/12] Updated aUTO --- .pathplanner/settings.json | 4 +- src/main/deploy/pathplanner/FullAuto.path | 136 ++++++++++--- src/main/deploy/pathplanner/New Path.path | 74 +++++++ src/main/deploy/pathplanner/ThreePiece.path | 183 ++++++++++++++++++ src/main/java/frc/robot/Constants.java | 38 ++-- src/main/java/frc/robot/GlobalVariables.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 20 +- .../robot/commands/AutoBalanceStation.java | 7 +- .../frc/robot/commands/AutoGroundIntake.java | 16 +- .../robot/commands/AutoPlaceGamePiece.java | 21 +- .../java/frc/robot/commands/GroundIntake.java | 2 +- .../commands/SingleSubstationIntake.java | 4 +- .../frc/robot/commands/UprightConeIntake.java | 4 +- .../java/frc/robot/subsystems/ArmSub.java | 24 +-- .../java/frc/robot/subsystems/Gripper.java | 13 ++ 15 files changed, 467 insertions(+), 81 deletions(-) create mode 100644 src/main/deploy/pathplanner/New Path.path create mode 100644 src/main/deploy/pathplanner/ThreePiece.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index a9266fc..3b8574c 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.61, - "robotLength": 0.64, + "robotWidth": 0.44, + "robotLength": 0.47, "holonomicMode": true, "generateJSON": false, "generateCSV": false diff --git a/src/main/deploy/pathplanner/FullAuto.path b/src/main/deploy/pathplanner/FullAuto.path index 842e8d2..c461061 100644 --- a/src/main/deploy/pathplanner/FullAuto.path +++ b/src/main/deploy/pathplanner/FullAuto.path @@ -2,13 +2,13 @@ "waypoints": [ { "anchorPoint": { - "x": 1.761330702148584, - "y": 5.138625661866708 + "x": 1.6699773570487313, + "y": 4.921661467254559 }, "prevControl": null, "nextControl": { - "x": 2.761330702148583, - "y": 5.138625661866708 + "x": 3.0973733742339267, + "y": 5.092948989316782 }, "holonomicAngle": 0, "isReversal": false, @@ -18,9 +18,35 @@ "stopEvent": { "names": [ "switchcone", - "placehigh", - "stowarm", - "switchcube" + "placemid", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.5 + } + }, + { + "anchorPoint": { + "x": 6.625896328715734, + "y": 4.510571414305222 + }, + "prevControl": { + "x": 4.855925267406089, + "y": 4.556248086855148 + }, + "nextControl": { + "x": 4.855925267406089, + "y": 4.556248086855148 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" ], "executionBehavior": "sequential", "waitBehavior": "none", @@ -29,59 +55,88 @@ }, { "anchorPoint": { - "x": 6.831441355190402, - "y": 4.613343927542556 + "x": 1.7499115340111024, + "y": 4.396379732930406 }, "prevControl": { - "x": 5.015793621330831, - "y": 4.601924759405074 + "x": 3.4171100820834126, + "y": 4.944499803529522 }, "nextControl": { - "x": 5.015793621330831, - "y": 4.601924759405074 + "x": 3.4171100820834126, + "y": 4.944499803529522 }, - "holonomicAngle": -3.8140748342904334, + "holonomicAngle": 0, "isReversal": true, "velOverride": null, "isLocked": false, "isStopPoint": true, "stopEvent": { "names": [ + "placemid", "stowarm" ], "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.0 + } + }, + { + "anchorPoint": { + "x": 6.637315496853216, + "y": 3.3686546005570643 + }, + "prevControl": { + "x": 6.561774940844737, + "y": 4.426222384675781 + }, + "nextControl": { + "x": 6.705830505678105, + "y": 2.409444477008612 + }, + "holonomicAngle": -38.290163192243014, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0 } }, { "anchorPoint": { - "x": 1.6813965251862129, - "y": 4.56766725499263 + "x": 3.4627867546333384, + "y": 3.1631095740823967 }, "prevControl": { - "x": 2.2180974276478453, - "y": 4.56766725499263 + "x": 3.6454934448330416, + "y": 3.1288520696699527 }, "nextControl": null, "holonomicAngle": 0, "isReversal": false, - "velOverride": null, + "velOverride": 0.1, "isLocked": false, "isStopPoint": false, "stopEvent": { - "names": [ - "placemid" - ], - "executionBehavior": "sequential", + "names": [], + "executionBehavior": "parallel", "waitBehavior": "none", "waitTime": 0 } } ], + "maxVelocity": 0.5, + "maxAcceleration": 1.0, + "isReversed": null, "markers": [ { - "position": 0.7563636363635975, + "position": 0.4, "names": [ "cubepickup" ] @@ -93,10 +148,41 @@ ] }, { - "position": 0.01, + "position": 0.0, "names": [ + "switchcube", "holdstow" ] + }, + { + "position": 0.0, + "names": [ + "event" + ] + }, + { + "position": 2.152727272727266, + "names": [ + "holdstow" + ] + }, + { + "position": 2.3563636363636347, + "names": [ + "conepickup" + ] + }, + { + "position": 3.025454545454368, + "names": [ + "holdstow" + ] + }, + { + "position": 1.8909090909090749, + "names": [ + "goarm" + ] } ] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path new file mode 100644 index 0000000..74c48a8 --- /dev/null +++ b/src/main/deploy/pathplanner/New Path.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/ThreePiece.path b/src/main/deploy/pathplanner/ThreePiece.path new file mode 100644 index 0000000..7684755 --- /dev/null +++ b/src/main/deploy/pathplanner/ThreePiece.path @@ -0,0 +1,183 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.6699773570487313, + "y": 4.921661467254559 + }, + "prevControl": null, + "nextControl": { + "x": 3.0973733742339267, + "y": 5.092948989316782 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcone", + "placemid", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.5 + } + }, + { + "anchorPoint": { + "x": 6.625896328715734, + "y": 4.510571414305222 + }, + "prevControl": { + "x": 4.855925267406089, + "y": 4.556248086855148 + }, + "nextControl": { + "x": 4.855925267406089, + "y": 4.556248086855148 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.8983607197983627, + "y": 4.4077989010678875 + }, + "prevControl": { + "x": 3.5655592678706736, + "y": 4.955918971667003 + }, + "nextControl": { + "x": 3.5655592678706736, + "y": 4.955918971667003 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "placemid", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.0 + } + }, + { + "anchorPoint": { + "x": 6.637315496853216, + "y": 3.3686546005570643 + }, + "prevControl": { + "x": 6.561774940844737, + "y": 4.426222384675781 + }, + "nextControl": { + "x": 6.561774940844737, + "y": 4.426222384675781 + }, + "holonomicAngle": -38.290163192243014, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.8118941707968883, + "y": 4.453475573617814 + }, + "prevControl": { + "x": 5.004374453193352, + "y": 6.303380811889829 + }, + "nextControl": null, + "holonomicAngle": -180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcube", + "placelow", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "switchcube", + "holdstow" + ] + }, + { + "position": 0.4, + "names": [ + "cubepickup" + ] + }, + { + "position": 1.01, + "names": [ + "holdstow" + ] + }, + { + "position": 2.15, + "names": [ + "holdstow" + ] + }, + { + "position": 0.0, + "names": [ + "event" + ] + }, + { + "position": 2.36, + "names": [ + "cubepickup" + ] + }, + { + "position": 3.03, + "names": [ + "holdstow" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e35ea2c..956d866 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -50,7 +50,7 @@ public static final class ArmRotationValues { public static final double armRotForMidCube = 40; public static final double armRotForLowCone = 5; - public static final double armRotForLowCube = 5; + public static final double armRotForLowCube = 15; public static final double armRotForConePickup = 3; public static final double armRotForCubePickup = 3; @@ -85,7 +85,7 @@ public static final class ArmExtendValues { public static final double armExtendForHighCone = 48000; public static final double armExtendForHighCube = 48000; - public static final double armExtendForMidCone = 35000; + public static final double armExtendForMidCone = 44000; public static final double armExtendForMidCube = 20000; public static final double armExtendForLowCone = 0; @@ -94,21 +94,21 @@ public static final class ArmExtendValues { public static final double armExtendForConePickup = 7000; public static final double armExtendForCubePickup = 7000; - public static final double armExtendForShelfCone = 45000; - public static final double armExtendForShelfCube = 45000; + public static final double armExtendForShelfCone = 41000; + public static final double armExtendForShelfCube = 41000; public static final double armExtendRevHighCone = 48000; - public static final double armExtendRevHighCube = 35000; + public static final double armExtendRevHighCube = 39000; - public static final double armExtendRevMidCone = 30000; + public static final double armExtendRevMidCone = 22000; public static final double armExtendRevMidCube = 0; public static final double armExtendRevLowCone = 0; public static final double armExtendRevLowCube = 0; - public static final double armExtendRevShelfCone = 48000; - public static final double armExtendRevShelfCube = 48000; + public static final double armExtendRevShelfCone = 43000; + public static final double armExtendRevShelfCube = 43000; } public static final class JointRotationValues { @@ -122,7 +122,7 @@ public static final class JointRotationValues { public static final double JointRotForHighCone = 16000; public static final double JointRotForHighCube = 17000; - public static final double JointRotForMidCone = 45000; + public static final double JointRotForMidCone = 43000; public static final double JointRotForMidCube = 17000 ; @@ -132,20 +132,20 @@ public static final class JointRotationValues { public static final double JointRotForConePickup = 11000; public static final double JointRotForCubePickup = 9000; - public static final double JointRotForShelfCone = 2000; + public static final double JointRotForShelfCone = 1500; public static final double JointRotForShelfCube = 9000; - public static final double JointRotRevHighCone = 13500; - public static final double JointRotRevHighCube = 4000; + public static final double JointRotRevHighCone = 11500; + public static final double JointRotRevHighCube = 2000; - public static final double JointRotRevMidCone = 6500; + public static final double JointRotRevMidCone = 7500; public static final double JointRotRevMidCube = 4000; - public static final double JointRotRevLowCone = 2000; + public static final double JointRotRevLowCone = 1000; public static final double JointRotRevLowCube = 0; - public static final double JointRotRevShelfCone = 48000; + public static final double JointRotRevShelfCone = 46000; public static final double JointRotRevShelfCube = 0; } @@ -223,7 +223,7 @@ public static final class Mod0 { public static final int driveMotorID = /* 2 */5; public static final int angleMotorID = /* 1 */4; public static final int canCoderID = /* 3 */6; - public static final double angleOffset = /* 274; */ 15; + public static final double angleOffset = /* 274; */ 15.82; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -233,7 +233,7 @@ public static final class Mod1 { public static final int driveMotorID = /* 11 */11; public static final int angleMotorID = /* 10 */10; public static final int canCoderID = /* 12 */12; - public static final double angleOffset = /* 240 ; */ 218.5; + public static final double angleOffset = /* 240 ; */ 218.67; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -243,7 +243,7 @@ public static final class Mod2 { public static final int driveMotorID = /* 5 */2; public static final int angleMotorID = /* 4 */1; public static final int canCoderID = /* 6 */3; - public static final double angleOffset = /* 319; */ 311; + public static final double angleOffset = /* 319; */ 311.66; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -253,7 +253,7 @@ public static final class Mod3 { public static final int driveMotorID = /* 8 */8; public static final int angleMotorID = /* 7 */7; public static final int canCoderID = /* 9 */9; - public static final double angleOffset = /* 305 ; */ 286; + public static final double angleOffset = /* 305 ; */ 285.73; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } diff --git a/src/main/java/frc/robot/GlobalVariables.java b/src/main/java/frc/robot/GlobalVariables.java index 2eb6107..55e99c3 100644 --- a/src/main/java/frc/robot/GlobalVariables.java +++ b/src/main/java/frc/robot/GlobalVariables.java @@ -13,7 +13,7 @@ public class GlobalVariables { public static int height; public static boolean robotDirection; public static double gripperConePlacement; - public static double armRotGoal; + public static double armRotGoal = 100; public static double armExtendGoal; public static enum StationSelect{ CLOSE_RAMP, LEFT_SHELF, LEFT_RAMP, RIGHT_SHELF, RIGHT_RAMP diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 04aa766..2d150f8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -74,6 +74,12 @@ public class RobotContainer { private final Trigger panelRelease = new JoystickButton(operatorPanel, 15); private final Trigger panelEmptyRight = new JoystickButton(operatorPanel, 12); private final Trigger panelEmptyLeft = new JoystickButton(operatorPanel, 14); + private final Trigger panelLED1 = new JoystickButton(operatorPanel, 4); + private final Trigger panelLED2 = new JoystickButton(operatorPanel, 2); + private final Trigger panelLED3 = new JoystickButton(operatorPanel, 7); + private final Trigger panelLED4 = new JoystickButton(operatorPanel, 6); + private final Trigger panelLED5 = new JoystickButton(operatorPanel, 1); + @@ -156,14 +162,17 @@ private void configureButtonBindings() { panelHigh.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.HIGH_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelMid.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelLow.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.GROUND_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - panelStow.onTrue(new AutoPlaceGamePiece(armSub, gripper, ArmPositions.HIGH_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // panelStow.onTrue(new AutoPlaceGamePiece(armSub, gripper, ArmPositions.HIGH_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelGround.whileTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelShelf.whileTrue(new ShelfIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelRelease.whileTrue(new AdaptiveOuttake(gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelRollers.whileTrue(new ManualRollers(gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelEmptyRight.whileTrue(new UprightConeIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelEmptyLeft.whileTrue(new SingleSubstationIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelLED3.onTrue(new InstantCommand(() -> armSub.homeTelescopePosition())); + panelLED4.onTrue(new InstantCommand(() -> armSub.homeGripperJointPos())); + // panelStow.whileTrue(new AutoBalanceStation(s_Swerve, false, true).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); // panelStow.whileTrue(new FullArmPosition(armSub, 150, 50000, 9000, true)); // panelStow.whileFalse(new FullArmPosition(armSub, 0, 0, 0, false)); @@ -260,13 +269,14 @@ private void configureButtonBindings() { 　| (￣ヽ＿_ヽ_)__) 　二つ */ /** + * * Use this to pass the autonomous command to the main {@link Robot} class. * * @return the command to run in autonomous */ public Command getAutonomousCommand() { - List pathGroup = PathPlanner.loadPathGroup("FullAuto", new PathConstraints(1, 1)); + List pathGroup = PathPlanner.loadPathGroup("ThreePiece", new PathConstraints(3, 3)); // This is just an example event map. It would be better to have a constant, global event map // in your code that will be used by all path following commands. @@ -281,6 +291,10 @@ public Command getAutonomousCommand() { eventMap.put("conepickup", new AutoGroundIntake(armSub, gripper, 0) ); eventMap.put("placemid", new AutoPlaceGamePiece(armSub, gripper, ArmPositions.MID_SCORE_ADAPTIVE)); eventMap.put("holdstow", new AdaptiveArmMovement(armSub, ArmPositions.STOWED_ADAPTIVE)); +eventMap.put("balance", new AutoBalanceStation(s_Swerve, true, false)); +eventMap.put("placelow", new AutoPlaceGamePiece(armSub, gripper, ArmPositions.GROUND_SCORE_ADAPTIVE)); + + @@ -292,7 +306,7 @@ public Command getAutonomousCommand() { s_Swerve::resetOdometry, // Pose2d consumer, used to reset odometry at the beginning of auto Constants.Swerve.swerveKinematics, // SwerveDriveKinematics new PIDConstants(5.0, 0.0, 0.0), // PID constants to correct for translation error (used to create the X and Y PID controllers) - new PIDConstants(0.5, 0.0, 0.0), // PID constants to correct for rotation error (used to create the rotation controller) + new PIDConstants(1.5, 0.0, 0.0), // PID constants to correct for rotation error (used to create the rotation controller) s_Swerve::setModuleStates, // Module states consumer used to output to the drive subsystem eventMap, false, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true diff --git a/src/main/java/frc/robot/commands/AutoBalanceStation.java b/src/main/java/frc/robot/commands/AutoBalanceStation.java index 4e770e4..8448fbe 100644 --- a/src/main/java/frc/robot/commands/AutoBalanceStation.java +++ b/src/main/java/frc/robot/commands/AutoBalanceStation.java @@ -29,7 +29,7 @@ private double mapdouble(double x, double in_min, double in_max, double out_min, public AutoBalanceStation(Swerve s_Swerve, boolean fieldRelative, boolean openLoop) { this.s_Swerve = s_Swerve; addRequirements(s_Swerve); - pidController = new PIDController(0.01 , 0, 0); + pidController = new PIDController(0.8 , 0, 0); pidController.setTolerance(1); this.fieldRelative = fieldRelative; @@ -82,4 +82,9 @@ public void execute() { rotation = rAxis * Constants.Swerve.maxAngularVelocity; s_Swerve.drive(translation, rotation, fieldRelative, openLoop); } + @Override + public boolean isFinished() { + // return pidController.atSetpoint(); + return false; + } } diff --git a/src/main/java/frc/robot/commands/AutoGroundIntake.java b/src/main/java/frc/robot/commands/AutoGroundIntake.java index da9643c..212fce9 100644 --- a/src/main/java/frc/robot/commands/AutoGroundIntake.java +++ b/src/main/java/frc/robot/commands/AutoGroundIntake.java @@ -39,33 +39,33 @@ public void initialize() { @Override public void execute() { if(gamePiece == 0){ - armSub.moveGripperJointPosition(33000); + armSub.moveGripperJointPosition(31000); armSub.moveTelescopeArmPosition(8000); armSub.moveRotArmPosition(1); - if((gripper.getDistanceSensorDist() <= 12)){ + /* if((gripper.getDistanceSensorDist() <= 12)){ gripper.moveGripper(0); finished = true; } - else{ + else{ */ gripper.moveGripper(-1); - } + /* } */ } else{ armSub.moveGripperJointPosition(24000); armSub.moveTelescopeArmPosition(8000); armSub.moveRotArmPosition(1); - if((gripper.getDistanceSensorDist() <= 12)){ + /* if((gripper.getDistanceSensorDist() <= 12)){ gripper.moveGripper(0); finished = true; } - else{ + else{ */ gripper.moveGripper(0.8); - } + /* } */ } @@ -80,6 +80,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return finished; + return false; } } diff --git a/src/main/java/frc/robot/commands/AutoPlaceGamePiece.java b/src/main/java/frc/robot/commands/AutoPlaceGamePiece.java index 2c2e517..45e1ad4 100644 --- a/src/main/java/frc/robot/commands/AutoPlaceGamePiece.java +++ b/src/main/java/frc/robot/commands/AutoPlaceGamePiece.java @@ -16,6 +16,7 @@ public class AutoPlaceGamePiece extends CommandBase { Gripper gripper; GlobalVariables.ArmPositions armPosition; boolean finished; + int timer; public AutoPlaceGamePiece(ArmSub armSub, Gripper gripper, GlobalVariables.ArmPositions armPosition) { this.armSub = armSub; this.armPosition = armPosition; @@ -29,6 +30,7 @@ public AutoPlaceGamePiece(ArmSub armSub, Gripper gripper, GlobalVariables.ArmPos @Override public void initialize() { finished = false; + timer = 0; } @@ -50,20 +52,26 @@ public void execute() { if(armSub.getArmEncoderPosition() >= GlobalVariables.armRotGoal*0.75){ armSub.armExtendPresetPositions(armPosition); - if(armSub.getTelescopePos()>= (GlobalVariables.armExtendGoal -1500) && - armSub.getTelescopePos()<= (GlobalVariables.armExtendGoal + 1500)){ - if(gripper.getDistanceSensorDist() <= 12){ + if(armSub.getTelescopePos()>= (GlobalVariables.armExtendGoal -500) && + armSub.getTelescopePos()<= (GlobalVariables.armExtendGoal + 500)){ + timer++; + if(/* gripper.getDistanceSensorDist() <= 12 */ timer>=5){ if(GlobalVariables.gamePiece == 0){ gripper.moveGripper(0.7); } else{ gripper.moveGripper(-1); + } + if (timer >= 20){ + finished = true; + gripper.moveGripper(0); + + } } else{ gripper.moveGripper(0); - finished = true; } } else{ @@ -79,7 +87,10 @@ public void execute() { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + gripper.moveGripper(0); + + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index 16d4921..92265ad 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -28,7 +28,7 @@ public void initialize() {} @Override public void execute() { if(GlobalVariables.gamePiece == 0){ - armSub.moveGripperJointPosition(33000); + armSub.moveGripperJointPosition(31000); armSub.moveTelescopeArmPosition(8000); armSub.moveRotArmPosition(1); if((gripper.getDistanceSensorDist() <= 12)){ diff --git a/src/main/java/frc/robot/commands/SingleSubstationIntake.java b/src/main/java/frc/robot/commands/SingleSubstationIntake.java index 82f40fc..28174c7 100644 --- a/src/main/java/frc/robot/commands/SingleSubstationIntake.java +++ b/src/main/java/frc/robot/commands/SingleSubstationIntake.java @@ -28,7 +28,7 @@ public void initialize() {} @Override public void execute() { if(GlobalVariables.gamePiece == 0){ - armSub.moveGripperJointPosition(27000); + armSub.moveGripperJointPosition(25000); armSub.moveTelescopeArmPosition(0); armSub.moveRotArmPosition(40); if((gripper.getDistanceSensorDist() <= 12)){ @@ -42,7 +42,7 @@ public void execute() { } else{ - armSub.moveGripperJointPosition(15000); + armSub.moveGripperJointPosition(13000); armSub.moveTelescopeArmPosition(0); armSub.moveRotArmPosition(40); if((gripper.getDistanceSensorDist() <= 12)){ diff --git a/src/main/java/frc/robot/commands/UprightConeIntake.java b/src/main/java/frc/robot/commands/UprightConeIntake.java index 9d62aa9..5c933e9 100644 --- a/src/main/java/frc/robot/commands/UprightConeIntake.java +++ b/src/main/java/frc/robot/commands/UprightConeIntake.java @@ -28,9 +28,9 @@ public void initialize() {} @Override public void execute() { - armSub.moveGripperJointPosition(48000); + armSub.moveGripperJointPosition(46000); armSub.moveTelescopeArmPosition(0); - armSub.moveRotArmPosition(42); + armSub.moveRotArmPosition(44); if((gripper.getDistanceSensorDist() <= 12)){ gripper.moveGripper(0); diff --git a/src/main/java/frc/robot/subsystems/ArmSub.java b/src/main/java/frc/robot/subsystems/ArmSub.java index 19cfae8..b0e1cf0 100644 --- a/src/main/java/frc/robot/subsystems/ArmSub.java +++ b/src/main/java/frc/robot/subsystems/ArmSub.java @@ -56,14 +56,14 @@ public ArmSub() { testCanCoder.configMagnetOffset(-12); - rotMaxSpeedFor = 0.5; - rotMaxSpeedRev = 0.3; + rotMaxSpeedFor = 1; + rotMaxSpeedRev = 0.8; - limit = new SupplyCurrentLimitConfiguration(true, 10, 10, 0); + limit = new SupplyCurrentLimitConfiguration(true, 30, 30, 0); - armPID = new PIDController(0.024, 0.000, 0.000); + armPID = new PIDController(0.022, 0.000, 0.000); armPID.setTolerance(0.1); armPID.enableContinuousInput(0, 360); @@ -109,14 +109,14 @@ public ArmSub() { telescopeArm.configPeakOutputReverse(-telMaxSpeedRev); telescopeArm.setSensorPhase(true); telescopeArm.setInverted(InvertType.None); - telescopeArm.config_kP(0, 0.2); + telescopeArm.config_kP(0, 0.7); telescopeArm.config_kI(0, 0.0); telescopeArm.config_kD(0, 0.0); telescopeArm.config_kF(0, 0.0); telescopeArm.setNeutralMode(NeutralMode.Brake); telescopeArm.configNeutralDeadband(0.1); - telescopeArm.configMotionAcceleration(10000); - telescopeArm.configMotionCruiseVelocity(10000); + telescopeArm.configMotionAcceleration(45000); + telescopeArm.configMotionCruiseVelocity(300000); telescopeArm.setSelectedSensorPosition(0); telescopeArm.configSupplyCurrentLimit(limit); @@ -125,14 +125,14 @@ public ArmSub() { gripperJointFalcon.configPeakOutputReverse(-telMaxSpeedRev); gripperJointFalcon.setSensorPhase(true); gripperJointFalcon.setInverted(InvertType.InvertMotorOutput); - gripperJointFalcon.config_kP(0, 0.4); + gripperJointFalcon.config_kP(0, 0.6); gripperJointFalcon.config_kI(0, 0.0); gripperJointFalcon.config_kD(0, 0.0); gripperJointFalcon.config_kF(0, 0.0); gripperJointFalcon.setNeutralMode(NeutralMode.Brake); gripperJointFalcon.configNeutralDeadband(0.001); - gripperJointFalcon.configMotionAcceleration(15000); - gripperJointFalcon.configMotionCruiseVelocity(15000); + gripperJointFalcon.configMotionAcceleration(32000); + gripperJointFalcon.configMotionCruiseVelocity(40000); gripperJointFalcon.setSelectedSensorPosition(0); gripperJointFalcon.configAllowableClosedloopError(0, 10); @@ -664,8 +664,8 @@ public void periodic() { SmartDashboard.putData("PID ERROR ", armPID); SmartDashboard.putNumber("Arm Encoder Position", getArmEncoderPosition() ); SmartDashboard.putNumber("Arm Motor Position", getArmMotorPos()); - SmartDashboard.putNumber("Arm Motor 1 Current", leftArm.getStatorCurrent()); - SmartDashboard.putNumber("Arm Motor 2 Current", rightArm.getStatorCurrent()); + SmartDashboard.putNumber("Arm Motor Actual Speed", leftArm.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Arm Motor Set Speed", leftArm.getMotorOutputPercent()); SmartDashboard.putNumber("Telescope Position", telescopeArm.getSelectedSensorPosition()); SmartDashboard.putNumber("Telescope Speed?", telescopeArm.getSupplyCurrent()); SmartDashboard.putNumber("Joint Position ", getGripperJointPos() ); diff --git a/src/main/java/frc/robot/subsystems/Gripper.java b/src/main/java/frc/robot/subsystems/Gripper.java index ad4223e..417fc1b 100644 --- a/src/main/java/frc/robot/subsystems/Gripper.java +++ b/src/main/java/frc/robot/subsystems/Gripper.java @@ -5,6 +5,7 @@ package frc.robot.subsystems; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; @@ -18,6 +19,8 @@ public class Gripper extends SubsystemBase { public TalonFX gripperFalcon; + private SupplyCurrentLimitConfiguration limit; + private Rev2mDistanceSensor distanceSensor; @@ -28,6 +31,11 @@ public Gripper() { gripperFalcon.setNeutralMode(NeutralMode.Brake); + limit = new SupplyCurrentLimitConfiguration(true, 50, 50, 0.5); + // gripperFalcon.configSupplyCurrentLimit(limit); + + + distanceSensor = new Rev2mDistanceSensor(Port.kMXP); distanceSensor.setAutomaticMode(true); @@ -37,6 +45,10 @@ public double getDistanceSensorDist(){ return distanceSensor.getRange(); } + public double getGripperMotorCurrent(){ + return gripperFalcon.getSupplyCurrent(); + } + public void moveGripper(double percent ){ gripperFalcon.set(TalonFXControlMode.PercentOutput, percent); } @@ -44,6 +56,7 @@ public void moveGripper(double percent ){ @Override public void periodic() { SmartDashboard.putNumber("Distance Sensor", getDistanceSensorDist()); + SmartDashboard.putNumber("GripperCurrent", gripperFalcon.getSupplyCurrent()); } } From d4c8cf01a993e8ddac1f954c9ce3ea368e25d9c9 Mon Sep 17 00:00:00 2001 From: Oscar Date: Wed, 26 Apr 2023 10:07:17 -0700 Subject: [PATCH 12/12] OFFICIAL HOUSTON CODE --- .../deploy/pathplanner/CableProtector.path | 227 ++++++++++++++++++ .../pathplanner/CableProtectorCharge.path | 221 +++++++++++++++++ .../pathplanner/CableProtectorDoubleHigh.path | 214 +++++++++++++++++ src/main/deploy/pathplanner/FullAuto.path | 32 +-- .../deploy/pathplanner/MiddleOnePiece.path | 85 +++++++ .../deploy/pathplanner/MiddleTwoPiece.path | 94 ++++++++ src/main/deploy/pathplanner/New Path.path | 8 +- src/main/deploy/pathplanner/ThreePiece.path | 52 ++-- .../deploy/pathplanner/ThreePieceCone.path | 185 ++++++++++++++ src/main/java/frc/robot/Constants.java | 47 ++-- src/main/java/frc/robot/GlobalVariables.java | 3 +- src/main/java/frc/robot/Robot.java | 27 ++- src/main/java/frc/robot/RobotContainer.java | 82 +++++-- .../robot/commands/AdaptiveArmMovement.java | 17 +- .../frc/robot/commands/AdaptiveOuttake.java | 5 +- .../robot/commands/AutoBalanceStation.java | 58 +---- .../frc/robot/commands/AutoGroundIntake.java | 4 +- .../robot/commands/AutoPlaceGamePiece.java | 2 +- .../frc/robot/commands/AutoShootCone.java | 87 +++++++ .../java/frc/robot/commands/ConstantHold.java | 44 ++++ .../java/frc/robot/commands/GroundIntake.java | 4 +- .../frc/robot/commands/ManualRollers.java | 14 +- .../java/frc/robot/commands/ShelfIntake.java | 40 +-- .../commands/SingleSubstationIntake.java | 2 +- .../frc/robot/commands/UprightConeIntake.java | 4 +- .../java/frc/robot/subsystems/ArmSub.java | 91 ++++--- .../java/frc/robot/subsystems/CANdleSub.java | 6 +- .../java/frc/robot/subsystems/Gripper.java | 2 +- .../java/frc/robot/subsystems/Swerve.java | 29 ++- 29 files changed, 1473 insertions(+), 213 deletions(-) create mode 100644 src/main/deploy/pathplanner/CableProtector.path create mode 100644 src/main/deploy/pathplanner/CableProtectorCharge.path create mode 100644 src/main/deploy/pathplanner/CableProtectorDoubleHigh.path create mode 100644 src/main/deploy/pathplanner/MiddleOnePiece.path create mode 100644 src/main/deploy/pathplanner/MiddleTwoPiece.path create mode 100644 src/main/deploy/pathplanner/ThreePieceCone.path create mode 100644 src/main/java/frc/robot/commands/AutoShootCone.java create mode 100644 src/main/java/frc/robot/commands/ConstantHold.java diff --git a/src/main/deploy/pathplanner/CableProtector.path b/src/main/deploy/pathplanner/CableProtector.path new file mode 100644 index 0000000..09a1514 --- /dev/null +++ b/src/main/deploy/pathplanner/CableProtector.path @@ -0,0 +1,227 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.6585581889112497, + "y": 0.6965692563863765 + }, + "prevControl": null, + "nextControl": { + "x": 2.6585581889112495, + "y": 0.6965692563863765 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcone", + "placemid", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.5 + } + }, + { + "anchorPoint": { + "x": 3.4, + "y": 0.7 + }, + "prevControl": { + "x": 2.2457598839377724, + "y": 0.7 + }, + "nextControl": { + "x": 4.505066359960422, + "y": 0.7 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.524769391419125, + "y": 0.7 + }, + "prevControl": { + "x": 3.9750315240726595, + "y": 0.7 + }, + "nextControl": { + "x": 5.074507258765594, + "y": 0.7 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.5231238154784, + "y": 1.073401804923269 + }, + "prevControl": { + "x": 6.180548771353952, + "y": 1.15904556595438 + }, + "nextControl": { + "x": 6.180548771353952, + "y": 1.15904556595438 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.49051188700668, + "y": 0.9249526191360088 + }, + "prevControl": { + "x": 4.684637745343868, + "y": 0.8650019864142308 + }, + "nextControl": { + "x": 4.296386028669493, + "y": 0.9849032518577867 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 0.5 + } + }, + { + "anchorPoint": { + "x": 6.8200221870529205, + "y": 1.8841627426844594 + }, + "prevControl": { + "x": 6.842860523327884, + "y": 1.5986835392474208 + }, + "nextControl": { + "x": 6.842860523327884, + "y": 1.5986835392474208 + }, + "holonomicAngle": 53.9726266148967, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm " + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.444835214456754, + "y": 1.1190784774731937 + }, + "prevControl": { + "x": 6.123452930666544, + "y": 0.0685150088248904 + }, + "nextControl": null, + "holonomicAngle": -16.927513064146837, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.01, + "names": [ + "holdstow" + ] + }, + { + "position": 2.01, + "names": [ + "conepickup" + ] + }, + { + "position": 4.01, + "names": [ + "holdstow" + ] + }, + { + "position": 4.101818181818184, + "names": [ + "conepickup" + ] + }, + { + "position": 3.02, + "names": [ + "shootcone" + ] + }, + { + "position": 5.01, + "names": [ + "holdstow" + ] + }, + { + "position": 5.410909090909058, + "names": [ + "shootcone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CableProtectorCharge.path b/src/main/deploy/pathplanner/CableProtectorCharge.path new file mode 100644 index 0000000..cb72e4c --- /dev/null +++ b/src/main/deploy/pathplanner/CableProtectorCharge.path @@ -0,0 +1,221 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.6585581889112497, + "y": 0.6965692563863765 + }, + "prevControl": null, + "nextControl": { + "x": 2.6585581889112495, + "y": 0.6965692563863765 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcone", + "placemid", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.5 + } + }, + { + "anchorPoint": { + "x": 3.4, + "y": 0.7 + }, + "prevControl": { + "x": 2.2457598839377724, + "y": 0.7 + }, + "nextControl": { + "x": 4.505066359960422, + "y": 0.7 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.524769391419125, + "y": 0.7 + }, + "prevControl": { + "x": 3.9750315240726595, + "y": 0.7 + }, + "nextControl": { + "x": 5.074507258765594, + "y": 0.7 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.5231238154784, + "y": 1.073401804923269 + }, + "prevControl": { + "x": 6.180548771353952, + "y": 1.15904556595438 + }, + "nextControl": { + "x": 6.180548771353952, + "y": 1.15904556595438 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.49051188700668, + "y": 0.9249526191360088 + }, + "prevControl": { + "x": 4.684637745343868, + "y": 0.8650019864142308 + }, + "nextControl": { + "x": 4.296386028669493, + "y": 0.9849032518577867 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 0.5 + } + }, + { + "anchorPoint": { + "x": 6.911375532152774, + "y": 2.021192760334239 + }, + "prevControl": { + "x": 6.922794700290257, + "y": 1.3817193446352711 + }, + "nextControl": { + "x": 6.899956364015291, + "y": 2.660666176033206 + }, + "holonomicAngle": 53.9726266148967, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm " + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.405690913945931, + "y": 2.8433728662329125 + }, + "prevControl": { + "x": 5.575332860067431, + "y": 3.1973670784948407 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.01, + "names": [ + "holdstow" + ] + }, + { + "position": 2.01, + "names": [ + "conepickup" + ] + }, + { + "position": 4.01, + "names": [ + "conepickup" + ] + }, + { + "position": 3.02, + "names": [ + "shootcone" + ] + }, + { + "position": 4.01, + "names": [ + "conepickup" + ] + }, + { + "position": 5.01, + "names": [ + "holdstow" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CableProtectorDoubleHigh.path b/src/main/deploy/pathplanner/CableProtectorDoubleHigh.path new file mode 100644 index 0000000..295617c --- /dev/null +++ b/src/main/deploy/pathplanner/CableProtectorDoubleHigh.path @@ -0,0 +1,214 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.6928156933236946, + "y": 1.0391443005108238 + }, + "prevControl": null, + "nextControl": { + "x": 2.692815693323695, + "y": 1.0391443005108238 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcube", + "placehigh", + "stowarm", + "switchcone" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.5 + } + }, + { + "anchorPoint": { + "x": 3.4, + "y": 0.7 + }, + "prevControl": { + "x": 2.2457598839377724, + "y": 0.7 + }, + "nextControl": { + "x": 4.505066359960422, + "y": 0.7 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "switchcone" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.524769391419125, + "y": 0.7 + }, + "prevControl": { + "x": 3.9750315240726595, + "y": 0.7 + }, + "nextControl": { + "x": 5.074507258765594, + "y": 0.7 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.534542983615882, + "y": 0.8678567784486001 + }, + "prevControl": { + "x": 6.191967939491434, + "y": 0.9535005394797109 + }, + "nextControl": { + "x": 6.191967939491434, + "y": 0.9535005394797109 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.5361885595566065, + "y": 0.8107609377611926 + }, + "prevControl": { + "x": 4.730314417893793, + "y": 0.7508103050394148 + }, + "nextControl": { + "x": 4.34206270121942, + "y": 0.8707115704829705 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 0.1 + } + }, + { + "anchorPoint": { + "x": 3.3828525776709673, + "y": 0.7993417696237097 + }, + "prevControl": { + "x": 3.2857896485023743, + "y": 0.8293170859845989 + }, + "nextControl": { + "x": 3.4799155068395606, + "y": 0.7693664532628208 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.85, + "y": 0.55 + }, + "prevControl": { + "x": 2.6950184421736356, + "y": 0.515742495587556 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "placemid", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.01, + "names": [ + "holdstow", + "switchcone" + ] + }, + { + "position": 3.01, + "names": [ + "holdstow" + ] + }, + { + "position": 2.01, + "names": [ + "conepickup" + ] + }, + { + "position": 5.95, + "names": [ + "goarm" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/FullAuto.path b/src/main/deploy/pathplanner/FullAuto.path index c461061..6eacc24 100644 --- a/src/main/deploy/pathplanner/FullAuto.path +++ b/src/main/deploy/pathplanner/FullAuto.path @@ -2,13 +2,13 @@ "waypoints": [ { "anchorPoint": { - "x": 1.6699773570487313, - "y": 4.921661467254559 + "x": 1.7384923658736207, + "y": 5.070110653041819 }, "prevControl": null, "nextControl": { - "x": 3.0973733742339267, - "y": 5.092948989316782 + "x": 3.165888383058816, + "y": 5.241398175104043 }, "holonomicAngle": 0, "isReversal": false, @@ -59,12 +59,12 @@ "y": 4.396379732930406 }, "prevControl": { - "x": 3.4171100820834126, - "y": 4.944499803529522 + "x": 3.554140099733191, + "y": 5.344170688341377 }, "nextControl": { - "x": 3.4171100820834126, - "y": 4.944499803529522 + "x": 3.554140099733191, + "y": 5.344170688341377 }, "holonomicAngle": 0, "isReversal": true, @@ -110,12 +110,12 @@ }, { "anchorPoint": { - "x": 3.4627867546333384, - "y": 3.1631095740823967 + "x": 3.497044259045783, + "y": 3.11743290153247 }, "prevControl": { - "x": 3.6454934448330416, - "y": 3.1288520696699527 + "x": 3.679750949245486, + "y": 3.083175397120026 }, "nextControl": null, "holonomicAngle": 0, @@ -155,13 +155,7 @@ ] }, { - "position": 0.0, - "names": [ - "event" - ] - }, - { - "position": 2.152727272727266, + "position": 2.01, "names": [ "holdstow" ] diff --git a/src/main/deploy/pathplanner/MiddleOnePiece.path b/src/main/deploy/pathplanner/MiddleOnePiece.path new file mode 100644 index 0000000..c374bd4 --- /dev/null +++ b/src/main/deploy/pathplanner/MiddleOnePiece.path @@ -0,0 +1,85 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.6699773570487313, + "y": 2.729181184858097 + }, + "prevControl": null, + "nextControl": { + "x": 2.6699773570487313, + "y": 2.729181184858097 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcube", + "placehigh", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.963584576741804, + "y": 2.6606661760332067 + }, + "prevControl": { + "x": 5.038631957605795, + "y": 2.7177620167206156 + }, + "nextControl": { + "x": 5.038631957605795, + "y": 2.7177620167206156 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.8167809668952675, + "y": 2.740600352995578 + }, + "prevControl": { + "x": 4.650380240931421, + "y": 2.740600352995578 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.01, + "names": [ + "holdstow" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/MiddleTwoPiece.path b/src/main/deploy/pathplanner/MiddleTwoPiece.path new file mode 100644 index 0000000..eb360d3 --- /dev/null +++ b/src/main/deploy/pathplanner/MiddleTwoPiece.path @@ -0,0 +1,94 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.6699773570487313, + "y": 2.729181184858097 + }, + "prevControl": null, + "nextControl": { + "x": 2.6699773570487313, + "y": 2.729181184858097 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcube", + "placehigh", + "stowarm", + "switchcone" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.128339726764924, + "y": 2.1696419461214993 + }, + "prevControl": { + "x": 8.277735991679151, + "y": 2.545187260400405 + }, + "nextControl": { + "x": 5.975003744879285, + "y": 1.7928093975846067 + }, + "holonomicAngle": -29.7448812969423, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.9195534801326013, + "y": 2.649247007895725 + }, + "prevControl": { + "x": 2.9195534801326013, + "y": 2.649247007895725 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.5527272727272412, + "names": [ + "conepickup" + ] + }, + { + "position": 1.01, + "names": [ + "holdstow" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path index 74c48a8..558f413 100644 --- a/src/main/deploy/pathplanner/New Path.path +++ b/src/main/deploy/pathplanner/New Path.path @@ -2,13 +2,13 @@ "waypoints": [ { "anchorPoint": { - "x": 1.0, - "y": 3.0 + "x": 2.046809905585623, + "y": 3.836840494193809 }, "prevControl": null, "nextControl": { - "x": 2.0, - "y": 3.0 + "x": 3.9272190852074016, + "y": 4.376782616813096 }, "holonomicAngle": 0, "isReversal": false, diff --git a/src/main/deploy/pathplanner/ThreePiece.path b/src/main/deploy/pathplanner/ThreePiece.path index 7684755..03a2d8c 100644 --- a/src/main/deploy/pathplanner/ThreePiece.path +++ b/src/main/deploy/pathplanner/ThreePiece.path @@ -59,12 +59,12 @@ "y": 4.4077989010678875 }, "prevControl": { - "x": 3.5655592678706736, - "y": 4.955918971667003 + "x": 3.531301763458228, + "y": 5.013014812354411 }, "nextControl": { - "x": 3.5655592678706736, - "y": 4.955918971667003 + "x": 3.531301763458228, + "y": 5.013014812354411 }, "holonomicAngle": 0, "isReversal": true, @@ -83,18 +83,18 @@ }, { "anchorPoint": { - "x": 6.637315496853216, - "y": 3.3686546005570643 + "x": 6.5231238154784, + "y": 3.5627804588942515 }, "prevControl": { - "x": 6.561774940844737, - "y": 4.426222384675781 + "x": 5.792297054679579, + "y": 4.396379732930407 }, "nextControl": { - "x": 6.561774940844737, - "y": 4.426222384675781 + "x": 5.792297054679579, + "y": 4.396379732930407 }, - "holonomicAngle": -38.290163192243014, + "holonomicAngle": -24.443954780416572, "isReversal": true, "velOverride": null, "isLocked": false, @@ -110,15 +110,15 @@ }, { "anchorPoint": { - "x": 2.8118941707968883, - "y": 4.453475573617814 + "x": 2.594929976184739, + "y": 4.4077989010678875 }, "prevControl": { - "x": 5.004374453193352, - "y": 6.303380811889829 + "x": 4.022325993369936, + "y": 4.430637237342851 }, "nextControl": null, - "holonomicAngle": -180.0, + "holonomicAngle": -167.73522627210778, "isReversal": false, "velOverride": null, "isLocked": false, @@ -156,17 +156,11 @@ ] }, { - "position": 2.15, + "position": 2.01, "names": [ "holdstow" ] }, - { - "position": 0.0, - "names": [ - "event" - ] - }, { "position": 2.36, "names": [ @@ -178,6 +172,18 @@ "names": [ "holdstow" ] + }, + { + "position": 3.8399999999998844, + "names": [ + "placelow" + ] + }, + { + "position": 1.890909090909075, + "names": [ + "goarm" + ] } ] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/ThreePieceCone.path b/src/main/deploy/pathplanner/ThreePieceCone.path new file mode 100644 index 0000000..e41d259 --- /dev/null +++ b/src/main/deploy/pathplanner/ThreePieceCone.path @@ -0,0 +1,185 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.6699773570487313, + "y": 5.0815298211793 + }, + "prevControl": null, + "nextControl": { + "x": 3.0973733742339267, + "y": 5.2528173432415235 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcone", + "placemid", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.5 + } + }, + { + "anchorPoint": { + "x": 6.625896328715734, + "y": 4.510571414305222 + }, + "prevControl": { + "x": 4.855925267406089, + "y": 4.556248086855148 + }, + "nextControl": { + "x": 4.855925267406089, + "y": 4.556248086855148 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.7270731977361389, + "y": 4.259349715280627 + }, + "prevControl": { + "x": 4.159356011019714, + "y": 4.784631449604782 + }, + "nextControl": { + "x": 4.159356011019714, + "y": 4.784631449604782 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "placemid", + "stowarm", + "switchcone" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.0 + } + }, + { + "anchorPoint": { + "x": 6.580219656165809, + "y": 3.4714271137943995 + }, + "prevControl": { + "x": 5.849392895366988, + "y": 4.305026387830555 + }, + "nextControl": { + "x": 5.849392895366988, + "y": 4.305026387830555 + }, + "holonomicAngle": -36.86989764584406, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm", + "switchcone" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.480738294809923, + "y": 4.4648947417552955 + }, + "prevControl": { + "x": 3.90813431199512, + "y": 4.487733078030259 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcone", + "placelow", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.4, + "names": [ + "cubepickup" + ] + }, + { + "position": 0.0, + "names": [ + "switchcube", + "holdstow" + ] + }, + { + "position": 1.01, + "names": [ + "holdstow" + ] + }, + { + "position": 2.15, + "names": [ + "holdstow" + ] + }, + { + "position": 2.36, + "names": [ + "conepickup" + ] + }, + { + "position": 3.03, + "names": [ + "holdstow" + ] + }, + { + "position": 3.6072727272727443, + "names": [ + "placelow" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 956d866..1c3d79c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -55,10 +55,10 @@ public static final class ArmRotationValues { public static final double armRotForConePickup = 3; public static final double armRotForCubePickup = 3; - public static final double armRotForShelfCone = 120; + public static final double armRotForShelfCone = 105;//120; public static final double armRotForShelfCube = 110; - public static final double armRotRevHighCone = 138; + public static final double armRotRevHighCone = 141; public static final double armRotRevHighCube = 145; public static final double armRotRevMidCone = 135; @@ -70,13 +70,14 @@ public static final class ArmRotationValues { public static final double armRotRevConePickup = 180; public static final double armRotRevCubePickup = 170; - public static final double armRotRevShelfCone = 81; + public static final double armRotRevShelfCone = 95;//81; public static final double armRotRevShelfCube = 120; + public static final double framePerimeter = 85; } public static final class ArmExtendValues { - public static final double armExtendStow = 0; + public static final double armExtendStow = 500; public static final double armExtendConePickup = 7000; public static final double armExtendCubePickup = 7000; @@ -94,11 +95,11 @@ public static final class ArmExtendValues { public static final double armExtendForConePickup = 7000; public static final double armExtendForCubePickup = 7000; - public static final double armExtendForShelfCone = 41000; + public static final double armExtendForShelfCone = 0;//41000; public static final double armExtendForShelfCube = 41000; - public static final double armExtendRevHighCone = 48000; + public static final double armExtendRevHighCone = 52000; public static final double armExtendRevHighCube = 39000; public static final double armExtendRevMidCone = 22000; @@ -107,8 +108,11 @@ public static final class ArmExtendValues { public static final double armExtendRevLowCone = 0; public static final double armExtendRevLowCube = 0; - public static final double armExtendRevShelfCone = 43000; + public static final double armExtendRevShelfCone = 0;//43000; public static final double armExtendRevShelfCube = 43000; + + public static final double framePerimeter = 0; + } public static final class JointRotationValues { @@ -116,7 +120,7 @@ public static final class JointRotationValues { public static final double JointRotStowCube = 1000; public static final double JointRotConePickup = 11000; - public static final double JointRotCubePickup = 9000; + public static final double JointRotCubePickup = 10000; public static final double JointRotForHighCone = 16000; @@ -126,27 +130,30 @@ public static final class JointRotationValues { public static final double JointRotForMidCube = 17000 ; - public static final double JointRotForLowCone = 17000; - public static final double JointRotForLowCube = 6000; + public static final double JointRotForLowCone = 20000; + public static final double JointRotForLowCube = 12000; public static final double JointRotForConePickup = 11000; - public static final double JointRotForCubePickup = 9000; + public static final double JointRotForCubePickup = 8000; - public static final double JointRotForShelfCone = 1500; + public static final double JointRotForShelfCone = 8500;//1500; public static final double JointRotForShelfCube = 9000; - public static final double JointRotRevHighCone = 11500; + public static final double JointRotRevHighCone = 12000; public static final double JointRotRevHighCube = 2000; public static final double JointRotRevMidCone = 7500; public static final double JointRotRevMidCube = 4000; - + public static final double JointRotRevLowCone = 1000; public static final double JointRotRevLowCube = 0; - public static final double JointRotRevShelfCone = 46000; + public static final double JointRotRevShelfCone = 43200;//46000; public static final double JointRotRevShelfCube = 0; + + public static final double framePerimeter = 0; + } @@ -162,7 +169,7 @@ public static final class Swerve { public static final double wheelDiameter = Units.inchesToMeters(3.94); public static final double wheelCircumference = wheelDiameter * Math.PI; - public static final double openLoopRamp = 0.0; + public static final double openLoopRamp = 0.05; public static final double closedLoopRamp = 0.61 ; public static final double driveGearRatio = (6.12 / 1.0); //8:14:1 @@ -204,7 +211,7 @@ public static final class Swerve { /* Swerve Profiling Values */ public static final double maxSpeed = 10; //meters per second - public static final double maxAngularVelocity = 11.5; + public static final double maxAngularVelocity = 16; /* Neutral Modes */ public static final NeutralMode angleNeutralMode = NeutralMode.Coast; @@ -223,7 +230,7 @@ public static final class Mod0 { public static final int driveMotorID = /* 2 */5; public static final int angleMotorID = /* 1 */4; public static final int canCoderID = /* 3 */6; - public static final double angleOffset = /* 274; */ 15.82; + public static final double angleOffset = /* 274; */ 15.22; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -233,7 +240,7 @@ public static final class Mod1 { public static final int driveMotorID = /* 11 */11; public static final int angleMotorID = /* 10 */10; public static final int canCoderID = /* 12 */12; - public static final double angleOffset = /* 240 ; */ 218.67; + public static final double angleOffset = /* 240 ; */ 219.02; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -253,7 +260,7 @@ public static final class Mod3 { public static final int driveMotorID = /* 8 */8; public static final int angleMotorID = /* 7 */7; public static final int canCoderID = /* 9 */9; - public static final double angleOffset = /* 305 ; */ 285.73; + public static final double angleOffset = /* 305 ; */ 285.53; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } diff --git a/src/main/java/frc/robot/GlobalVariables.java b/src/main/java/frc/robot/GlobalVariables.java index 55e99c3..ecd191c 100644 --- a/src/main/java/frc/robot/GlobalVariables.java +++ b/src/main/java/frc/robot/GlobalVariables.java @@ -33,7 +33,8 @@ public static enum ArmPositions{ SHELF_PICKUP_ADAPTIVE, MID_SCORE_ADAPTIVE, HIGH_SCORE_ADAPTIVE, - CONE_UPRIGHT + CONE_UPRIGHT, + FRAME_PERIMETER } public static ArmPositions armPosition; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 57cca93..064a057 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -41,7 +41,8 @@ public class Robot extends TimedRobot { private PowerDistribution pdh; DigitalInput input = new DigitalInput(0); - + DigitalInput inputb = new DigitalInput(1); + boolean resetFlag = false; private Joystick panel = new Joystick(2); @@ -71,8 +72,16 @@ public void robotInit() { */ @Override public void robotPeriodic() { - + boolean conevcube; CommandScheduler.getInstance().run(); + if(GlobalVariables.gamePiece == 0){ + conevcube = true; + } + else{ + conevcube = false; + } + SmartDashboard.putBoolean("Game Piece", conevcube); + m_robotContainer.s_Swerve.updateRobotDirection(); } /** This function is called once each time the robot enters Disabled mode. */ @@ -89,7 +98,11 @@ public void disabledPeriodic() { } - + if (inputb.get() != resetFlag) { + resetFlag = inputb.get(); + m_robotContainer.armSub.homeGripperJointPos(); + m_robotContainer.armSub.homeTelescopePosition(); + } candlesub.disabledLed(); pdh.setSwitchableChannel(false); } @@ -98,7 +111,7 @@ public void disabledPeriodic() { @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - m_robotContainer.s_Swerve.resetOdometry(new Pose2d(Units.inchesToMeters(72), Units.inchesToMeters(176), new Rotation2d(0))); + // m_robotContainer.s_Swerve.resetOdometry(new Pose2d(Units.inchesToMeters(72), Units.inchesToMeters(176), new Rotation2d(0))); // schedule the autonomous command (example) if (m_autonomousCommand != null) { @@ -121,8 +134,8 @@ public void teleopInit() { // m_robotContainer.s_Swerve.normalizeOdometry(); // m_robotContainer.s_Swerve.autoZeroGyro(); - m_robotContainer.s_Swerve.zeroGyro(); - m_robotContainer.s_Swerve.resetOdometry(new Pose2d(Units.inchesToMeters(72), Units.inchesToMeters(176), new Rotation2d(0))); + // m_robotContainer.s_Swerve.zeroGyro(); + // m_robotContainer.s_Swerve.resetOdometry(new Pose2d(Units.inchesToMeters(72), Units.inchesToMeters(176), new Rotation2d(0))); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove @@ -144,7 +157,7 @@ public void teleopPeriodic() { Alliance alliance; alliance = DriverStation.getAlliance(); m_robotContainer.s_Swerve.updateOdometry(); -pdh.setSwitchableChannel(true); +pdh.setSwitchableChannel(false); // System.out.println(alliance); // candlesub.enabledLed(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2d150f8..f81d547 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,6 +79,7 @@ public class RobotContainer { private final Trigger panelLED3 = new JoystickButton(operatorPanel, 7); private final Trigger panelLED4 = new JoystickButton(operatorPanel, 6); private final Trigger panelLED5 = new JoystickButton(operatorPanel, 1); + private final Trigger panelLock = new JoystickButton(operatorPanel, 8); @@ -96,7 +97,6 @@ public class RobotContainer { // private final SequentialCommandGroup rightSideRed = new LeftSideAutoRed(s_Swerve, armSub, gripper); // private final SequentialCommandGroup balanceBlue = new BalanceAuto(s_Swerve, armSub, gripper); // private final SequentialCommandGroup balanceRed = new BalanceAutoRed(s_Swerve, armSub, gripper); - /* Subsystems */ @@ -126,16 +126,66 @@ public RobotContainer() { operatorPanel.setOutput(17, true ); operatorPanel.setOutput(18, true ); + HashMap eventMap = new HashMap<>(); + eventMap.put("marker1", new PrintCommand("Passed marker 1")); + eventMap.put("goarm", new AdaptiveArmMovement(armSub, ArmPositions.MID_SCORE_ADAPTIVE)); + eventMap.put("stowarm", new AutoAdaptiveArmMovement(armSub, ArmPositions.STOWED_ADAPTIVE)); + eventMap.put("placehigh", new AutoPlaceGamePiece(armSub, gripper, ArmPositions.HIGH_SCORE_ADAPTIVE)); + eventMap.put("switchcone", new ConeVCube(0)); + eventMap.put("switchcube", new ConeVCube(1)); + eventMap.put("cubepickup", new AutoGroundIntake(armSub, gripper, 1) ); + eventMap.put("conepickup", new AutoGroundIntake(armSub, gripper, 0) ); + eventMap.put("placemid", new AutoPlaceGamePiece(armSub, gripper, ArmPositions.MID_SCORE_ADAPTIVE)); + eventMap.put("holdstow", new AdaptiveArmMovement(armSub, ArmPositions.STOWED_ADAPTIVE)); + eventMap.put("balance", new AutoBalanceStation(s_Swerve)); + eventMap.put("placelow", new AutoPlaceGamePiece(armSub, gripper, ArmPositions.GROUND_SCORE_ADAPTIVE)); + eventMap.put("shootcone", new AutoShootCone(armSub, gripper)); + + SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder( + s_Swerve::getPose, // Pose2d supplier + s_Swerve::resetOdometry, // Pose2d consumer, used to reset odometry at the beginning of auto + Constants.Swerve.swerveKinematics, // SwerveDriveKinematics + new PIDConstants(5.0, 0.0, 0.0), // PID constants to correct for translation error (used to create the X and Y PID controllers) + new PIDConstants(1.5, 0.0, 0.0), // PID constants to correct for rotation error (used to create the rotation controller) + s_Swerve::setModuleStates, // Module states consumer used to output to the drive subsystem + eventMap, + true, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true + s_Swerve // The drive subsystem. Used to properly set the requirements of path following commands + ); + List CableProtector = PathPlanner.loadPathGroup("CableProtector", new PathConstraints(3 , 3), new PathConstraints(0.5, 1),new PathConstraints(3 , 3)); + List ThreePiece = PathPlanner.loadPathGroup("ThreePieceCone", new PathConstraints(3, 3)); + List TwoPieceClimb = PathPlanner.loadPathGroup("FullAuto", new PathConstraints(3, 3),new PathConstraints(3, 3),new PathConstraints(3, 3),new PathConstraints(3, 3), new PathConstraints(2, 2)); + List MiddleOnePiece = PathPlanner.loadPathGroup("MiddleOnePiece", new PathConstraints(2, 2)); + List MiddleTwoPiece = PathPlanner.loadPathGroup("MiddleTwoPiece", new PathConstraints(2, 2)); + List CableProtectorClimb = PathPlanner.loadPathGroup("CableProtectorCharge", new PathConstraints(3 , 3), new PathConstraints(0.5, 1),new PathConstraints(3 , 3)); + List CableProtectorDoubleHigh = PathPlanner.loadPathGroup("CableProtectorDoubleHigh", new PathConstraints(3.5 , 3), new PathConstraints(0.5, 1),new PathConstraints(3.5 , 3),new PathConstraints(3.3 , 3.3),new PathConstraints(0.5, 1),new PathConstraints(3.0 , 3.0)); + + + Command cCableProtector = autoBuilder.fullAuto(CableProtector); + Command cThreePiece = autoBuilder.fullAuto(ThreePiece); + Command cTwoPieceClimb = autoBuilder.fullAuto(TwoPieceClimb); + Command cMiddleOnePiece = autoBuilder.fullAuto(MiddleOnePiece).andThen(new AutoBalanceStation(s_Swerve)); + Command cMiddleTwoPiece = autoBuilder.fullAuto(MiddleTwoPiece).andThen(new AutoBalanceStation(s_Swerve)); + Command cCableProtectorClimb = autoBuilder.fullAuto(CableProtectorClimb); + Command cCableProtectorDoubleHigh = autoBuilder.fullAuto(CableProtectorDoubleHigh); + + + + m_chooser.setDefaultOption("Cable Protector Shoot 3", cCableProtector); + m_chooser.addOption("Three Piece", cThreePiece); + m_chooser.addOption("Two Piece Climb", cTwoPieceClimb); + m_chooser.addOption("Middle One Piece", cMiddleOnePiece); + m_chooser.addOption("Mid!dle Two Piece", cMiddleTwoPiece); + m_chooser.addOption("Cable Protector Climb", cCableProtectorClimb); + m_chooser.addOption("Cable Protector Double High", cCableProtectorDoubleHigh); - // m_chooser.setDefaultOption("Balance Blue", balanceBlue); - // m_chooser.addOption("Balance Red", balanceRed); // m_chooser.addOption("Left Blue", leftSideBlue); // m_chooser.addOption("Right Red", rightSideRed); SmartDashboard.putData(m_chooser); - s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, joystickPanel, translationAxis, strafeAxis, rotationAxis, fieldRelative, openLoop). - withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, joystickPanel, translationAxis, strafeAxis, rotationAxis, fieldRelative, openLoop).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + //s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, driver, 1,0,4,true,true)); // armSub.setDefaultCommand(new HoldArmPos(armSub, ArmPositions.STOWED_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); //candle.setDefaultCommand(new InstantCommand(() -> candle.setColor(operatorExtraRight.getAsBoolean()))); // armSub.setDefaultCommand(new MoveJointTemp(armSub, operatorPanel)); @@ -143,6 +193,7 @@ public RobotContainer() { // armSub.setDefaultCommand(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); armSub.setDefaultCommand(new AdaptiveArmMovement(armSub, ArmPositions.STOWED_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); candle.setDefaultCommand(new SetLedCommand(candle)); + gripper.setDefaultCommand(new ConstantHold(gripper)); configureButtonBindings(); } @@ -162,17 +213,18 @@ private void configureButtonBindings() { panelHigh.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.HIGH_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelMid.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelLow.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.GROUND_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - // panelStow.onTrue(new AutoPlaceGamePiece(armSub, gripper, ArmPositions.HIGH_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelStow.onTrue(new AutoPlaceGamePiece(armSub, gripper, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelGround.whileTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - panelShelf.whileTrue(new ShelfIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelShelf.whileTrue(new ShelfIntake(armSub).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelRelease.whileTrue(new AdaptiveOuttake(gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - panelRollers.whileTrue(new ManualRollers(gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelRollers.whileTrue(new ManualRollers(gripper, candle).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelEmptyRight.whileTrue(new UprightConeIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelEmptyLeft.whileTrue(new SingleSubstationIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); panelLED3.onTrue(new InstantCommand(() -> armSub.homeTelescopePosition())); panelLED4.onTrue(new InstantCommand(() -> armSub.homeGripperJointPos())); - - // panelStow.whileTrue(new AutoBalanceStation(s_Swerve, false, true).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelLock.whileTrue(new MoveJointTemp(armSub, operatorPanel).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelLED5.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.FRAME_PERIMETER).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // panelStow.onTrue(new AutoShootCone(armSub, gripper)); // panelStow.whileTrue(new FullArmPosition(armSub, 150, 50000, 9000, true)); // panelStow.whileFalse(new FullArmPosition(armSub, 0, 0, 0, false)); @@ -275,14 +327,14 @@ private void configureButtonBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - - List pathGroup = PathPlanner.loadPathGroup("ThreePiece", new PathConstraints(3, 3)); + /* + List pathGroup = PathPlanner.loadPathGroup("CableProtector", new PathConstraints(0.1, 1),new PathConstraints(4 , 2)); // This is just an example event map. It would be better to have a constant, global event map // in your code that will be used by all path following commands. HashMap eventMap = new HashMap<>(); eventMap.put("marker1", new PrintCommand("Passed marker 1")); -eventMap.put("goarm", new AdaptiveArmMovement(armSub, ArmPositions.HIGH_SCORE_ADAPTIVE)); +eventMap.put("goarm", new AdaptiveArmMovement(armSub, ArmPositions.MID_SCORE_ADAPTIVE)); eventMap.put("stowarm", new AutoAdaptiveArmMovement(armSub, ArmPositions.STOWED_ADAPTIVE)); eventMap.put("placehigh", new AutoPlaceGamePiece(armSub, gripper, ArmPositions.HIGH_SCORE_ADAPTIVE)); eventMap.put("switchcone", new ConeVCube(0)); @@ -309,7 +361,7 @@ public Command getAutonomousCommand() { new PIDConstants(1.5, 0.0, 0.0), // PID constants to correct for rotation error (used to create the rotation controller) s_Swerve::setModuleStates, // Module states consumer used to output to the drive subsystem eventMap, - false, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true + true, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true s_Swerve // The drive subsystem. Used to properly set the requirements of path following commands ); @@ -322,5 +374,7 @@ public Command getAutonomousCommand() { return fullAuto;//new ButterAutoTest(s_Swerve); // return new LeftSideAuto(s_Swerve, armSub, gripper); // return new LeftSideAutoRed(s_Swerve, armSub, gripper); + */ + return m_chooser.getSelected(); } } diff --git a/src/main/java/frc/robot/commands/AdaptiveArmMovement.java b/src/main/java/frc/robot/commands/AdaptiveArmMovement.java index bb42e26..0885e7a 100644 --- a/src/main/java/frc/robot/commands/AdaptiveArmMovement.java +++ b/src/main/java/frc/robot/commands/AdaptiveArmMovement.java @@ -31,18 +31,31 @@ public void execute() { armSub.armExtendPresetPositions(armPosition); armSub.jointRotPresetPositions(armPosition); - if(armSub.getTelescopePos() <= 10000){ + if(armSub.getTelescopePos() <= 10000 && armSub.getGripperJointPos() <= 5000){ armSub.armRotPresetPositions(armPosition); } } else{ - + if(GlobalVariables.robotDirection == false){ armSub.armRotPresetPositions(armPosition); armSub.jointRotPresetPositions(armPosition); if(armSub.getArmEncoderPosition() >= GlobalVariables.armRotGoal*0.75){ armSub.armExtendPresetPositions(armPosition); + } + } + else{ + + armSub.armRotPresetPositions(armPosition); + if(armSub.getArmEncoderPosition() >= GlobalVariables.armRotGoal*0.15){ + armSub.jointRotPresetPositions(armPosition); + + if(armSub.getGripperJointPos()>= 10000){ + armSub.armExtendPresetPositions(armPosition); + } + } + } } diff --git a/src/main/java/frc/robot/commands/AdaptiveOuttake.java b/src/main/java/frc/robot/commands/AdaptiveOuttake.java index 92a7a75..ca02dac 100644 --- a/src/main/java/frc/robot/commands/AdaptiveOuttake.java +++ b/src/main/java/frc/robot/commands/AdaptiveOuttake.java @@ -25,14 +25,15 @@ public void initialize() {} @Override public void execute() { if(GlobalVariables.gamePiece == 0){ - gripper.moveGripper(1); + gripper.moveGripper(0.5); } else{ - gripper.moveGripper(-1); + gripper.moveGripper(-0.6); } } // Called once the command ends or is interrupted. + @Override public void end(boolean interrupted) { gripper.moveGripper(0); diff --git a/src/main/java/frc/robot/commands/AutoBalanceStation.java b/src/main/java/frc/robot/commands/AutoBalanceStation.java index 8448fbe..ef7ea1b 100644 --- a/src/main/java/frc/robot/commands/AutoBalanceStation.java +++ b/src/main/java/frc/robot/commands/AutoBalanceStation.java @@ -26,61 +26,29 @@ private double mapdouble(double x, double in_min, double in_max, double out_min, /** * Driver control */ - public AutoBalanceStation(Swerve s_Swerve, boolean fieldRelative, boolean openLoop) { - this.s_Swerve = s_Swerve; + public AutoBalanceStation(Swerve s_Swerve) { + this.s_Swerve = s_Swerve ; addRequirements(s_Swerve); - pidController = new PIDController(0.8 , 0, 0); - pidController.setTolerance(1); - - this.fieldRelative = fieldRelative; - this.openLoop = openLoop; + pidController = new PIDController(0.1 , 0, 0); } @Override public void execute() { - double yAxis = pidController.calculate(s_Swerve.getPitch(), 0); + double value = 0; + if (s_Swerve.getPitch() > 2.5) { + value = 1; + } + if (s_Swerve.getPitch() < -2.5) { + value = -1; + } + double yAxis = pidController.calculate(value , 0); double xAxis = 0; double rAxis = 0; - System.out.println("AUTO BALANCING "); - - /* Deadbands */ - - if (Math.abs(yAxis) > Constants.stickDeadband) { - if (yAxis > 0){ - yAxis = mapdouble(yAxis, Constants.stickDeadband, 1, 0, 1); - } else { - yAxis = mapdouble(yAxis, -Constants.stickDeadband, -1, 0, -1); - } - } - else{ - yAxis = 0; - } - - if (Math.abs(xAxis) > Constants.stickDeadband) { - if (xAxis > 0){ - xAxis = mapdouble(xAxis, Constants.stickDeadband, 1, 0, 1); - } else { - xAxis = mapdouble(xAxis, -Constants.stickDeadband, -1, 0, -1); - } - } - else{ - xAxis = 0; - } - - if (Math.abs(rAxis) > Constants.stickDeadband) { - if (rAxis > 0){ - rAxis = mapdouble(rAxis, Constants.stickDeadband, 1, 0, 1); - } else { - rAxis = mapdouble(rAxis, -Constants.stickDeadband, -1, 0, -1); - } - } - else{ - rAxis = 0; - } + translation = new Translation2d(yAxis, xAxis).times(Constants.Swerve.maxSpeed); rotation = rAxis * Constants.Swerve.maxAngularVelocity; - s_Swerve.drive(translation, rotation, fieldRelative, openLoop); + s_Swerve.drive(translation, rotation, true, true); } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/AutoGroundIntake.java b/src/main/java/frc/robot/commands/AutoGroundIntake.java index 212fce9..dddacfe 100644 --- a/src/main/java/frc/robot/commands/AutoGroundIntake.java +++ b/src/main/java/frc/robot/commands/AutoGroundIntake.java @@ -39,7 +39,7 @@ public void initialize() { @Override public void execute() { if(gamePiece == 0){ - armSub.moveGripperJointPosition(31000); + armSub.moveGripperJointPosition(31000*armSub.multiplier); armSub.moveTelescopeArmPosition(8000); armSub.moveRotArmPosition(1); /* if((gripper.getDistanceSensorDist() <= 12)){ @@ -54,7 +54,7 @@ public void execute() { } else{ - armSub.moveGripperJointPosition(24000); + armSub.moveGripperJointPosition(22000*armSub.multiplier); armSub.moveTelescopeArmPosition(8000); armSub.moveRotArmPosition(1); /* if((gripper.getDistanceSensorDist() <= 12)){ diff --git a/src/main/java/frc/robot/commands/AutoPlaceGamePiece.java b/src/main/java/frc/robot/commands/AutoPlaceGamePiece.java index 45e1ad4..c5f4638 100644 --- a/src/main/java/frc/robot/commands/AutoPlaceGamePiece.java +++ b/src/main/java/frc/robot/commands/AutoPlaceGamePiece.java @@ -60,7 +60,7 @@ public void execute() { gripper.moveGripper(0.7); } else{ - gripper.moveGripper(-1); + gripper.moveGripper(-0.6); } if (timer >= 20){ diff --git a/src/main/java/frc/robot/commands/AutoShootCone.java b/src/main/java/frc/robot/commands/AutoShootCone.java new file mode 100644 index 0000000..b8e8707 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoShootCone.java @@ -0,0 +1,87 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.subsystems.ArmSub; +import frc.robot.subsystems.Gripper; + +public class AutoShootCone extends CommandBase { + /** Creates a new AdaptiveArmMovement. */ + ArmSub armSub; + Gripper gripper; + GlobalVariables.ArmPositions armPosition; + boolean finished; + int timer; + public AutoShootCone(ArmSub armSub, Gripper gripper) { + this.armSub = armSub; + this.gripper = gripper; + addRequirements(armSub, gripper); + finished = false; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + finished = false; + timer = 0; + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + armSub.moveRotArmPosition(90); + armSub.moveGripperJointPosition(11000*armSub.multiplier); + + if(armSub.getArmEncoderPosition() >= 80){ + + timer++; + if(/* gripper.getDistanceSensorDist() <= 12 */ timer>=15){ + if(GlobalVariables.gamePiece == 0){ + gripper.moveGripper(1); + } + else{ + gripper.moveGripper(-1); + + } + if (timer >= 30){ + gripper.moveGripper(0); + finished = true; + + + } + } + else{ + gripper.moveGripper(0); + } + } + else{ + finished = false; + } + } + + + + + + + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + gripper.moveGripper(0); + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return finished; + } +} diff --git a/src/main/java/frc/robot/commands/ConstantHold.java b/src/main/java/frc/robot/commands/ConstantHold.java new file mode 100644 index 0000000..06f3a91 --- /dev/null +++ b/src/main/java/frc/robot/commands/ConstantHold.java @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.subsystems.Gripper; + +public class ConstantHold extends CommandBase { + Gripper gripper; + /** Creates a new ConstantHold. */ + public ConstantHold(Gripper gripper) { + this.gripper = gripper; + addRequirements(gripper); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(GlobalVariables.gamePiece == 0){ + gripper.moveGripper(-0.075); + } + else{ + gripper.moveGripper(0.07); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index 92265ad..ae41b4f 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -28,7 +28,7 @@ public void initialize() {} @Override public void execute() { if(GlobalVariables.gamePiece == 0){ - armSub.moveGripperJointPosition(31000); + armSub.moveGripperJointPosition(31000 * armSub.multiplier); armSub.moveTelescopeArmPosition(8000); armSub.moveRotArmPosition(1); if((gripper.getDistanceSensorDist() <= 12)){ @@ -42,7 +42,7 @@ public void execute() { } else{ - armSub.moveGripperJointPosition(24000); + armSub.moveGripperJointPosition(22000*armSub.multiplier); armSub.moveTelescopeArmPosition(8000); armSub.moveRotArmPosition(1); if((gripper.getDistanceSensorDist() <= 12)){ diff --git a/src/main/java/frc/robot/commands/ManualRollers.java b/src/main/java/frc/robot/commands/ManualRollers.java index d340f3e..18d1620 100644 --- a/src/main/java/frc/robot/commands/ManualRollers.java +++ b/src/main/java/frc/robot/commands/ManualRollers.java @@ -4,16 +4,22 @@ package frc.robot.commands; +import com.ctre.phoenix.CANifier.LEDChannel; +import com.ctre.phoenix.led.CANdle; + import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.GlobalVariables; +import frc.robot.subsystems.CANdleSub; import frc.robot.subsystems.Gripper; public class ManualRollers extends CommandBase { /** Creates a new AdaptiveOuttake. */ Gripper gripper; - public ManualRollers(Gripper gripper) { + CANdleSub candlesub; + public ManualRollers(Gripper gripper, CANdleSub candlesub) { this.gripper = gripper; - addRequirements(gripper); + this.candlesub = candlesub; + addRequirements(gripper, candlesub); // Use addRequirements() here to declare subsystem dependencies. } @@ -30,6 +36,10 @@ public void execute() { else{ gripper.moveGripper(1); } + + if(gripper.getGripperMotorCurrent() >= 100){ + candlesub.pickupLed(); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/ShelfIntake.java b/src/main/java/frc/robot/commands/ShelfIntake.java index 5fafe62..0183d1c 100644 --- a/src/main/java/frc/robot/commands/ShelfIntake.java +++ b/src/main/java/frc/robot/commands/ShelfIntake.java @@ -13,10 +13,10 @@ public class ShelfIntake extends CommandBase { /** Creates a new AdaptiveArmMovement. */ ArmSub armSub; - Gripper gripper; - public ShelfIntake(ArmSub armSub, Gripper gripper) { + + public ShelfIntake(ArmSub armSub) { this.armSub = armSub; - this.gripper = gripper; + addRequirements(armSub); // Use addRequirements() here to declare subsystem dependencies. } @@ -37,35 +37,35 @@ public void execute() { armSub.armExtendPresetPositions(ArmPositions.SHELF_PICKUP_ADAPTIVE); } - if(GlobalVariables.gamePiece == 0){ - if((gripper.getDistanceSensorDist() <= 12)){ - gripper.moveGripper(0); + // if(GlobalVariables.gamePiece == 0){ + // if((gripper.getDistanceSensorDist() <= 12)){ + // gripper.moveGripper(0); - } - else{ - gripper.moveGripper(-1); + // } + // else{ + // gripper.moveGripper(-1); - } - } - else{ - if((gripper.getDistanceSensorDist() <= 12)){ - gripper.moveGripper(0); + // } + // } + // else{ + // if((gripper.getDistanceSensorDist() <= 12)){ + // gripper.moveGripper(0); - } - else{ - gripper.moveGripper(0.8); + // } + // else{ + // gripper.moveGripper(0.8); - } + // } } - } + // } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - gripper.moveGripper(0); + // gripper.moveGripper(0); } diff --git a/src/main/java/frc/robot/commands/SingleSubstationIntake.java b/src/main/java/frc/robot/commands/SingleSubstationIntake.java index 28174c7..5e3a78f 100644 --- a/src/main/java/frc/robot/commands/SingleSubstationIntake.java +++ b/src/main/java/frc/robot/commands/SingleSubstationIntake.java @@ -28,7 +28,7 @@ public void initialize() {} @Override public void execute() { if(GlobalVariables.gamePiece == 0){ - armSub.moveGripperJointPosition(25000); + armSub.moveGripperJointPosition(25000*armSub.multiplier); armSub.moveTelescopeArmPosition(0); armSub.moveRotArmPosition(40); if((gripper.getDistanceSensorDist() <= 12)){ diff --git a/src/main/java/frc/robot/commands/UprightConeIntake.java b/src/main/java/frc/robot/commands/UprightConeIntake.java index 5c933e9..e3c7123 100644 --- a/src/main/java/frc/robot/commands/UprightConeIntake.java +++ b/src/main/java/frc/robot/commands/UprightConeIntake.java @@ -28,9 +28,9 @@ public void initialize() {} @Override public void execute() { - armSub.moveGripperJointPosition(46000); + armSub.moveGripperJointPosition(47000 * armSub.multiplier); armSub.moveTelescopeArmPosition(0); - armSub.moveRotArmPosition(44); + armSub.moveRotArmPosition(47); if((gripper.getDistanceSensorDist() <= 12)){ gripper.moveGripper(0); diff --git a/src/main/java/frc/robot/subsystems/ArmSub.java b/src/main/java/frc/robot/subsystems/ArmSub.java index b0e1cf0..21d80f8 100644 --- a/src/main/java/frc/robot/subsystems/ArmSub.java +++ b/src/main/java/frc/robot/subsystems/ArmSub.java @@ -36,6 +36,8 @@ public class ArmSub extends SubsystemBase { public PIDController telescopePID; public CANCoder testCanCoder; + public PIDController slowArmPID; + private SupplyCurrentLimitConfiguration limit; public double rotMaxSpeedFor; @@ -44,6 +46,7 @@ public class ArmSub extends SubsystemBase { public double telMaxSpeedFor; public double telMaxSpeedRev; + public double multiplier; /** Creates a new ArmSub. */ @@ -53,19 +56,26 @@ public ArmSub() { testCanCoder = new CANCoder(14, "Karen"); testCanCoder.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360); testCanCoder.configSensorDirection(true); - testCanCoder.configMagnetOffset(-12); + testCanCoder.configMagnetOffset(150); + + multiplier = 0.677; rotMaxSpeedFor = 1; - rotMaxSpeedRev = 0.8; + rotMaxSpeedRev = 0.6; limit = new SupplyCurrentLimitConfiguration(true, 30, 30, 0); - armPID = new PIDController(0.022, 0.000, 0.000); + armPID = new PIDController(0.018, 0.000, 0.000); armPID.setTolerance(0.1); armPID.enableContinuousInput(0, 360); + + slowArmPID = new PIDController(0.007, 0.000, 0.000); + slowArmPID.setTolerance(0.1); + slowArmPID.enableContinuousInput(0, 360); + @@ -131,7 +141,7 @@ public ArmSub() { gripperJointFalcon.config_kF(0, 0.0); gripperJointFalcon.setNeutralMode(NeutralMode.Brake); gripperJointFalcon.configNeutralDeadband(0.001); - gripperJointFalcon.configMotionAcceleration(32000); + gripperJointFalcon.configMotionAcceleration(30000); gripperJointFalcon.configMotionCruiseVelocity(40000); gripperJointFalcon.setSelectedSensorPosition(0); gripperJointFalcon.configAllowableClosedloopError(0, 10); @@ -156,6 +166,10 @@ public void moveRotArmPosition(double degrees){ } + public void moveRotArmPositionSlow(double degrees){ + leftArm.set(ControlMode.PercentOutput, slowArmPID.calculate(testCanCoder.getAbsolutePosition(), degrees)); + } + /** * Moves the arm by percent output. @@ -183,6 +197,8 @@ public void moveGripperJointPosition(double position){ gripperJointFalcon.set(TalonFXControlMode.MotionMagic, position); } + + public void moveGripperJointPercentOutput(double percent ){ // gripperJointNeo.set(percent); gripperJointFalcon.set(TalonFXControlMode.PercentOutput, percent); @@ -231,9 +247,14 @@ public void armRotPresetPositions(GlobalVariables.ArmPositions position){ break; case STOWED_ADAPTIVE: - moveRotArmPosition(Constants.ArmRotationValues.armRotStow); + if(GlobalVariables.robotDirection){ + moveRotArmPositionSlow(Constants.ArmRotationValues.armRotStow); GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotStow; - + } + else{ + moveRotArmPosition(Constants.ArmRotationValues.armRotStow); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotStow; + } break; case GROUND_SCORE_ADAPTIVE: @@ -345,7 +366,10 @@ public void armRotPresetPositions(GlobalVariables.ArmPositions position){ } break; - + case FRAME_PERIMETER: + moveRotArmPosition(Constants.ArmRotationValues.framePerimeter); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.framePerimeter; + break; default: moveRotArmPosition(Constants.ArmRotationValues.armRotStow); @@ -492,7 +516,10 @@ public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ break; - + case FRAME_PERIMETER: + moveTelescopeArmPosition(Constants.ArmExtendValues.framePerimeter); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.framePerimeter; + break; default: moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendStow); @@ -508,20 +535,20 @@ public void jointRotPresetPositions(GlobalVariables.ArmPositions position){ case GROUND_PICKUP_ADAPTIVE: if(GlobalVariables.gamePiece == 0){ - moveGripperJointPosition(Constants.JointRotationValues.JointRotConePickup); + moveGripperJointPosition(Constants.JointRotationValues.JointRotConePickup * multiplier); } else{ - moveGripperJointPosition(Constants.JointRotationValues.JointRotCubePickup); + moveGripperJointPosition(Constants.JointRotationValues.JointRotCubePickup* multiplier); } break; case STOWED_ADAPTIVE: if(GlobalVariables.gamePiece == 0){ - moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCone* multiplier); } else{ - moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCube* multiplier); } break; @@ -529,18 +556,18 @@ public void jointRotPresetPositions(GlobalVariables.ArmPositions position){ case GROUND_SCORE_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - moveGripperJointPosition(Constants.JointRotationValues.JointRotForLowCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForLowCone* multiplier); } else{ - moveGripperJointPosition(Constants.JointRotationValues.JointRotForLowCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForLowCube* multiplier); } } else{ if(GlobalVariables.gamePiece == 0){ - moveGripperJointPosition(Constants.JointRotationValues.JointRotRevLowCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevLowCone* multiplier); } else{ - moveGripperJointPosition(Constants.JointRotationValues.JointRotRevLowCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevLowCube* multiplier); } } @@ -550,18 +577,18 @@ public void jointRotPresetPositions(GlobalVariables.ArmPositions position){ case SHELF_PICKUP_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - moveGripperJointPosition(Constants.JointRotationValues.JointRotForShelfCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForShelfCone* multiplier); } else{ - moveGripperJointPosition(Constants.JointRotationValues.JointRotForShelfCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForShelfCube* multiplier); } } else{ if(GlobalVariables.gamePiece == 0){ - moveGripperJointPosition(Constants.JointRotationValues.JointRotRevShelfCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevShelfCone* multiplier); } else{ - moveGripperJointPosition(Constants.JointRotationValues.JointRotRevShelfCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevShelfCube* multiplier); } } @@ -570,18 +597,18 @@ public void jointRotPresetPositions(GlobalVariables.ArmPositions position){ case MID_SCORE_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - moveGripperJointPosition(Constants.JointRotationValues.JointRotForMidCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForMidCone* multiplier); } else{ - moveGripperJointPosition(Constants.JointRotationValues.JointRotForMidCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForMidCube* multiplier); } } else{ if(GlobalVariables.gamePiece == 0){ - moveGripperJointPosition(Constants.JointRotationValues.JointRotRevMidCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevMidCone* multiplier); } else{ - moveGripperJointPosition(Constants.JointRotationValues.JointRotRevMidCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevMidCube* multiplier); } } @@ -591,32 +618,34 @@ public void jointRotPresetPositions(GlobalVariables.ArmPositions position){ case HIGH_SCORE_ADAPTIVE: if(GlobalVariables.robotDirection){ if(GlobalVariables.gamePiece == 0){ - moveGripperJointPosition(Constants.JointRotationValues.JointRotForHighCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForHighCone* multiplier); } else{ - moveGripperJointPosition(Constants.JointRotationValues.JointRotForHighCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotForHighCube* multiplier); } } else{ if(GlobalVariables.gamePiece == 0){ - moveGripperJointPosition(Constants.JointRotationValues.JointRotRevHighCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevHighCone* multiplier); } else{ - moveGripperJointPosition(Constants.JointRotationValues.JointRotRevHighCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevHighCube* multiplier); } } break; - + case FRAME_PERIMETER: + moveGripperJointPosition(Constants.JointRotationValues.framePerimeter * multiplier); + break; default: if(GlobalVariables.gamePiece == 0){ - moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCone); + moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCone* multiplier); } else{ - moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCube); + moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCube* multiplier); } break; diff --git a/src/main/java/frc/robot/subsystems/CANdleSub.java b/src/main/java/frc/robot/subsystems/CANdleSub.java index b25fb18..f7c5ce6 100644 --- a/src/main/java/frc/robot/subsystems/CANdleSub.java +++ b/src/main/java/frc/robot/subsystems/CANdleSub.java @@ -30,13 +30,17 @@ public CANdleSub() { } public void disabledLed(){ - m_candle.animate(new SingleFadeAnimation(255, 0, 0, 0, 0.6, numLed, ledOffset)); + m_candle.animate(new SingleFadeAnimation(200, 0, 0, 0, 0.6, numLed, ledOffset)); } public void enabledLed(){ m_candle.animate(new SingleFadeAnimation(230, 10, 10, 0, 0.7, numLed, ledOffset)); } + public void pickupLed(){ + m_candle.setLEDs(0, numLed, 0, 0, ledOffset, numLed); + } + public void setColor(boolean cone) { m_candle.clearAnimation(0); if (cone) { diff --git a/src/main/java/frc/robot/subsystems/Gripper.java b/src/main/java/frc/robot/subsystems/Gripper.java index 417fc1b..c7ec640 100644 --- a/src/main/java/frc/robot/subsystems/Gripper.java +++ b/src/main/java/frc/robot/subsystems/Gripper.java @@ -36,7 +36,7 @@ public Gripper() { - distanceSensor = new Rev2mDistanceSensor(Port.kMXP); + distanceSensor = new Rev2mDistanceSensor(Port.kMXP); distanceSensor.setAutomaticMode(true); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index c112f6d..49920d3 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -154,22 +154,22 @@ public void updateOdometry() { } public void updateRobotDirection(){ - if (DriverStation.getAlliance() == Alliance.Blue){ + // if (DriverStation.getAlliance() == Alliance.Blue){ if(gyro.getYaw()>=90 || gyro.getYaw() <= -90){ GlobalVariables.robotDirection = true; } else{ GlobalVariables.robotDirection = false; - } - } - else{ - if(gyro.getYaw()>=90 || gyro.getYaw() <= -90){ - GlobalVariables.robotDirection = false; - } - else{ - GlobalVariables.robotDirection = true; - } + // } } + // else{ + // if(gyro.getYaw()>=90 || gyro.getYaw() <= -90){ + // GlobalVariables.robotDirection = false; + // } + // else{ + // GlobalVariables.robotDirection = true; + // } + // } } @@ -270,7 +270,7 @@ public Rotation2d getYaw() { } public double getPitch(){ - return gyro.getRoll(); + return gyro.getPitch(); } public double getCamOffset(){ @@ -336,9 +336,9 @@ public double getCamOffset(){ @Override public void periodic(){ - updateRobotDirection(); + // updateRobotDirection(); // pcw.frontCamPipeline(2); - pcw.cameraPipelines(GlobalVariables.gamePiece); + // pcw.cameraPipelines(GlobalVariables.gamePiece); AprilTagGrid.setDefaultOption("Left Tag", gridTag1); AprilTagGrid.addOption("Mid Tag", gridTag2); @@ -365,6 +365,9 @@ public void periodic(){ SmartDashboard.putNumber("Gyro Yaw ", gyro.getYaw()); + SmartDashboard.putNumber("Gyro Pitch ", gyro.getPitch()); + SmartDashboard.putNumber("Gyro Roll ", gyro.getRoll()); + SmartDashboard.putNumber("AprilTag Yaw ", getAprilTagEstPosition().getRotation().getDegrees()); SmartDashboard.putBoolean("robot direction ", GlobalVariables.robotDirection); SmartDashboard.putNumber("Rear Cam offset ", pcw.getRearCamOffset());