diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6321cbe..2300828 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -95,7 +95,7 @@ public static final class IntakeConstants { // Intake Feedforward characterization constants public static final double ks = 0.1; public static final double kv = 5; - public static double percentPower = 2; + public static double percentPower = 0.3; } public static final class ArmConstants { @@ -154,7 +154,7 @@ public static final class Presets { } public static final class WristConstants { - public static final int kWristMotorId = 60; // ! Change before testing + public static final int kWristMotorId = 15; // ! Change before testing public static final int kCurrentLimit = 30; @@ -199,7 +199,9 @@ public static final class ClimberConstants { } public static final class SticksConstants { - public static final int kSticksMotorId = 10000; // ! Change before testing + public static final int kLeftStickMotorId = 19; + public static final int kRightStickMotorId = 18; + public static final int kCurrentLimit = 30; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 183d3da..3fdac28 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,9 @@ import frc.robot.commands.FeedForwardCharacterization; import frc.robot.commands.Positions.AmpShot; import frc.robot.commands.Positions.FloorPickup; +import frc.robot.commands.Positions.ShootClose; +import frc.robot.commands.Positions.ShootFar; +import frc.robot.commands.Positions.ShootMedium; import frc.robot.commands.Positions.SpeakerShot; import frc.robot.commands.Positions.StowPosition; import frc.robot.commands.Positions.Upwards; @@ -328,6 +331,10 @@ private void configureButtonBindings() { sClimber::stop, sClimber)); + operatorController.povDown().whileTrue(ShootFar.run(sArm, sArmElevator, sWrist)); + operatorController.povRight().whileTrue(ShootMedium.run(sArm, sArmElevator, sWrist)); + operatorController.povUp().whileTrue(ShootClose.run(sArm, sArmElevator, sWrist)); + // run straight up position when y is pressed on operator. Using command Upwards operatorController.y().whileTrue(Upwards.run(sArm, sArmElevator, sWrist)); diff --git a/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java b/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java index ab9d276..3b2a1b0 100644 --- a/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java @@ -27,7 +27,7 @@ import java.util.function.DoubleSupplier; public class DriveCommands { - private static final double DEADBAND = 0.1; + private static final double DEADBAND = 0.17; private DriveCommands() {} @@ -65,7 +65,7 @@ public static Command joystickDrive( new Rotation2d( MathUtil.applyDeadband(xSupplier.getAsDouble(), DEADBAND), MathUtil.applyDeadband(ySupplier.getAsDouble(), DEADBAND)); - omega = (MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND)) / 2; + omega = (MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND)); } // Square values @@ -87,7 +87,7 @@ public static Command joystickDrive( ChassisSpeeds.fromFieldRelativeSpeeds( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega * drive.getMaxAngularSpeedRadPerSec(), + omega * drive.getMaxAngularSpeedRadPerSec() / 2.3, isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation())); diff --git a/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java b/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java index ff439c0..91dde6d 100644 --- a/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java +++ b/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java @@ -59,14 +59,19 @@ public static Command intakeShooterDrive( revSpeed = MathUtil.applyDeadband(revSpeed, DEADBAND); double stickMagnitude = fwdSpeed - revSpeed; + stickMagnitude = + stickMagnitude * stickMagnitude * stickMagnitude; // more control over lower speeds int proximity = intake.getProximity(); - boolean noteDetected = proximity < 2047 && proximity > 1000; - // ! An educated guess, may cause problems. It's at its highest when - // ! close and the smallest when far. + boolean noteDetected = proximity > 50; if (noteDetected) { - intake.runPercentSpeed(0); + if (stickMagnitude > 0.15) { + intake.runPercentSpeed(0); + } else { + intake.runPercentSpeed( + Constants.IntakeConstants.percentPower * stickMagnitude); // allow for extake + } } else { intake.runPercentSpeed(Constants.IntakeConstants.percentPower * stickMagnitude); } diff --git a/src/main/java/frc/robot/commands/Positions/FloorPickup.java b/src/main/java/frc/robot/commands/Positions/FloorPickup.java index 620064d..fb1764b 100644 --- a/src/main/java/frc/robot/commands/Positions/FloorPickup.java +++ b/src/main/java/frc/robot/commands/Positions/FloorPickup.java @@ -12,7 +12,7 @@ public static Command run(Arm arm, ArmElevator armElevator, Wrist wrist) { return Commands.run( () -> { // Position preset settings - double ARMANGLE = 0.18; + double ARMANGLE = 0.205; double ARMELEVATORPOSITION = 0; double WRISTANGLE = 0; diff --git a/src/main/java/frc/robot/commands/Positions/ShootClose.java b/src/main/java/frc/robot/commands/Positions/ShootClose.java new file mode 100644 index 0000000..1dce96d --- /dev/null +++ b/src/main/java/frc/robot/commands/Positions/ShootClose.java @@ -0,0 +1,27 @@ +package frc.robot.commands.Positions; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.armElevator.ArmElevator; +import frc.robot.subsystems.wrist.Wrist; + +public class ShootClose { // bumper pressed against the wall + + public static Command run(Arm arm, ArmElevator armElevator, Wrist wrist) { + return Commands.run( + () -> { + // Position preset settings + double ARMANGLE = 0.4342; + double ARMELEVATORPOSITION = 0; + double WRISTANGLE = 0; + + arm.runTargetAngle(ARMANGLE); + armElevator.runTargetPosition(ARMELEVATORPOSITION); + wrist.runTargetAngle(WRISTANGLE); + }, + arm, + armElevator, + wrist); + } +} diff --git a/src/main/java/frc/robot/commands/Positions/ShootFar.java b/src/main/java/frc/robot/commands/Positions/ShootFar.java new file mode 100644 index 0000000..498ab58 --- /dev/null +++ b/src/main/java/frc/robot/commands/Positions/ShootFar.java @@ -0,0 +1,27 @@ +package frc.robot.commands.Positions; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.armElevator.ArmElevator; +import frc.robot.subsystems.wrist.Wrist; + +public class ShootFar { // two robots away from wall + + public static Command run(Arm arm, ArmElevator armElevator, Wrist wrist) { + return Commands.run( + () -> { + // Position preset settings + double ARMANGLE = 0.7917; + double ARMELEVATORPOSITION = 0; + double WRISTANGLE = 0; + + arm.runTargetAngle(ARMANGLE); + armElevator.runTargetPosition(ARMELEVATORPOSITION); + wrist.runTargetAngle(WRISTANGLE); + }, + arm, + armElevator, + wrist); + } +} diff --git a/src/main/java/frc/robot/commands/Positions/ShootMedium.java b/src/main/java/frc/robot/commands/Positions/ShootMedium.java new file mode 100644 index 0000000..4b57c4a --- /dev/null +++ b/src/main/java/frc/robot/commands/Positions/ShootMedium.java @@ -0,0 +1,27 @@ +package frc.robot.commands.Positions; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.armElevator.ArmElevator; +import frc.robot.subsystems.wrist.Wrist; + +public class ShootMedium { // one robot length away + + public static Command run(Arm arm, ArmElevator armElevator, Wrist wrist) { + return Commands.run( + () -> { + // Position preset settings + double ARMANGLE = 0.6429; + double ARMELEVATORPOSITION = 0; + double WRISTANGLE = 0; + + arm.runTargetAngle(ARMANGLE); + armElevator.runTargetPosition(ARMELEVATORPOSITION); + wrist.runTargetAngle(WRISTANGLE); + }, + arm, + armElevator, + wrist); + } +}