diff --git a/FRC_TBD_5348e8acf25c64ea.wpilog b/FRC_TBD_5348e8acf25c64ea.wpilog new file mode 100644 index 0000000..256a1ac Binary files /dev/null and b/FRC_TBD_5348e8acf25c64ea.wpilog differ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4fd1b46..6511734 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -45,9 +46,7 @@ public class RobotContainer { private final Tank m_Tank = new Tank(); private final PID m_PID = new PID(); private final PDP m_PDP = new PDP(); - private final Vision m_Vision = new Vision(m_Tank); - - + private final Vision m_Vision = new Vision(m_Tank, m_Claw); // INSTANTIATES ALL COMMANDS private final MoveClawCommand m_MoveClawCommand = new MoveClawCommand(m_Claw); @@ -63,8 +62,8 @@ public class RobotContainer { private final FineDriveCommand m_FineDriveCommand = new FineDriveCommand(m_Tank); private final PlaceCubeSecondLevelCommand m_PlaceCubeSecondLevelCommand = new PlaceCubeSecondLevelCommand(m_Tank, m_Arm, m_Claw); - private final TrackPiece m_TrackPiece = new TrackPiece(m_Vision, m_Tank); - + private final TrackPiece m_TrackPiece = new TrackPiece(m_Vision, m_Tank, m_Claw); + // private final XTracking m_XTracking = new XTracking(m_Tank, null, m_Target); // private final YTracking m_YTracking = new YTr @@ -100,9 +99,6 @@ public RobotContainer() { Logger.configureLoggingAndConfig(this, false); - - - } /** @@ -128,11 +124,8 @@ private void configureButtonBindings() { .whileTrue(m_FineDriveCommand); new JoystickButton(m_armController2, XboxController.Button.kY.value).whileTrue(m_PlaceConeSecondLevelCommand); - new JoystickButton(m_armController2, XboxController.Button.kX.value).whileTrue(m_TrackPiece); - - // new JoystickButton(m_armController2, XboxController.Button.kB.value) // .whileTrue(m_PlaceConeSecondLevelCommand); } @@ -250,6 +243,19 @@ public static double getControllerRightStickX() { } return axis; } + + public static double getControllerRightStickY() { + double axis = m_armController2.getRawAxis(5); + if (Math.abs(axis) < 0.4) { + axis = 0; + } + return -axis; + } + + public static void rumbleGabeController(double rumble) { + XboxController controller = m_driverController; + controller.setRumble(RumbleType.kBothRumble, rumble); + } /** * gets value from the right trigger axis (yes, it's an axis) for grabbing up @@ -301,6 +307,4 @@ public void robotPeriodic() { timeWidget.setDouble(DriverStation.getMatchTime()); } - - } diff --git a/src/main/java/frc/robot/commands/FineDriveCommand.java b/src/main/java/frc/robot/commands/FineDriveCommand.java index b59800d..f7f2e1b 100644 --- a/src/main/java/frc/robot/commands/FineDriveCommand.java +++ b/src/main/java/frc/robot/commands/FineDriveCommand.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.RebindHat; +import frc.robot.Robot; +import frc.robot.RobotContainer; import frc.robot.subsystems.Tank; public class FineDriveCommand extends CommandBase { @@ -30,6 +32,7 @@ public void initialize() { @Override public void execute() { // double check getMaxSpeed(), might be wrong + RobotContainer.rumbleGabeController(1); m_drivebase.arcadeDrive( RebindHat.ControllerToYAxis() * multWidget.getDouble(0), RebindHat.ControllerToXAxis() * multWidget.getDouble(0)); } @@ -37,6 +40,7 @@ public void execute() { @Override public void end(boolean interrupted) { m_drivebase.arcadeDrive(0, 0); + RobotContainer.rumbleGabeController(0); } @Override diff --git a/src/main/java/frc/robot/commands/TrackPiece.java b/src/main/java/frc/robot/commands/TrackPiece.java index 4c90e0a..915d0a1 100644 --- a/src/main/java/frc/robot/commands/TrackPiece.java +++ b/src/main/java/frc/robot/commands/TrackPiece.java @@ -6,6 +6,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Claw; import frc.robot.subsystems.Tank; import frc.robot.subsystems.Vision; @@ -13,11 +15,13 @@ public class TrackPiece extends CommandBase { /** Creates a new TrackPiece. */ private Vision m_Vision; private Tank m_Tank; - public TrackPiece(Vision vision, Tank tank) { + private Claw m_Claw; + public TrackPiece(Vision vision, Tank tank, Claw claw) { // Use addRequirements() here to declare subsystem dependencies. m_Vision = vision; m_Tank = tank; - addRequirements(m_Vision, m_Tank); + m_Claw = claw; + addRequirements(m_Vision, m_Tank, m_Claw); } // Called when the command is initially scheduled. @@ -26,25 +30,30 @@ public void initialize() { //m_Tank.arcadeDrive(0.05, m_Vision.getController().calculate(m_Vision.getMeasurement(), m_Vision.getSetpoint())); m_Vision.enable(); SmartDashboard.putString("a", "pid on"); + m_Claw.closeClaw(); } @Override public void execute() { + //m_Tank.setAllMotors(-0.2); + RobotContainer.rumbleGabeController(1); SmartDashboard.putNumber(getName(), m_Vision.getController().calculate(m_Vision.getMeasurement(), m_Vision.getSetpoint())); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + RobotContainer.rumbleGabeController(0); m_Vision.disable(); m_Tank.arcadeDrive(0,0); SmartDashboard.putString("a", "pid off"); + m_Claw.stopClaw(); } // Returns true when the command should end. @Override public boolean isFinished() { - return m_Vision.atSetpoint(); + return false; } } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 9782b51..cd21f4d 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -42,6 +42,9 @@ public class Arm extends SubsystemBase { public AbsoluteEncoder encoder; + double lastEncoderValue; + int rotations; + public Arm() { kP = 0.0001; @@ -71,15 +74,17 @@ public Arm() { armPID.setOutputRange(kMinOutput, kMaxOutput); // set the spark max to alternate encoder mode - - // configure the data port on top to be used with the REV Through Bore Encoder using the absolute encoder adapter + // configure the data port on top to be used with the REV Through Bore Encoder + // using the absolute encoder adapter encoder = armMotor.getAbsoluteEncoder(Type.kDutyCycle); encoderWidget = Shuffleboard.getTab("Motors").add("Arm Encoder", armMotor.getEncoder().getPosition()).getEntry(); Shuffleboard.getTab("Motors").add("Arm", armMotor); + lastEncoderValue = armMotor.getEncoder().getPosition(); + // configure the PID loop to use the alternate encoder armPID.setFeedbackDevice(encoder); @@ -94,8 +99,18 @@ public Arm() { @Override public void periodic() { // SmartDashboard.putBoolean("zeroDeg", allTheWayDownRear.get()); + double encoderValue = armMotor.getEncoder().getPosition(); + double change = encoderValue - lastEncoderValue; + if (Math.abs(change) > 0.5) { + if (change < 0) { + rotations++; + } else { + rotations--; + } + } + lastEncoderValue = encoderValue; - encoderWidget.setDouble(armMotor.getEncoder().getPosition()); + encoderWidget.setDouble(encoderValue); } /** diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index d8e2e9b..23604e6 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -25,7 +25,7 @@ public class Claw extends SubsystemBase { public Claw() { clawMotor = new CANSparkMax(8, MotorType.kBrushless); clawMotor.setIdleMode(IdleMode.kBrake); - clawMotor.setSmartCurrentLimit(17); + clawMotor.setSmartCurrentLimit(20); clawMotor.burnFlash(); addChild("Claw Motor", clawMotor); @@ -39,11 +39,11 @@ public void periodic() { } public void closeClaw() { - clawMotor.set(1); + clawMotor.set(0.75); } public void openClaw() { - clawMotor.set(-1); + clawMotor.set(-0.75); } public void stopClaw() { diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index a876883..b47fb37 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -9,25 +9,28 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.PIDSubsystem; +import frc.robot.RobotContainer; public class Vision extends PIDSubsystem { /** Creates a new Vision. */ - private static double kP = 0.02; - private static double kI = 0.05; - private static double kD = 0.0007; + private static double kP = 0.1; + private static double kI = 0; + private static double kD = 0; public PIDController pidController; public Tank m_tank; + public Claw m_claw; - public Vision(Tank tank) { + public Vision(Tank tank, Claw claw) { super( // The PIDController used by the subsystem new PIDController(kP, kI, kD)); addChild(getName(), m_controller); m_tank = tank; + m_claw = claw; m_controller.setSetpoint(0); - m_controller.setTolerance(2); + m_controller.setTolerance(1); m_controller.setIntegratorRange(-0.43, 0.43); Shuffleboard.getTab(getName()).add(m_controller); @@ -36,20 +39,34 @@ public Vision(Tank tank) { @Override public void useOutput(double output, double setpoint) { + + double ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0.0); + // Use the output here + double forward = 1 - Math.abs(output) / 30; + if (forward < .1) { + forward = .1; + } + forward *= 0.6; + if (ty < 0) { + forward = 0.5; + } + double stick = RobotContainer.getControllerRightStickY(); + if (stick > .5) { + forward += (stick / 2) - .25; + } - if (0.40 < output) { - output = 0.40; - } else if (-0.40 > output) { - output = -0.40; - } else if (0 < output && output < 0.40) { + if (0.50 < output) { + output = 0.50; + } else if (-0.50 > output) { + output = -0.50; + } else if (0.1 < output && output < 0.40) { output = 0.40; - } else if (-0.40 < output && output < 0) { + } else if (-0.40 < output && output < -0.1) { output = -0.40; } - - m_tank.arcadeDrive(0, -output * 1); + m_tank.arcadeDrive(-forward, -output * 1); SmartDashboard.putNumber("will this work?", output); SmartDashboard.putNumber("setpoint", setpoint);