diff --git a/src/main/deploy/pathplanner/center_mobility.path b/src/main/deploy/pathplanner/center_mobility.path index 77cc004..34e0d4a 100644 --- a/src/main/deploy/pathplanner/center_mobility.path +++ b/src/main/deploy/pathplanner/center_mobility.path @@ -2,13 +2,13 @@ "waypoints": [ { "anchorPoint": { - "x": 1.0, - "y": 3.0 + "x": 1.8058163911466711, + "y": 0.4585108250077607 }, "prevControl": null, "nextControl": { - "x": 2.0, - "y": 3.0 + "x": 2.8058163911466787, + "y": 0.4585108250077607 }, "holonomicAngle": 0, "isReversal": false, @@ -24,19 +24,19 @@ }, { "anchorPoint": { - "x": 3.2257993235545075, - "y": 2.9976040783512325 + "x": 7.0900275255411636, + "y": 0.9205009278074111 }, "prevControl": { - "x": 2.43846492164065, - "y": 3.0023737159346475 + "x": 6.302693123627306, + "y": 0.9252705653908261 }, "nextControl": { - "x": 4.225780974640867, - "y": 2.9915462329755536 + "x": 6.302693123627306, + "y": 0.9252705653908261 }, "holonomicAngle": 0, - "isReversal": false, + "isReversal": true, "velOverride": 1.0, "isLocked": false, "isStopPoint": false, @@ -49,12 +49,37 @@ }, { "anchorPoint": { - "x": 6.162371757772435, - "y": 3.025877703201374 + "x": 1.8230968041797195, + "y": 1.068776149705077 + }, + "prevControl": { + "x": 2.268778005912175, + "y": 1.068776149705077 + }, + "nextControl": { + "x": 2.268778005912175, + "y": 1.068776149705077 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.8415394018288036, + "y": 2.0653572852284596 }, "prevControl": { - "x": 6.015742280723526, - "y": 3.175457471538993 + "x": 2.569189420969124, + "y": 2.0653572852284596 }, "nextControl": null, "holonomicAngle": 0, @@ -72,7 +97,7 @@ ], "maxVelocity": 6.0, "maxAcceleration": 3.0, - "isReversed": null, + "isReversed": false, "markers": [ { "position": 0.5117897727272727, diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 39ccf97..1c6589d 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -6,14 +6,12 @@ /*----------------------------------------------------------------------------*/ package org.usfirst.frc4904.robot; -import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import org.usfirst.frc4904.robot.humaninterface.drivers.NathanGain; import org.usfirst.frc4904.robot.humaninterface.operators.DefaultOperator; import org.usfirst.frc4904.robot.seenoevil.DriveSubsystem; import org.usfirst.frc4904.robot.seenoevil.RobotContainer2; -import org.usfirst.frc4904.robot.subsystems.arm.ArmPivotSubsystem; import org.usfirst.frc4904.standard.CommandRobotBase; import org.usfirst.frc4904.standard.humaninput.Driver; @@ -21,10 +19,7 @@ import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages; -import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; import static org.usfirst.frc4904.robot.Utils.nameCommand; @@ -33,10 +28,14 @@ public class Robot extends CommandRobotBase { private final RobotMap map = new RobotMap(); private final RobotContainer2 donttouchme = new RobotContainer2(RobotMap.Component.frontLeftWheelTalon, RobotMap.Component.backLeftWheelTalon, RobotMap.Component.frontRightWheelTalon, RobotMap.Component.backRightWheelTalon, RobotMap.Component.navx); - + private final NathanGain nathangain = new NathanGain(); private final Driver driver = new NathanGain(); private final org.usfirst.frc4904.standard.humaninput.Operator operator = new DefaultOperator(); + protected double scaleGain(double input, double gain, double exp) { + return Math.pow(Math.abs(input), exp) * gain * Math.signum(input); + } + @Override public void initialize() { } @@ -47,20 +46,21 @@ public void teleopInitialize() { if (RobotContainer2.Component.leftBTalonFX != null) RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Brake); if (RobotContainer2.Component.rightATalonFX != null) RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Brake); if (RobotContainer2.Component.rightBTalonFX != null) RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Brake); - RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.neutralOutput(); - RobotMap.Component.arm.armExtensionSubsystem.motor.setBrakeOnNeutral(); + RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.brake(); - RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setBrakeOnNeutral(); - RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.neutralOutput(); + RobotMap.Component.arm.armExtensionSubsystem.motor.brake(); + + driver.bindCommands(); + operator.bindCommands(); /*********************** * HAZMAT BLOCK START *************************/ // SATURDAY MORNING TEST - can you run drive train in queueline - donttouchme.m_robotDrive.m_leftMotors = null; - donttouchme.m_robotDrive.m_rightMotors = null; - donttouchme.m_robotDrive.m_drive = null; + // donttouchme.m_robotDrive.m_leftMotors = null; + // donttouchme.m_robotDrive.m_rightMotors = null; + // donttouchme.m_robotDrive.m_drive = null; DriveSubsystem.skuffedaf_teleop_initialized = true; @@ -91,37 +91,17 @@ public void teleopInitialize() { // RobotMap.Component.arm.c_posReturnToHomeUp(NathanGain.isFlippy) // )); - final DoubleSupplier pivot_getter = () -> RobotMap.HumanInput.Operator.joystick.getAxis(1) * 50; + final DoubleSupplier pivot_getter = () -> scaleGain(RobotMap.HumanInput.Operator.joystick.getAxis(1),60, 1); (new Trigger(() -> pivot_getter.getAsDouble() != 0)).onTrue( nameCommand("arm - teleop - armPivot operator override", RobotMap.Component.arm.armPivotSubsystem.c_controlAngularVelocity(pivot_getter::getAsDouble) ) ); - RobotMap.HumanInput.Operator.joystick.button3.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> -0.3)); - RobotMap.HumanInput.Operator.joystick.button3.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0)); - - RobotMap.HumanInput.Operator.joystick.button5.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0.3)); - RobotMap.HumanInput.Operator.joystick.button5.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0)); - - // Intake // FIXME: use nameCommand to make it cleaner with expresions (no varibales) - var cmdnull = RobotMap.Component.intake.c_holdVoltage(0); + // var cmdnull = RobotMap.Component.intake.c_holdVoltage(0); - var cmd2 = RobotMap.Component.intake.c_holdVoltage(-8); - cmd2.setName("Intake - manual intake activation"); - var cmdhold = RobotMap.Component.intake.c_holdVoltage(-2).withTimeout(0.5).andThen(RobotMap.Component.intake.c_holdVoltage(-1)); - - cmdnull.setName("Intake - deactivated"); - RobotMap.HumanInput.Operator.joystick.button2.onTrue(cmd2); - RobotMap.HumanInput.Operator.joystick.button2.onFalse(cmdhold); - - // Outtake - var cmd1 = RobotMap.Component.intake.c_holdVoltage(3); - cmd1.setName("Intake - manual outtake activation"); - RobotMap.HumanInput.Operator.joystick.button1.onTrue(cmd1); - RobotMap.HumanInput.Operator.joystick.button1.onFalse(cmdnull); } @Override @@ -136,12 +116,17 @@ public void teleopExecute() { // SmartDashboard.putNumber("Driver out", driver.getTurnSpeed()); - SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); + // SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); + // if (RobotMap.HumanInput.Operator.joystick.getPOV() != 0) { SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); SmartDashboard.putNumber("arm extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); + SmartDashboard.putNumber("zeroing", RobotMap.Component.arm.armExtensionSubsystem.motor.getSensorPositionRotations()); SmartDashboard.putNumber("arm pivot angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); + //} + // SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); - SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); + SmartDashboard.putData(RobotMap.Component.arm.armPivotSubsystem); + SmartDashboard.putData(RobotMap.Component.arm.armExtensionSubsystem); } @@ -172,7 +157,9 @@ public void autonomousInitialize() { // SATURDAY MORNING TEST: is the cube shooter auton gonna work // var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts); - var commnand = donttouchme.getAutonomousCommand(donttouchme.getTrajectory("straight_forward")); + // var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts); + + var commnand = donttouchme.simpleAuton(); //for calgames; just place a cone and leave the zone commnand.schedule(); } @@ -181,7 +168,7 @@ public void autonomousExecute() { // TODO remove logging - SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); + // SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); SmartDashboard.putString("pose string", donttouchme.m_robotDrive.getPose().toString()); SmartDashboard.putNumber("pose x", donttouchme.m_robotDrive.getPose().getX()); SmartDashboard.putNumber("pose y", donttouchme.m_robotDrive.getPose().getY()); @@ -191,8 +178,12 @@ public void autonomousExecute() { SmartDashboard.putNumber("armV extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); SmartDashboard.putNumber("arm pivot angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); - SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); + // SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); + // SmartDashboard.putData(RobotMap.Component.arm.armPivotSubsystem); + // SmartDashboard.putData(RobotMap.Component.arm.armExtensionSubsystem); + // SmartDashboard.putData(RobotMap.Component.arm); + // SmartDashboard.putData(donttouchme.m_robotDrive); } @Override @@ -232,18 +223,17 @@ public void testInitialize() { @Override public void testExecute() { RobotMap.Component.arm.armExtensionSubsystem.initializeEncoderPositions(0); - - } @Override public void alwaysExecute() { - // SmartDashboard.putNumber("Arm angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); - // SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); + SmartDashboard.putNumber("Arm angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); + SmartDashboard.putNumber("arm extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); + SmartDashboard.putNumber("intake speed", RobotMap.HumanInput.Operator.joystick.getAxis(3)); - + // SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); } } diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index cce0fa6..b38fd0f 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -28,6 +28,7 @@ import org.usfirst.frc4904.standard.subsystems.chassis.WestCoastDrive; import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorSubsystem; import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class RobotMap { @@ -41,6 +42,8 @@ public static class Port { public static class HumanInput { public static final int joystick = 0; public static final int xboxController = 1; + public static final int throttleJoystick = 2; + public static final int turnJoystick = 3; } // // blinky constants @@ -87,8 +90,8 @@ public static class Metrics { // // 2023-robot constants public static class Chassis { public static final double GEAR_RATIO = 496/45; // https://www.desmos.com/calculator/llz7giggcf - public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(5 + 1/8); - public static final double TRACK_WIDTH_METERS = .533; // +/- 0.5 inches + public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(5.12); + public static final double TRACK_WIDTH_METERS = .8713; // +/- 0.5 inches public static final double CHASSIS_LENGTH = Units.inchesToMeters(37); // +/- 0.5 inches } @@ -155,6 +158,8 @@ public static class Input { public static class HumanInput { public static class Driver { public static CustomCommandXbox xbox; + public static CustomCommandJoystick throttleJoystick; + public static CustomCommandJoystick turnJoystick; } public static class Operator { @@ -166,7 +171,9 @@ public RobotMap() { Component.navx = new AHRS(SerialPort.Port.kMXP); HumanInput.Driver.xbox = new CustomCommandXbox(Port.HumanInput.xboxController, 0.1); - HumanInput.Operator.joystick = new CustomCommandJoystick(Port.HumanInput.joystick, 0.05); + HumanInput.Operator.joystick = new CustomCommandJoystick(Port.HumanInput.joystick, 0.1); + HumanInput.Driver.throttleJoystick = new CustomCommandJoystick(Port.HumanInput.throttleJoystick, 0.1); + HumanInput.Driver.turnJoystick = new CustomCommandJoystick(Port.HumanInput.turnJoystick, 0.1); // // UDP things // try { // Component.robotUDP = new RobotUDP(Port.Network.LOCAL_SOCKET_ADDRESS, Port.Network.LOCALIZATION_ADDRESS); @@ -185,6 +192,11 @@ public RobotMap() { Component.backLeftWheelTalon = new CANTalonFX(Port.CANMotor.LEFT_DRIVE_A, InvertType.None); Component.frontLeftWheelTalon = new CANTalonFX(Port.CANMotor.LEFT_DRIVE_B, InvertType.None); + // Component.backRightWheelTalon.setSafetyEnabled(false); + // Component.frontRightWheelTalon.setSafetyEnabled(false); + // Component.backLeftWheelTalon.setSafetyEnabled(false); + // Component.frontLeftWheelTalon.setSafetyEnabled(false); + TalonMotorSubsystem leftDriveMotors = new TalonMotorSubsystem("left drive motors", NeutralMode.Brake, 0, Component.frontLeftWheelTalon, Component.backLeftWheelTalon); TalonMotorSubsystem rightDriveMotors = new TalonMotorSubsystem("right drive motors", NeutralMode.Brake, 0, Component.frontRightWheelTalon, Component.backRightWheelTalon); Component.chassis = new WestCoastDrive( @@ -205,7 +217,7 @@ public RobotMap() { TalonMotorSubsystem armRotationMotors = new TalonMotorSubsystem("Arm Pivot Subsystem", NeutralMode.Brake, 0, leftPivotMotor, rightPivotMotor); ArmExtensionSubsystem armExtensionSubsystem = new ArmExtensionSubsystem( new TalonMotorSubsystem("Arm Extension Subsystem", NeutralMode.Brake, 0, armExtensionMotor), - () -> ArmPivotSubsystem.motorRevsToAngle(armRotationMotors.getSensorPositionRotations()) + () -> ArmPivotSubsystem.motorRevsToAngle(armRotationMotors.getSensorPositionRotations() * 0.911 - 6.3) ); // Autonomous.autonCommand = Component.chassis.c_buildPathPlannerAuto( // PID.Drive.kS, PID.Drive.kV, PID.Drive.kA, diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index 4fb7e75..bf577f2 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -11,15 +11,15 @@ public class NathanGain extends Driver { public static final double SPEED_EXP = 2; public static final double Y_SPEED_SCALE = 1; - public static final double TURN_EXP = 3; + public static final double TURN_EXP = 4; public static final double TURN_SPEED_SCALE = 1; - public static final double NORMAL_SPEED_GAIN = 0.7; // TODO TUNE - public static final double NORMAL_TURN_GAIN = 0.3; + public static final double NORMAL_SPEED_GAIN = 0.5; // TODO TUNE + public static final double NORMAL_TURN_GAIN = 0.25; - public static final double PRECISE_SPEED_SCALE = 0.4; - public static final double PRECISE_TURN_SCALE = 0.3; + public static final double PRECISE_SPEED_SCALE = 0.2; + public static final double PRECISE_TURN_SCALE = 0.1; // public static final double SPEEDY_TURN_GAIN = 0.7; @@ -36,21 +36,32 @@ protected double scaleGain(double input, double gain, double exp) { @Override public void bindCommands() { - RobotMap.HumanInput.Driver.xbox.leftBumper().onTrue(new InstantCommand( + RobotMap.HumanInput.Driver.throttleJoystick.button1.onTrue(new InstantCommand( () -> { NathanGain.precisionScaleY = PRECISE_SPEED_SCALE; NathanGain.precisionScaleTurn = PRECISE_TURN_SCALE; } )); - RobotMap.HumanInput.Driver.xbox.leftBumper().onFalse(new InstantCommand(( + RobotMap.HumanInput.Driver.throttleJoystick.button1.onFalse(new InstantCommand(( ) -> { NathanGain.precisionScaleY = NORMAL_SPEED_GAIN; NathanGain.precisionScaleTurn = NORMAL_TURN_GAIN; } )); - RobotMap.HumanInput.Driver.xbox.y().onTrue(new InstantCommand(() -> NathanGain.precisionScaleY = 1)); - RobotMap.HumanInput.Driver.xbox.y().onFalse(new InstantCommand(() -> NathanGain.precisionScaleY = NORMAL_SPEED_GAIN)); + RobotMap.HumanInput.Driver.throttleJoystick.button1.onTrue(new InstantCommand(() -> NathanGain.precisionScaleY = 1)); + RobotMap.HumanInput.Driver.throttleJoystick.button1.onFalse(new InstantCommand(() -> NathanGain.precisionScaleY = NORMAL_SPEED_GAIN)); + + RobotMap.HumanInput.Driver.throttleJoystick.button2.onTrue(new InstantCommand(() -> NathanGain.precisionScaleY = 1)); + RobotMap.HumanInput.Driver.throttleJoystick.button2.onFalse(new InstantCommand(() -> NathanGain.precisionScaleY = NORMAL_SPEED_GAIN)); + + RobotMap.HumanInput.Driver.turnJoystick.button1.onTrue(new InstantCommand(() -> NathanGain.precisionScaleY = 1)); + RobotMap.HumanInput.Driver.turnJoystick.button1.onFalse(new InstantCommand(() -> NathanGain.precisionScaleY = NORMAL_SPEED_GAIN)); + + RobotMap.HumanInput.Driver.turnJoystick.button2.onTrue(new InstantCommand(() -> NathanGain.precisionScaleY = 1)); + RobotMap.HumanInput.Driver.turnJoystick.button1.onFalse(new InstantCommand(() -> NathanGain.precisionScaleY = NORMAL_SPEED_GAIN)); + + } @Override @@ -60,7 +71,7 @@ public double getX() { @Override public double getY() { - double rawSpeed = RobotMap.HumanInput.Driver.xbox.getRightTriggerAxis() - RobotMap.HumanInput.Driver.xbox.getLeftTriggerAxis(); + double rawSpeed = RobotMap.HumanInput.Driver.throttleJoystick.getY(); double speed; speed = scaleGain(rawSpeed, NathanGain.precisionScaleY, NathanGain.SPEED_EXP) * NathanGain.Y_SPEED_SCALE; @@ -75,12 +86,12 @@ public double getY() { @Override public double getTurnSpeed() { - double rawTurnSpeed = RobotMap.HumanInput.Driver.xbox.getLeftX(); + double rawTurnSpeed = RobotMap.HumanInput.Driver.turnJoystick.getX(); double turnSpeed = scaleGain(rawTurnSpeed, NathanGain.precisionScaleTurn, NathanGain.TURN_EXP) * NathanGain.TURN_SPEED_SCALE; // double precisionTurnSpeed = scaleGain(RobotMap.HumanInput.Driver.xbox.getRightX(), 0.08, 1.2); // double operatorControlTurnSpeed = scaleGain(RobotMap.HumanInput.Operator.joystick.getAxis(0), 0.2, 1.5); - if (NathanGain.isFlippy) return (turnSpeed) * -1; - return turnSpeed;// + precisionTurnSpeed; + if (NathanGain.isFlippy) return (turnSpeed); + return turnSpeed*-1;// + precisionTurnSpeed; //is mounted inverted so we need to revert by default } } diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java index 4893d54..f782ec4 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java @@ -2,8 +2,11 @@ import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.robot.humaninterface.drivers.NathanGain; +import org.usfirst.frc4904.standard.commands.TriggerCommandFactory; import org.usfirst.frc4904.standard.humaninput.Operator; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; public class DefaultOperator extends Operator { @@ -17,31 +20,52 @@ public DefaultOperator(String name) { @Override public void bindCommands() { + var joystick = RobotMap.HumanInput.Operator.joystick; + // manual extension and retraction + joystick.button3.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> -0.45)); + joystick.button3.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0)); + joystick.button5.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0.45)); + joystick.button5.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0)); - // Flippy button - // RobotMap.HumanInput.Operator.joystick.button12.onTrue(new InstantCommand( - // () -> { - // NathanGain.isFlippy = !NathanGain.isFlippy; - // } - // )); - // don't use flippy because confusion - - // // position + place cube - // RobotMap.HumanInput.Operator.joystick.button7.onTrue(RobotMap.Component.arm.placeCube(3)); - // RobotMap.HumanInput.Operator.joystick.button9.onTrue(RobotMap.Component.arm.placeCube(2)); - // RobotMap.HumanInput.Operator.joystick.button11.onTrue(RobotMap.Component.arm.placeCube(1)); - - // //position cone - // RobotMap.HumanInput.Operator.joystick.button8.onTrue(RobotMap.Component.arm.c_angleCones(3)); - // RobotMap.HumanInput.Operator.joystick.button10.onTrue(RobotMap.Component.arm.c_angleCones(2)); - // RobotMap.HumanInput.Operator.joystick.button12.onTrue(RobotMap.Component.arm.c_angleCones(1)); + // Intake + // FIXME: use nameCommand to make it cleaner with expresions (no varibales) + var zeroIntake = RobotMap.Component.intake.c_holdVoltage(9); + var runOuttake = RobotMap.Component.intake.c_holdVoltage(joystick.getAxis(3)); - // //shelf intake - // RobotMap.HumanInput.Operator.joystick.button6.onTrue(RobotMap.Component.arm.c_posIntakeShelf()); - - // //ground intake - // RobotMap.HumanInput.Operator.joystick.button4.onTrue(RobotMap.Component.arm.c_posIntakeGround()); + // intake + joystick.button2.onTrue(RobotMap.Component.intake.c_startIntake()); + joystick.button2.onFalse(RobotMap.Component.intake.c_holdItem()); + + // outtake + joystick.button1.onTrue(runOuttake); + joystick.button1.onFalse(zeroIntake); + + + // position + place cube + joystick.button7 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_shootCubes(3))); + joystick.button9 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_shootCubes(2))); + + // position cone + joystick.button8 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_shootCones(3))); + joystick.button10.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_shootCones(2))); + + + // intake positions + joystick.button6 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeShelf(null))); + joystick.button4 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeFloor(null))); + + // stow positions + joystick.button11.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posReturnToHomeDown(null))); + joystick.button12.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posReturnToHomeUp(null))); + + + // // intake positions + // joystick.button6 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeShelf(() -> RobotMap.Component.intake.c_startIntake()))); + // joystick.button4 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeFloor(() -> RobotMap.Component.intake.c_startIntake()))); + + // // stow positions + // joystick.button11.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posReturnToHomeDown(() -> RobotMap.Component.intake.c_holdItem()))); + // joystick.button12.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posReturnToHomeUp(() -> RobotMap.Component.intake.c_holdItem()))); } - } diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java index 0efaf3e..81ba365 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java @@ -78,9 +78,10 @@ public static final class DriveConstants { public static final class AutoConstants { public static final double kMaxSpeedMetersPerSecond = 3.0; // max like 6mps public static final double kMaxAccelerationMetersPerSecondSquared = 2.0; + public static final double kMaxCentripitalAcceleration = -1; // TODO set to something useful // Reasonable baseline values for a RAMSETE follower in units of meters and seconds public static final double kRamseteB = 2; - public static final double kRamseteZeta = 0.2; + public static final double kRamseteZeta = 0.127; } } diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java index cfc5143..d7c5317 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java @@ -102,7 +102,7 @@ public void periodic() { // RobotContainer2.Component.rightATalonFX.feed(); // RobotContainer2.Component.rightBTalonFX.feed(); - + m_drive.feed(); // m_leftEncoder.debug(); // m_rightEncoder.debug(); } diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index de566fe..bcfe553 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.TrajectoryUtil; +import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -27,6 +28,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -41,6 +44,7 @@ import org.usfirst.frc4904.robot.seenoevil.Constants.AutoConstants; import org.usfirst.frc4904.robot.seenoevil.Constants.DriveConstants; +import org.usfirst.frc4904.standard.commands.TriggerCommandFactory; import org.usfirst.frc4904.standard.custom.sensors.NavX; import org.usfirst.frc4904.robot.Robot; import org.usfirst.frc4904.robot.RobotMap; @@ -61,56 +65,58 @@ */ public class RobotContainer2 { - private static final double placeholderconstant = 0; //TODO: add value - private static TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - AutoConstants.kMaxSpeedMetersPerSecond, - AutoConstants.kMaxAccelerationMetersPerSecondSquared) - // Add kinematics to ensure max speed is actually obeyed - .setKinematics(DriveConstants.kDriveKinematics) - // Apply the voltage constraint - .addConstraint(new DifferentialDriveVoltageConstraint( - new SimpleMotorFeedforward( - DriveConstants.ksVolts, - DriveConstants.kvVoltSecondsPerMeter, - DriveConstants.kaVoltSecondsSquaredPerMeter), - DriveConstants.kDriveKinematics, - 10)); - private static TrajectoryConfig trajectoryConfigReversed = new TrajectoryConfig( - AutoConstants.kMaxSpeedMetersPerSecond, - AutoConstants.kMaxAccelerationMetersPerSecondSquared) - .setKinematics(DriveConstants.kDriveKinematics) - .addConstraint(new DifferentialDriveVoltageConstraint( - new SimpleMotorFeedforward( - DriveConstants.ksVolts, - DriveConstants.kvVoltSecondsPerMeter, - DriveConstants.kaVoltSecondsSquaredPerMeter), - DriveConstants.kDriveKinematics, - 10)) - .setReversed(true); - private static TrajectoryConfig trajectoryConfigSlow = new TrajectoryConfig( - AutoConstants.kMaxSpeedMetersPerSecond/3, - AutoConstants.kMaxAccelerationMetersPerSecondSquared/2) - .setKinematics(DriveConstants.kDriveKinematics) - .addConstraint(new DifferentialDriveVoltageConstraint( - new SimpleMotorFeedforward( - DriveConstants.ksVolts, - DriveConstants.kvVoltSecondsPerMeter, - DriveConstants.kaVoltSecondsSquaredPerMeter), - DriveConstants.kDriveKinematics, - 10)) - .setReversed(false); - private static TrajectoryConfig trajectoryConfigSlowReversed = new TrajectoryConfig( - AutoConstants.kMaxSpeedMetersPerSecond/3, - AutoConstants.kMaxAccelerationMetersPerSecondSquared/2) - .setKinematics(DriveConstants.kDriveKinematics) - .addConstraint(new DifferentialDriveVoltageConstraint( - new SimpleMotorFeedforward( - DriveConstants.ksVolts, - DriveConstants.kvVoltSecondsPerMeter, - DriveConstants.kaVoltSecondsSquaredPerMeter), - DriveConstants.kDriveKinematics, - 10)) - .setReversed(true); + private static final double placeholderconstant = 0; //TODO: add value + private static TrajectoryConfig trajectoryConfig = new TrajectoryConfig( + AutoConstants.kMaxSpeedMetersPerSecond, + AutoConstants.kMaxAccelerationMetersPerSecondSquared) + // Add kinematics to ensure max speed is actually obeyed + .setKinematics(DriveConstants.kDriveKinematics) + // Apply the voltage constraint + .addConstraint(new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward( + DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerMeter, + DriveConstants.kaVoltSecondsSquaredPerMeter + ), + DriveConstants.kDriveKinematics, + 10 + )); + private static TrajectoryConfig trajectoryConfigReversed = new TrajectoryConfig( + AutoConstants.kMaxSpeedMetersPerSecond, + AutoConstants.kMaxAccelerationMetersPerSecondSquared) + .setKinematics(DriveConstants.kDriveKinematics) + .addConstraint(new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward( + DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerMeter, + DriveConstants.kaVoltSecondsSquaredPerMeter), + DriveConstants.kDriveKinematics, + 10)) + .setReversed(true); + private static TrajectoryConfig trajectoryConfigSlow = new TrajectoryConfig( + AutoConstants.kMaxSpeedMetersPerSecond / 3, + AutoConstants.kMaxAccelerationMetersPerSecondSquared / 2) + .setKinematics(DriveConstants.kDriveKinematics) + .addConstraint(new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward( + DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerMeter, + DriveConstants.kaVoltSecondsSquaredPerMeter), + DriveConstants.kDriveKinematics, + 10)) + .setReversed(false); + private static TrajectoryConfig trajectoryConfigSlowReversed = new TrajectoryConfig( + AutoConstants.kMaxSpeedMetersPerSecond / 3, + AutoConstants.kMaxAccelerationMetersPerSecondSquared / 2) + .setKinematics(DriveConstants.kDriveKinematics) + .addConstraint(new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward( + DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerMeter, + DriveConstants.kaVoltSecondsSquaredPerMeter), + DriveConstants.kDriveKinematics, + 10)) + .setReversed(true); private static Map trajectories = Map.ofEntries( entry("sickle", TrajectoryGenerator.generateTrajectory( @@ -126,14 +132,14 @@ public class RobotContainer2 { ), entry("straight_forward", TrajectoryGenerator.generateTrajectory( new Pose2d(0, 0, new Rotation2d(0)), - List.of(new Translation2d(1, 0)), - new Pose2d(6, 0, new Rotation2d(0)), + List.of(),//List.of(new Translation2d(2, 0)), + new Pose2d(2, 0, new Rotation2d(0)), trajectoryConfig )), entry("straight_backward", TrajectoryGenerator.generateTrajectory( new Pose2d(0, 0, new Rotation2d(Math.PI)), - List.of(new Translation2d(1, 0)), - new Pose2d(6, 0, new Rotation2d(Math.PI)), + List.of(), + new Pose2d(2, 0, new Rotation2d(Math.PI)), trajectoryConfigReversed )), entry("turn_right", TrajectoryGenerator.generateTrajectory( @@ -155,110 +161,128 @@ public class RobotContainer2 { trajectoryConfigReversed )), - //new auton - entry("to_ramp", TrajectoryGenerator.generateTrajectory( - new Pose2d(0,0, new Rotation2d(0)), - List.of(), - new Pose2d(Units.inchesToMeters(24.19-.5),0,new Rotation2d(0)), - trajectoryConfig - )), - - entry("go_over_ramp", TrajectoryGenerator.generateTrajectory( - new Pose2d(0,0,new Rotation2d(0)), - List.of(), - new Pose2d(Units.inchesToMeters(118.02-.5)+placeholderconstant,0,new Rotation2d(0)), - trajectoryConfig - )), - entry("ramp_start", TrajectoryGenerator.generateTrajectory( - new Pose2d(0,0,new Rotation2d(0)), - List.of(), - new Pose2d(Units.inchesToMeters(5.001)+placeholderconstant,0,new Rotation2d(0)), - trajectoryConfigSlow - )), - entry("angle_ramp_backward", TrajectoryGenerator.generateTrajectory( - new Pose2d(0,0,new Rotation2d(Math.PI)), - List.of(), - new Pose2d(Units.inchesToMeters(5.001)+placeholderconstant,0,new Rotation2d(Math.PI)), - trajectoryConfigSlowReversed - )), - entry("go_middle_ramp", TrajectoryGenerator.generateTrajectory( - new Pose2d(0,0,new Rotation2d(Math.PI)), - List.of(), - new Pose2d(Units.inchesToMeters(53.5),0,new Rotation2d(Math.PI)), - trajectoryConfigReversed - )) + // new auton + entry("to_ramp", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of(), + new Pose2d(Units.inchesToMeters(24.19 - .5), 0, new Rotation2d(0)), + trajectoryConfig)), + + entry("go_over_ramp", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of(), + new Pose2d(Units.inchesToMeters(118.02 - .5) + placeholderconstant, 0, new Rotation2d(0)), + trajectoryConfig)), + entry("ramp_start", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of(), + new Pose2d(Units.inchesToMeters(5.001) + placeholderconstant, 0, new Rotation2d(0)), + trajectoryConfigSlow)), + entry("angle_ramp_backward", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(Math.PI)), + List.of(), + new Pose2d(Units.inchesToMeters(5.001) + placeholderconstant, 0, new Rotation2d(Math.PI)), + trajectoryConfigSlowReversed)), + entry("go_middle_ramp", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(Math.PI)), + List.of(), + new Pose2d(Units.inchesToMeters(53.5), 0, new Rotation2d(Math.PI)), + trajectoryConfigReversed)), + entry("go_to_pickup_next", TrajectoryGenerator.generateTrajectory( //from cone place to cube pickup + new Pose2d(0, 0, new Rotation2d(0)), + List.of(), + new Pose2d(4.7625, 0.45085, new Rotation2d(0)), //x is 15 foot 7.5, y is 17.75 inches + trajectoryConfig)), + entry("from_pickup_to_place", TrajectoryGenerator.generateTrajectory( //from cube pickup to cube node + new Pose2d(0, 0, new Rotation2d(Math.PI)), + List.of(),//same x as last time, little extra is to straighten out, could be tuned + new Pose2d(4.7625+0.1, 0.15, new Rotation2d(Math.PI)),//y is 15 cm to get to the cube node + trajectoryConfigReversed)), + entry("from_cube_place_to_ramp_edge", TrajectoryGenerator.generateTrajectory( //very curvy, might not work if we cant physically turn fast enough + new Pose2d(0, 0, new Rotation2d(0)), //y is 15 cm to get to the cube node + List.of(), + new Pose2d(0.503+0.4, 0.95, new Rotation2d(0)),//TODO: 0.4 is extra to get onto ramp, both needs tuning and we need to now how far we get onto ramp before stalling out + trajectoryConfig)),//I chose 0.4 bc its the length of the first section of the ramp, but other values might be better + entry("onto_ramp", TrajectoryGenerator.generateTrajectory( //balancing on ramp, NEEDS TUNING + new Pose2d(0, 0, new Rotation2d(0)), + List.of(), //chargestation has 2 parts, ramp and platform. 0.61 to balance form robot center at platform start + new Pose2d(0.61+0.2, 0, new Rotation2d(0)),//TODO: 0.2 is extra depending on how far onto the ramp we get stuck -- NEEDS TUNING + trajectoryConfig))); + + + + + public static class Component { + public static WPI_TalonFX leftATalonFX; + public static WPI_TalonFX leftBTalonFX; + public static WPI_TalonFX rightATalonFX; + public static WPI_TalonFX rightBTalonFX; + public static WPI_TalonFX testTalon; + } - ); + // The robot's subsystems + // motors - public static class Component { - public static WPI_TalonFX leftATalonFX; - public static WPI_TalonFX leftBTalonFX; - public static WPI_TalonFX rightATalonFX; - public static WPI_TalonFX rightBTalonFX; - public static WPI_TalonFX testTalon; - } - - // The robot's subsystems - // motors - - public final DriveSubsystem m_robotDrive; - // public final XboxController m_driverController; - - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer2(WPI_TalonFX leftATalonFX, WPI_TalonFX leftBTalonFX, WPI_TalonFX rightATalonFX, WPI_TalonFX rightBTalonFX, AHRS navx) { - - // The driver's controller - // m_driverController = new XboxController(OIConstants.kDriverControllerPort); - - // Configure the button bindings - Component.leftATalonFX = leftATalonFX; - Component.leftBTalonFX = leftBTalonFX; - Component.rightATalonFX = rightATalonFX; - Component.rightBTalonFX = rightBTalonFX; - - - RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Coast); - RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Coast); - RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Coast); - RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Coast); - - Component.testTalon = new WPI_TalonFX(1); - // Component.leftATalonFX.setInverted(true); - // Component.leftBTalonFX.setInverted(true); - - this.m_robotDrive = new DriveSubsystem(navx); - - // Configure default commands - // Set the default drive command to split-stick arcade drive - // // A split-stick arcade command, with forward/backward controlled by the left - // // hand, and turning controlled by the right. - // new RunCommand( - // () -> m_robotDrive.arcadeDrive( - // -m_driverController.getLeftY(), -m_driverController.getRightX()), - // m_robotDrive)); - } - - public Command getAutonomousCommand(Trajectory trajectory) { - // RamseteCommandDebug ramseteCommand = new RamseteCommandDebug( - RamseteController EEEE = new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta); - EEEE.setEnabled(false); - RamseteCommand ramseteCommand = new RamseteCommand( - trajectory, - m_robotDrive::getPose, - EEEE, - new SimpleMotorFeedforward( - DriveConstants.ksVolts, - DriveConstants.kvVoltSecondsPerMeter, - DriveConstants.kaVoltSecondsSquaredPerMeter), - DriveConstants.kDriveKinematics, - m_robotDrive::getWheelSpeeds, - // new PIDController(DriveConstants.kPDriveVel, 0, 0), new PIDController(DriveConstants.kPDriveVel, 0, 0), - new PIDController(0, 0, 0), new PIDController(0, 0, 0), - // RamseteCommand passes volts to the callback - m_robotDrive::tankDriveVolts, - m_robotDrive); + public final DriveSubsystem m_robotDrive; + // public final XboxController m_driverController; + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer2(WPI_TalonFX leftATalonFX, WPI_TalonFX leftBTalonFX, WPI_TalonFX rightATalonFX, + WPI_TalonFX rightBTalonFX, AHRS navx) { + + // The driver's controller + // m_driverController = new XboxController(OIConstants.kDriverControllerPort); + + // Configure the button bindings + Component.leftATalonFX = leftATalonFX; + Component.leftBTalonFX = leftBTalonFX; + Component.rightATalonFX = rightATalonFX; + Component.rightBTalonFX = rightBTalonFX; + + RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Coast); + RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Coast); + RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Coast); + RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Coast); + + Component.testTalon = new WPI_TalonFX(1); + // Component.leftATalonFX.setInverted(true); + // Component.leftBTalonFX.setInverted(true); + + this.m_robotDrive = new DriveSubsystem(navx); + + // Configure default commands + // Set the default drive command to split-stick arcade drive + // // A split-stick arcade command, with forward/backward controlled by the left + // // hand, and turning controlled by the right. + // new RunCommand( + // () -> m_robotDrive.arcadeDrive( + // -m_driverController.getLeftY(), -m_driverController.getRightX()), + // m_robotDrive)); + } + + public Command getAutonomousCommand(Trajectory trajectory) { + // RamseteCommandDebug ramseteCommand = new RamseteCommandDebug( + RamseteController EEEE = new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta); + EEEE.setEnabled(false); + RamseteCommand ramseteCommand = new RamseteCommand( + trajectory, + m_robotDrive::getPose, + EEEE, + new SimpleMotorFeedforward( + DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerMeter, + DriveConstants.kaVoltSecondsSquaredPerMeter), + DriveConstants.kDriveKinematics, + m_robotDrive::getWheelSpeeds, + // new PIDController(DriveConstants.kPDriveVel, 0, 0), new + // PIDController(DriveConstants.kPDriveVel, 0, 0), + new PIDController(0, 0, 0), new PIDController(0, 0, 0), + // RamseteCommand passes volts to the callback + m_robotDrive::tankDriveVolts, + m_robotDrive + ); // Reset odometry to the starting pose of the trajectory. // Pose2d initialPose = trajectory.getInitialPose(); @@ -268,9 +292,29 @@ public Command getAutonomousCommand(Trajectory trajectory) { // return Commands.run(() -> m_robotDrive.tankDriveVolts(1, 1), m_robotDrive); //return Commands.runOnce(() -> m_robotDrive.arcadeDrive(0.5, 0), m_robotDrive); //return Commands.runOnce(() -> Component.testTalon.setVoltage(6)); - return Commands.runOnce(() -> m_robotDrive.resetOdometry(trajectory.getInitialPose()) - ) .andThen( ramseteCommand) - .andThen(() -> m_robotDrive.tankDriveVolts(0, 0)); + // m_robotDrive.resetOdometry(trajectory.getInitialPose()); + // return Commands.runOnce(() -> m_robotDrive.resetOdometry(trajectory.getInitialPose()) + // ) .andThen( ramseteCommand) + // .andThen(() -> m_robotDrive.tankDriveVolts(0, 0)); + + + var cmd = new SequentialCommandGroup( + Commands.runOnce(() -> { + Pose2d init = trajectory.getInitialPose(); + m_robotDrive.resetOdometry(new Pose2d(init.getX(), init.getY(), new Rotation2d(0))); + SmartDashboard.putString("Trajg", init.toString()); + }), + ramseteCommand, + Commands.runOnce(() -> m_robotDrive.tankDriveVolts(0, 0)), + Commands.runOnce(() -> { + Pose2d init = trajectory.getInitialPose(); + // m_robotDrive.resetOdometry(init); + m_robotDrive.resetOdometry(new Pose2d(init.getX(), init.getY(), new Rotation2d(0))); + + SmartDashboard.putString("Trajg2", init.toString()); + }) ); + cmd.addRequirements(m_robotDrive); + return cmd; } /** @@ -290,133 +334,157 @@ public Trajectory getTrajectory(String trajectoryName) { // Trajectory trajectory = new Trajectory(); // try { - // Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); - // trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); - // System.out.println("v\nv\nv\nv\ntrajectory total time" + String.valueOf(trajectory.getTotalTimeSeconds())); + // Path trajectoryPath = + // Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); + // trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); + // System.out.println("v\nv\nv\nv\ntrajectory total time" + + // String.valueOf(trajectory.getTotalTimeSeconds())); // } catch (IOException ex) { - // System.out.println("SHEEEEEESH"); - // DriverStation.reportError("Unable to open trajectory: " + trajectoryJSON, ex.getStackTrace()); + // System.out.println("SHEEEEEESH"); + // DriverStation.reportError("Unable to open trajectory: " + trajectoryJSON, + // ex.getStackTrace()); // } - // return exampleTrajectory; return trajectories.get(trajectoryName); } - public Command balanceAuton(Supplier wheelSpeeds, BiConsumer outputVolts){ - var command = new SequentialCommandGroup( - //1. Position arm to place gamepiece - // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state + public Command balanceAuton(Supplier wheelSpeeds, BiConsumer outputVolts) { + var command = new SequentialCommandGroup( + // 1. Position arm to place gamepiece + // TODO: options: either place the game picee, or try to flip over, shoot, and + // then come back so that we are in the same state - // implement going over and shooting a cone? + // implement going over and shooting a cone? new ParallelCommandGroup( - //3. Retract arm + // 3. Retract arm // RobotMap.Component.arm.c_posReturnToHomeDown(false), new SequentialCommandGroup( - new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? - //4. Drive forward past ramp + new WaitCommand(1), // TODO: set wait time to allow arm to get started before moving? + // 4. Drive forward past ramp getAutonomousCommand(getTrajectory("past_ramp")), - //5. Drive back to get partially on ramp + // 5. Drive back to get partially on ramp getAutonomousCommand(getTrajectory("back_to_ramp")) ) ) - // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) - //6. balance code here + // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) + // 6. balance code here ); - + return command; - } + } - public Command balanceAutonAndShootCube(Supplier wheelSpeeds, BiConsumer outputVolts){ - - var command = new SequentialCommandGroup( - //1. Position arm to place gamepiece - // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state - - // implement going over and shooting a cone? - - new ParallelCommandGroup( - //3. Retract arm - // RobotMap.Component.arm.c_posReturnToHomeDown(false), - RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst() - .withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()) - .andThen(new WaitCommand(0.8)) - .andThen(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getSecond()).andThen(new InstantCommand(() -> RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setVoltage(0))) - ), - new SequentialCommandGroup( - new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 100).getSecond()), - RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.8).andThen(RobotMap.Component.intake.c_holdVoltage(0)) - ), - new SequentialCommandGroup( - new WaitCommand(2.5), //TODO: set wait time to allow arm to get started before moving? - //4. Drive forward past ramp - getAutonomousCommand(getTrajectory("past_ramp")), - - //5. Drive back to get partially on ramp - getAutonomousCommand(getTrajectory("back_to_ramp")) - ) - ) - // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) - //6. balance code here - ); - - return command; - } - - public Command notBalanceAuton(){ - var command = new SequentialCommandGroup( - //1. Position arm to place gamepiece - RobotMap.Component.arm.placeCube(2, true) //TODO: set actual timeout - , - new ParallelCommandGroup( - //3. Retract arm - RobotMap.Component.arm.c_posReturnToHomeUp(false), - new SequentialCommandGroup( - new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? - //4. Drive forward past ramp - getAutonomousCommand(getTrajectory("past_ramp")) - ) - ) + public Command balanceAutonAndShootCube(Supplier wheelSpeeds, + BiConsumer outputVolts) { + var cmd = RobotMap.Component.arm.c_shootCubes(4, () -> new SequentialCommandGroup( + getAutonomousCommand(getTrajectory("past_ramp")), + getAutonomousCommand(getTrajectory("back_to_ramp")) + )); // high shelf flippy + + return cmd; + } + + // public Command notBalanceAuton(){ + // var command = new SequentialCommandGroup( + // //1. Position arm to place gamepiece + // RobotMap.Component.arm.placeCube(2, true) //TODO: set actual timeout + // , + // new ParallelCommandGroup( + // //3. Retract arm + // RobotMap.Component.arm.c_posReturnToHomeUp(false), + // new SequentialCommandGroup( + // new WaitCommand(1), //TODO: set wait time to allow arm to get started before + // moving? + // //4. Drive forward past ramp + // getAutonomousCommand(getTrajectory("past_ramp")) + // ) + // ) + // ); + + // return command; + // } + + public Command newAuton() { + var cmd = RobotMap.Component.arm.c_shootCubes(4, () -> new SequentialCommandGroup( + getAutonomousCommand(getTrajectory("to_ramp")), + getAutonomousCommand(getTrajectory("angle_ramp_forward")), + new WaitCommand(1), + getAutonomousCommand(getTrajectory("go_over_ramp")), + getAutonomousCommand(getTrajectory("angle_ramp_backward")), + new WaitCommand(1), + getAutonomousCommand(getTrajectory("go_middle_ramp")) + )); + + return cmd; + } + + public Command twoPieceAuton() { // shoot cone, grab cube, shoot cube, doesn't balance + var cmd = RobotMap.Component.arm.c_shootCones(4, // shoot cone + () -> new SequentialCommandGroup( + new ParallelCommandGroup( // then, in parallel + getAutonomousCommand(getTrajectory("go_to_pickup_next")), // go to pickup location + (new WaitCommand(3)).andThen(RobotMap.Component.intake.c_holdVoltage(-8)) // start intaking when we get close + ), + new TriggerCommandFactory( // then, as a separated parallel schedule, + () -> RobotMap.Component.intake.c_holdVoltage(-1), // hold the game piece in + () -> getAutonomousCommand(getTrajectory("from_pickup_to_place")) // return to the next placement location + ), + RobotMap.Component.arm.c_shootCubes(4) // finally, shoot the cube we just picked up and stow + )); + return cmd; + } + public Command simpleAuton() { // this is JUST placing a cube and exiting the zone, no complicated stuff we cant do + var cmd = RobotMap.Component.arm.c_holdRotation(135, + () -> new SequentialCommandGroup( + RobotMap.Component.intake.c_holdVoltage(9).withTimeout(0.5), + // new ParallelCommandGroup( // then, in parallel + // getAutonomousCommand(getTrajectory("go_to_pickup_next")), // go to pickup location + // (new WaitCommand(2.5)).andThen(RobotMap.Component.intake.c_holdVoltage(-8)) // start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) + // ), + // new TriggerCommandFactory( // then, as a separated parallel schedule, + // () -> RobotMap.Component.intake.c_holdVoltage(-1), // hold the game piece in + // () -> getAutonomousCommand(getTrajectory("from_pickup_to_place")) // return to the next placement location + // ), + // RobotMap.Component.arm.c_shootCubes(4), // finally, shoot the cube we just picked up and stow + getAutonomousCommand(getTrajectory("past_ramp")) + + )); + return cmd; + } + + public Command hallwayPracticeAuton() { // shoot cone, grab cube, shoot cube, doesn't balance + // var onArrivalCommand = getAutonomousCommand(getTrajectory("straight_forward")); // go to pickup location + // var onArrivalCommand = new SequentialCommandGroup( + // getAutonomousCommand(getTrajectory("straight_forward")), // go to pickup location + // getAutonomousCommand(getTrajectory("straight_backward"))//, // return to the next placement location + // // getAutonomousCommand(getTrajectory("straight_forward")) + // ); + // SmartDashboard.putString("funnyboi", getTrajectory("straight_backward").getInitialPose().toString()); + var onArrivalCommand = new SequentialCommandGroup( + new ParallelDeadlineGroup( // then, in parallel + (new WaitCommand(1)).andThen(getAutonomousCommand(getTrajectory("straight_forward"))) // go to pickup location + //, new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeFloor(() -> RobotMap.Component.intake.c_holdVoltage(-8).withTimeout(5).andThen(RobotMap.Component.intake.c_holdItem())))// start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) + , new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeFloor(() -> RobotMap.Component.intake.c_holdVoltage(-8.)))// start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) + ), + new ParallelDeadlineGroup( // then, as a separated parallel schedule, + getAutonomousCommand(getTrajectory("straight_backward")) // return to the next placement location + , new TriggerCommandFactory(() -> RobotMap.Component.intake.c_holdItem()) // hold the game piece in + ), + new TriggerCommandFactory(() -> RobotMap.Component.arm.c_shootCubes(5, () -> getAutonomousCommand(getTrajectory("straight_forward")))) // finally, shoot the cube we just picked up and stow + // getAutonomousCommand(getTrajectory("straight_forward")) ); - - return command; - } - - public Command newAuton() { - var command = new SequentialCommandGroup( - //1. Position arm to place gamepiece - // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state - - // implement going over and shooting a cone? - - new ParallelCommandGroup( - //3. Retract arm - // RobotMap.Component.arm.c_posReturnToHomeDown(false), - RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), - new SequentialCommandGroup( - new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), - RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), - RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) - ), - - new SequentialCommandGroup( - new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()+0.5), - getAutonomousCommand(getTrajectory("to_ramp")), - getAutonomousCommand(getTrajectory("angle_ramp_forward")), - new WaitCommand(2), - getAutonomousCommand(getTrajectory("go_over_ramp")), - getAutonomousCommand(getTrajectory("angle_ramp_backward")), - new WaitCommand(2), - getAutonomousCommand(getTrajectory("go_middle_ramp")) - ) - ) - // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) - //6. balance code here - ); - - return command; - - } + + // var cmd = RobotMap.Component.arm.c_shootCubes(4, () -> onArrivalCommand); + var cmd = RobotMap.Component.arm.c_shootCubes(4); + // SmartDashboard.putString("auton reqs", onArrivalCommand.getRequirements().toString()); + + var total_parallel = new ParallelCommandGroup( + new TriggerCommandFactory(() -> cmd), + new TriggerCommandFactory(() -> (new WaitCommand(2.5)).andThen(onArrivalCommand)) + ); + // return cmd; + return total_parallel; + } } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java index e63843f..79d87e9 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java @@ -26,11 +26,23 @@ public void setPower(double power) { leftMotors.setPower(power); rightMotors.setPower(-power); } + public Command c_neutralOutput() { + return Commands.runOnce(() -> { + leftMotors.neutralOutput(); + rightMotors.neutralOutput(); + }, leftMotors, rightMotors); + } public Command c_holdVoltage(double voltage) { return Commands.run(() -> { setVoltage(voltage); }, leftMotors, rightMotors); } + public Command c_startIntake() { + return c_holdVoltage(-8); + } + public Command c_holdItem() { + return c_holdVoltage(-2).withTimeout(0.5).andThen(c_holdVoltage(-1.6)); + } public Command c_holdVoltageDefault() { var cmd = Commands.run(() -> setVoltage(DEFAULT_INTAKE_VOLTS), leftMotors, rightMotors); diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java index 2ad39a4..78e81ea 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java @@ -4,8 +4,10 @@ package org.usfirst.frc4904.robot.subsystems.arm; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; import org.opencv.core.Mat.Tuple2; +import org.usfirst.frc4904.standard.commands.TriggerCommandFactory; import org.usfirst.frc4904.standard.custom.motioncontrollers.ezControl; import org.usfirst.frc4904.standard.custom.motioncontrollers.ezMotion; import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorSubsystem; @@ -16,7 +18,12 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WrapperCommand; +import static org.usfirst.frc4904.robot.Utils.nameCommand; public class ArmExtensionSubsystem extends SubsystemBase { public static final double MAXIMUM_HORIZONTAL_SAFE_EXTENSION_M = Units.inchesToMeters(48); @@ -94,20 +101,32 @@ public Command c_controlVelocity(DoubleSupplier metersPerSecondSupplier) { return cmd; } - public Pair c_holdExtension(double extensionLengthMeters, double maxVelocity, double maxAcceleration) { - ezControl controller = new ezControl(kP, kI, kD, - (double position, double velocity) -> this.feedforward.calculate( - Units.degreesToRadians(angleDealer_DEG.getAsDouble()) - (Math.PI/2), - velocity, - 0 - )); + public Command c_holdExtension(double extensionLengthMeters, double maxVelocity, double maxAcceleration, Supplier onArrivalCommandDealer) { + ezControl controller = new ezControl( + kP, kI, kD, + (double position, double velocity) -> this.feedforward.calculate( + Units.degreesToRadians(angleDealer_DEG.getAsDouble()) - (Math.PI/2), + velocity, + 0 + )) { + @Override + public void updateSetpoint(double setpoint, double setpoint_dt) { + super.updateSetpoint(setpoint * 0.968 - 0.0853, setpoint_dt); + } + }; TrapezoidProfile profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(maxVelocity, maxAcceleration), - new TrapezoidProfile.State(extensionLengthMeters, 0), - new TrapezoidProfile.State(getCurrentExtensionLength(), 0)); - var cmd = new ezMotion(controller, this::getCurrentExtensionLength, motor::setVoltage, (double t) -> new Tuple2(profile.calculate(t).position, profile.calculate(t).velocity), this, motor); + new TrapezoidProfile.State((extensionLengthMeters - 0.0853)/0.968, 0), + new TrapezoidProfile.State((getCurrentExtensionLength() - 0.0853)/0.968, 0)); + var cmd = new ezMotion(controller, this::getCurrentExtensionLength, motor::setVoltage, () -> (double t) -> new Tuple2(profile.calculate(t).position, profile.calculate(t).velocity), this, motor); cmd.setName("arm - c_holdExtension"); - return new Pair(cmd, profile.totalTime()); + return onArrivalCommandDealer == null ? cmd : nameCommand("extension w/ onArrival", new ParallelCommandGroup( + cmd, + new SequentialCommandGroup( + new WaitCommand(profile.totalTime()), + new TriggerCommandFactory("arm extension", onArrivalCommandDealer) + )) + ); } } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java index b60ee7c..dec9ef2 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java @@ -1,8 +1,12 @@ package org.usfirst.frc4904.robot.subsystems.arm; +import java.util.function.DoubleFunction; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; import org.opencv.core.Mat.Tuple2; +import org.usfirst.frc4904.standard.commands.Noop; +import org.usfirst.frc4904.standard.commands.TriggerCommandFactory; import org.usfirst.frc4904.standard.custom.motioncontrollers.ezControl; import org.usfirst.frc4904.standard.custom.motioncontrollers.ezMotion; import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorSubsystem; @@ -13,7 +17,11 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import static org.usfirst.frc4904.robot.Utils.nameCommand; public class ArmPivotSubsystem extends SubsystemBase { public static final double HARD_STOP_ARM_ANGLE = -38; @@ -56,42 +64,34 @@ public class ArmPivotSubsystem extends SubsystemBase { // public static final double kA_extended = .12082; public static final double kG_retracted = 0.32; - public static final double kG_extended = 0.7; + public static final double kG_extended = 0.6; // TODO: tune - public static final double kP = 0;//0.04; //extended: .36915 retracted: .01464 - public static final double kI = 0;//0.01; + public static final double kP = 0.06;//;//0.04; //extended: .36915 retracted: .01464 + public static final double kI = 0.02;//0.01; public static final double kD = 0; public final TalonMotorSubsystem armMotorGroup; public final TelescopingArmPivotFeedForward feedforward; public final DoubleSupplier extensionDealerMeters; - private final EncoderWithSlack slackyEncoder; public ArmPivotSubsystem(TalonMotorSubsystem armMotorGroup, DoubleSupplier extensionDealerMeters) { this.armMotorGroup = armMotorGroup; this.extensionDealerMeters = () -> extensionDealerMeters.getAsDouble(); this.feedforward = new TelescopingArmPivotFeedForward(kG_retracted, kG_extended, kS, kV, kA); - this.slackyEncoder = new EncoderWithSlack( - GEARBOX_SLACK_DEGREES, - armMotorGroup::getSensorPositionRotations, - Units.rotationsToDegrees(1/GEARBOX_RATIO), - true - ); } public double getCurrentAngleDegrees() { // return slackyEncoder.getRealPosition(); - return motorRevsToAngle(armMotorGroup.getSensorPositionRotations()); + return motorRevsToAngle(armMotorGroup.getSensorPositionRotations())*0.985 - 5.18; // * 0.911 - 6.3 } - /** + /**[p] * Expects sensors to be zeroed at forward hard-stop. */ public void initializeEncoderPositions() { armMotorGroup.zeroSensors(angleToMotorRevs(HARD_STOP_ARM_ANGLE)); - slackyEncoder.zeroSlackDirection(true); } public static double motorRevsToAngle(double revs) { @@ -124,26 +124,32 @@ public Command c_controlAngularVelocity(DoubleSupplier degPerSecDealer) { return cmd; } - public Pair c_holdRotation(double degreesFromHorizontal, double maxVelDegPerSec, double maxAccelDegPerSecSquare) { + public Command c_holdRotation(double degreesFromHorizontal, double maxVelDegPerSec, double maxAccelDegPerSecSquare, Supplier onArrivalCommandDealer) { ezControl controller = new ezControl( kP, kI, kD, (position, velocityDegPerSec) -> { double brr = this.feedforward.calculate( extensionDealerMeters.getAsDouble()/ArmExtensionSubsystem.MAX_EXTENSION_M, - Units.degreesToRadians(getCurrentAngleDegrees()), + Units.degreesToRadians((getCurrentAngleDegrees() + 6.3)/0.911), Units.degreesToRadians(velocityDegPerSec), 0 ); SmartDashboard.putNumber("Intended voltage", maxAccelDegPerSecSquare); return brr; } - ); + ){ + @Override + public void updateSetpoint(double setpoint, double setpoint_dt) { + super.updateSetpoint(setpoint * 0.911 - 6.3, setpoint_dt); + } + }; TrapezoidProfile profile = new TrapezoidProfile( new TrapezoidProfile.Constraints(maxVelDegPerSec, maxAccelDegPerSecSquare), - new TrapezoidProfile.State(degreesFromHorizontal, 0), - new TrapezoidProfile.State(getCurrentAngleDegrees(), 0) + new TrapezoidProfile.State((degreesFromHorizontal + 6.3)/0.911, 0), + new TrapezoidProfile.State((getCurrentAngleDegrees() + 6.3)/0.911, 0) ); + var cmd = new ezMotion( controller, () -> this.getCurrentAngleDegrees(), @@ -162,7 +168,12 @@ public Pair c_holdRotation(double degreesFromHorizontal, double cmd.setName("arm - c_holdRotation"); // return new Pair(new ezMotion(controller, () -> this.getCurrentAngleDegrees() * Math.PI / 180, armMotorGroup::setVoltage, // (double t) -> new Tuple2(profile.calculate(t).position, profile.calculate(t).velocity), this), profile.totalTime()); - return new Pair( - cmd, profile.totalTime()); + return onArrivalCommandDealer == null ? cmd : nameCommand("pivot w/ onArrival: " + cmd.getName(), new ParallelCommandGroup( + cmd, + new SequentialCommandGroup( + new WaitCommand(profile.totalTime()), + new TriggerCommandFactory("arm pivot", onArrivalCommandDealer) + )) + ); } } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index ecd9a98..96df8ab 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -1,15 +1,22 @@ package org.usfirst.frc4904.robot.subsystems.arm; import java.util.HashMap; +import java.util.concurrent.Callable; +import java.util.function.Function; +import java.util.function.Supplier; +import org.usfirst.frc4904.robot.Robot; import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.robot.subsystems.Intake; +import org.usfirst.frc4904.standard.commands.Noop; +import org.usfirst.frc4904.standard.commands.TriggerCommandFactory; +import org.usfirst.frc4904.standard.custom.Triple; import org.usfirst.frc4904.robot.humaninterface.drivers.NathanGain; import edu.wpi.first.math.Pair; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; - +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -19,32 +26,47 @@ public class ArmSubsystem extends SubsystemBase { public final ArmPivotSubsystem armPivotSubsystem; public final ArmExtensionSubsystem armExtensionSubsystem; - public static final double MAX_VELOCITY_EXTENSION = 1; - public static final double MAX_ACCEL_EXTENSION = 2; + public static final double MAX_VELOCITY_EXTENSION = 1.5; + public static final double MAX_ACCEL_EXTENSION = 3; - public static final double MAX_VELOCITY_PIVOT = 150; - public static final double MAX_ACCEL_PIVOT = 200; - + public static final double MAX_VELOCITY_PIVOT = 160; + public static final double MAX_ACCEL_PIVOT = 210; + + // SHELF CONES + public static final HashMap> shelfCones = new HashMap<>(); //in degrees, meters + static { // SHELF CONES + // cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); + shelfCones.put(2, new Triple<>(29.5, Units.inchesToMeters(18), 3.)); + shelfCones.put(3, new Triple<>(41., ArmExtensionSubsystem.MAX_EXTENSION_M, 3.)); + shelfCones.put(4, new Triple<>(180.0-41, ArmExtensionSubsystem.MAX_EXTENSION_M, 3.)); + } - public static final HashMap> cones = new HashMap<>(); //in degrees, meters - static { - cones.put(1, new Pair<>(-19.,Units.inchesToMeters(0.))); - cones.put(2, new Pair<>(29.,Units.inchesToMeters(18.))); - cones.put(3, new Pair<>(31.,Units.inchesToMeters(38.))); + // FLOOR CONES + public static final HashMap> floorCones = new HashMap<>(); //in degrees, meters + static { // FLOOR CONES + // cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); + floorCones.put(2, new Triple<>(29., Units.inchesToMeters(14), 3.)); + floorCones.put(3, new Triple<>(29., ArmExtensionSubsystem.MAX_EXTENSION_M-0.02, 3.)); + floorCones.put(4, new Triple<>(180.0-41, ArmExtensionSubsystem.MAX_EXTENSION_M, 3.)); } - public static final HashMap> cubes = new HashMap>(); //in degrees, meters + + public static HashMap> cones = floorCones; + + public static final HashMap> cubes = new HashMap<>(); //in degrees, meters static { - cubes.put(1, new Pair<>(-33.,Units.inchesToMeters(0.))); - cubes.put(2, new Pair<>(14.,Units.inchesToMeters(6.))); - cubes.put(3, new Pair<>(22.,Units.inchesToMeters(28.))); + // cubes.put(1, new Triple<>(-33., Units.inchesToMeters(0), 3.)); + cubes.put(2, new Triple<>(15., Units.inchesToMeters(0), 4.5)); + cubes.put(3, new Triple<>(20., Units.inchesToMeters(0), 4.5)); + cubes.put(4, new Triple<>(180.-35, Units.inchesToMeters(0), 4.5)); + cubes.put(5, new Triple<>(180.-25, Units.inchesToMeters(0), 4.5)); } public static final HashMap> otherPositions = new HashMap<>(); static { // https://docs.google.com/spreadsheets/d/1B7Ie4efOpuZb4UQsk8lHycGvi6BspnF74DUMLmiKGUM/edit#gid=0 in degrees, meters - otherPositions.put("homeUp", new Pair<>(70., Units.inchesToMeters(0.))); // TODO: get number @thomasrimer - otherPositions.put("homeDown", new Pair<>(-37., Units.inchesToMeters(0.))); - otherPositions.put("intakeGround", new Pair<>(-37., Units.inchesToMeters(4.))); + otherPositions.put("homeUp", new Pair<>(65., Units.inchesToMeters(0.))); // TODO: get number @thomasrimer + otherPositions.put("homeDown", new Pair<>(-41., -0.1)); + otherPositions.put("intakeShelf", new Pair<>(25., Units.inchesToMeters(20.))); } @@ -53,120 +75,126 @@ public ArmSubsystem(ArmPivotSubsystem armPivotSubsystem, ArmExtensionSubsystem a this.armExtensionSubsystem = armExtensionSubsystem; } - public Command c_posReturnToHomeUp(boolean flippy) { - var cmd = c_holdArmPoseFlippy(otherPositions.get("homeUp"), NathanGain.isFlippy); + public Command c_posReturnToHomeUp() { return c_posReturnToHomeUp(null); } + public Command c_posReturnToHomeUp(Supplier onArrivalCommandDealer) { + var cmd = new TriggerCommandFactory(() -> c_holdArmPose(otherPositions.get("homeUp"), onArrivalCommandDealer)); cmd.setName("arm position - home (up)"); return cmd; } - public Command c_posReturnToHomeDown(boolean flippy) { - var cmd = c_holdArmPoseFlippy(otherPositions.get("homeDown"), NathanGain.isFlippy); + + public Command c_posReturnToHomeDown() { return c_posReturnToHomeDown(null); } + public Command c_posReturnToHomeDown(Supplier onArrivalCommandDealer) { + var cmd = new TriggerCommandFactory(() -> c_holdArmPose(otherPositions.get("homeDown"), onArrivalCommandDealer)); cmd.setName("arm position - home (down)"); return cmd; } - public Command c_posIntakeGround() { - var cmd = c_holdArmPoseFlippy(otherPositions.get("intakeGround"), NathanGain.isFlippy); - cmd.setName("arm position - ground intake"); - return cmd; - } - public Command c_posIntakeShelf() { + public Command c_posIntakeShelf(Supplier onArrivalCommandDealer) { // TODO: back up 14 inches -- remember to always use meters - var cmd = c_holdArmPoseFlippy(otherPositions.get("intakeShelf"), NathanGain.isFlippy); + cones = shelfCones; + var cmd = c_holdArmPose(otherPositions.get("intakeShelf"), onArrivalCommandDealer); cmd.setName("arm position - pre shelf intake"); return cmd; } - - public Command c_angleCubes(int shelf) { - var degreesFromHorizontal = cubes.get(shelf).getFirst(); - var extensionLengthMeters = cubes.get(shelf).getSecond(); - - if (NathanGain.isFlippy) { - degreesFromHorizontal = (degreesFromHorizontal * -1) + 180; - } - - var cmd = c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); - cmd.setName("arm - c_angleCubes - " + shelf); + public Command c_posIntakeFloor(Supplier onArrivalCommandDealer) { + cones = floorCones; + var cmd = c_holdArmPose(otherPositions.get("homeDown"), onArrivalCommandDealer); + cmd.setName("arm position - pre floor intake"); return cmd; } - //for auton - public Command placeCube(int shelf, boolean flippy) { - double degreesFromHorizontal = cubes.get(shelf).getFirst(); - if (flippy) { - degreesFromHorizontal = (degreesFromHorizontal * -1) + 180; - } - double extensionLengthMeters= cubes.get(shelf).getSecond(); - - return ( - c_holdArmPose(degreesFromHorizontal, extensionLengthMeters) - .alongWith(new WaitCommand(1).andThen(RobotMap.Component.intake.c_holdVoltage(-Intake.DEFAULT_INTAKE_VOLTS)) - )).withTimeout(5); //TODO: change timeout - } - //for buttons - public Command placeCube(int shelf) { - double degreesFromHorizontal = cubes.get(shelf).getFirst(); - if (NathanGain.isFlippy) { - degreesFromHorizontal = (degreesFromHorizontal * -1) + 180; - } - double extensionLengthMeters= cubes.get(shelf).getSecond(); - return ( - c_holdArmPose(degreesFromHorizontal, extensionLengthMeters) - .alongWith(new WaitCommand(1).andThen(RobotMap.Component.intake.c_holdVoltage(Intake.DEFAULT_INTAKE_VOLTS)) - )).withTimeout(5); //TODO: change timeout - } public Command c_angleCones(int shelf) { var degreesFromHorizontal = cones.get(shelf).getFirst(); var extensionLengthMeters = cones.get(shelf).getSecond(); - - if (NathanGain.isFlippy) { - degreesFromHorizontal = (degreesFromHorizontal * -1) + 180; + + return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); + } + + public Command c_shootCones(int shelf) { return c_shootCones(shelf, null); } + public Command c_shootCones(int shelf, Supplier onArrivalCommandDealer) { + var degreesFromHorizontal = cones.get(shelf).getFirst(); + var extensionLengthMeters = cones.get(shelf).getSecond(); + var voltage = cones.get(shelf).getThird(); + + + if (cones == shelfCones) { + return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); } - var cmd = c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); - cmd.setName("arm - c_angleCones - " + shelf); - return cmd; + return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, + () -> new SequentialCommandGroup( + RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5), + RobotMap.Component.intake.c_neutralOutput(), c_posReturnToHomeUp(onArrivalCommandDealer) + ) + ); } + + public Command c_angleCubes(int shelf) { + var degreesFromHorizontal = cubes.get(shelf).getFirst(); + var extensionLengthMeters = cubes.get(shelf).getSecond(); - public Command c_holdArmPoseFlippy(Pair angleAndExtensionMeters, boolean flippy) { - var degreesFromHorizontal = angleAndExtensionMeters.getFirst(); - var extensionLengthMeters = angleAndExtensionMeters.getSecond(); + return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, null); + } - if (flippy) degreesFromHorizontal = 180 - degreesFromHorizontal; + public Command c_shootCubes(int shelf) { return c_shootCubes(shelf, null); } + public Command + c_shootCubes(int shelf, Supplier onArrivalCommandDealer) { + var degreesFromHorizontal = cubes.get(shelf).getFirst(); + var extensionLengthMeters = cubes.get(shelf).getSecond(); + var voltage = cubes.get(shelf).getThird(); + + return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, + () -> new SequentialCommandGroup( + RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5), + RobotMap.Component.intake.c_neutralOutput(), c_posReturnToHomeUp(onArrivalCommandDealer) + ) + ); + } - return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); + public Command c_holdRotation(double degreesFromHorizontal, Supplier onArrivalCommandDealer) { + return armPivotSubsystem.c_holdRotation(degreesFromHorizontal, MAX_VELOCITY_PIVOT, MAX_ACCEL_PIVOT, onArrivalCommandDealer); } + + public Command c_holdArmPose(Pair emacs) { + return c_holdArmPose(emacs.getFirst(), emacs.getSecond()); + } + public Command c_holdArmPose(Pair emacs, Supplier onArrivalCommandDealer) { + return c_holdArmPose(emacs.getFirst(), emacs.getSecond(), onArrivalCommandDealer); + } public Command c_holdArmPose(double degreesFromHorizontal, double extensionLengthMeters) { - // TODO: crashes - // return this.runOnce(() -> System.out.println("TODO: hold arm pose crashes the code!")); - Command firstCommand; - Command secondCommand; - double wait; - - Pair pivotMovement = armPivotSubsystem.c_holdRotation(degreesFromHorizontal, MAX_VELOCITY_PIVOT, MAX_ACCEL_PIVOT); - Pair extensionMovement = armExtensionSubsystem.c_holdExtension(extensionLengthMeters, MAX_VELOCITY_EXTENSION, MAX_ACCEL_EXTENSION); - - if ((extensionLengthMeters - armExtensionSubsystem.getCurrentExtensionLength()) > 0) { - firstCommand = pivotMovement.getFirst(); - wait = pivotMovement.getSecond(); - secondCommand = extensionMovement.getFirst(); + return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, null); + } + + public Command c_holdArmPose(double degreesFromHorizontal, double extensionLengthMeters, Supplier onArrivalCommandDealer) { + final Supplier sanitizedArrivalCommandDealer = onArrivalCommandDealer != null ? onArrivalCommandDealer : () -> new Noop(); + + Function, Command> pivotAndThen = (then) -> armPivotSubsystem.c_holdRotation(degreesFromHorizontal, MAX_VELOCITY_PIVOT, MAX_ACCEL_PIVOT, then); + Function, Command> extendAndThen = (then) -> armExtensionSubsystem.c_holdExtension(extensionLengthMeters, MAX_VELOCITY_EXTENSION, MAX_ACCEL_EXTENSION, then); + + if (extensionLengthMeters > armExtensionSubsystem.getCurrentExtensionLength()) { + return pivotAndThen.apply(() -> extendAndThen.apply(sanitizedArrivalCommandDealer)); } else { - firstCommand = extensionMovement.getFirst(); - wait = extensionMovement.getSecond(); - secondCommand = pivotMovement.getFirst(); + return extendAndThen.apply(() -> pivotAndThen.apply(sanitizedArrivalCommandDealer)); } - var cmd = this.runOnce(() -> { - firstCommand.schedule(); - // (Commands.run(() -> System.out.println( - // "1: " + String.valueOf(firstCommand.isScheduled()) - // + " ... 2: " + String.valueOf(secondCommand.isScheduled()) - // + " ... cur : " + armExtensionSubsystem.getCurrentCommand().getName() - // + " ... joystick: " + String.valueOf(RobotMap.HumanInput.Operator.joystick.getAxis(3)) - // ))).schedule(); - (new SequentialCommandGroup(new WaitCommand(wait), secondCommand)).schedule(); - // ((new WaitCommand(1)).andThen(secondCommand)).schedule(); - // secondCommand.schedule(); - }); // long story. basically, parallel command group requires it's subcommands' requirements. however, we want one subcommand to be able to die wihle the other one lives, so we just do this instead and leak commands. it's fine because they'll get cleaned up when their atomic base subsystems gets taken over by new commands - cmd.setName("arm - c_holdArmPose"); - return cmd; + + // if ((extensionLengthMeters - armExtensionSubsystem.getCurrentExtensionLength()) > 0) { + // firstCommand = pivotMovement.getFirst(); + // wait = pivotMovement.getSecond(); + // secondCommand = extensionMovement.getFirst(); + // secondWait = wait + extensionMovement.getSecond(); + // } else { + // firstCommand = extensionMovement.getFirst(); + // wait = extensionMovement.getSecond(); + // secondCommand = pivotMovement.getFirst(); + // secondWait = wait + pivotMovement.getSecond(); + // } + + // firstCommand.schedule(); + // (new SequentialCommandGroup(new WaitCommand(wait), secondCommand)).schedule(); + // if (onArrivalCommandDealer != null) (new SequentialCommandGroup(new WaitCommand(secondWait), onArrivalCommandDealer.get())).schedule(); + + // }); // long story. basically, parallel command group requires its subcommands' requirements. however, we want one subcommand to be able to die wihle the other one lives, so we just do this instead and leak commands. it's fine because they'll get cleaned up when their atomic base subsystems gets taken over by new commands + // cmd.setName("arm - c_holdArmPose"); + // return cmd; } } diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index 5173578..5492244 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit 51735784ae2cc3ee6f420282d516eea222492983 +Subproject commit 54922444352baeef934533ff5f45bc07bed78500