diff --git a/RobotCode2021-Imported/build.gradle b/RobotCode2021-Imported/build.gradle index 8af7951..3efa0da 100644 --- a/RobotCode2021-Imported/build.gradle +++ b/RobotCode2021-Imported/build.gradle @@ -59,6 +59,10 @@ dependencies { simulation wpi.deps.sim.gui(wpi.platforms.desktop, false) simulation wpi.deps.sim.driverstation(wpi.platforms.desktop, false) + compile 'com.googlecode.json-simple:json-simple:1.1.1' + + + // Websocket extensions require additional configuration. // simulation wpi.deps.sim.ws_server(wpi.platforms.desktop, false) // simulation wpi.deps.sim.ws_client(wpi.platforms.desktop, false) diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/Constants.java b/RobotCode2021-Imported/src/main/java/frc/robot/Constants.java index c1aada0..37b1b9e 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/Constants.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/Constants.java @@ -196,9 +196,9 @@ public static final class Yellow { } public static final class Vision { - public static final double kCameraHeight = 0.23; // meter -- REMEASURE - public static final double kCameraPitch = 0.436; // radians - public static final double kTargetHeight = 2.06; // meters + public static final double kCameraHeight = 0.5461; // meter -- REMEASURE + public static final double kCameraPitch = 1.249; // radians + public static final double kTargetHeight = 2.49555 / 2; // meters public static final double turretKP = 0; public static final double turretKI = 0; diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java b/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java index 5076759..04c34d6 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java @@ -55,6 +55,7 @@ public void robotInit() { @Override public void robotPeriodic() { Scheduler.getInstance().run(); + SmartDashboard.putString("SCHEDULER STATUS", Scheduler.getInstance().getClass().toString()); // SmartDashboard.putNumber("hoodEncoder", hoodEncoder.getPosition()); // SmartDashboard.putNumber("hoodVoltage", hoodEncoder.getBusVoltage()); } @@ -69,6 +70,7 @@ public void disabledPeriodic() { @Override public void autonomousInit() { +<<<<<<< Updated upstream RobotContainer.swerveDrive.resetOdometry(new Pose2d(0, 0, new Rotation2d(-Math.PI / 2.))); RobotContainer.theta.reset(-Math.PI / 2.); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); @@ -77,6 +79,10 @@ public void autonomousInit() { if (m_autonomousCommand != null) { Scheduler.getInstance().add(m_autonomousCommand); } +======= + RobotContainer.swerveDrive.resetOdometry(new Pose2d(0, 0, new Rotation2d(Math.PI / 2))); //0.053, .8539 + RobotContainer.theta.reset(Math.PI / 2); +>>>>>>> Stashed changes try { f = new File("/home/lvuser/Output.txt"); diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java index 39023b2..72cb17c 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,8 @@ import java.util.List; +import org.photonvision.PhotonCamera; + import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; @@ -49,14 +51,13 @@ public class RobotContainer { private final Conveyor conveyor = new Conveyor(); public final static Shooter shooter = new Shooter(); private final Climb climb = new Climb(); - public final Vision vision = new Vision(); - private final Turret turret = new Turret(vision, swerveDrive); - + private final PhotonCamera camera = new PhotonCamera("TurretCamera"); + private final Vision vision = new Vision(camera); + private final Turret turret = new Turret(); // The driver's controller public static Joystick m_driverController = new Joystick(OIConstants.kDriverControllerPort); public static Joystick m_operatorController = new Joystick(1); -// public static final GenericHID.RumbleType kLeftRumble = 1; public static ProfiledPIDController theta = new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0, @@ -70,38 +71,17 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - // Gets the default instance of NetworkTables - NetworkTableInstance table = NetworkTableInstance.getDefault(); - - // Gets the MyCamName table under the chamelon-vision table - // MyCamName will vary depending on the name of your camera - NetworkTable cameraTable = table.getTable("chameleon-vision").getSubTable("Shooter"); - - // Gets the yaw to the target from the cameraTable - yaw = cameraTable.getEntry("yaw"); - - // Gets the driveMode boolean from the cameraTable - isDriverMode = cameraTable.getEntry("driver_mode"); - + // Configure the button bindings configureButtonBindings(); // Configure default commands // Set the default drive command to split-stick arcade drive - swerveDrive.setDefaultCommand(new DefaultDrive(swerveDrive, m_driverController, false)); + swerveDrive.setDefaultCommand(new DefaultDrive(swerveDrive, vision, m_driverController, 1)); conveyor.setDefaultCommand(new AutoIndexConveyor(conveyor)); intake.setDefaultCommand(new RunIntake(intake, m_operatorController)); - turret.setDefaultCommand(new SpinTurret(turret, false, 0)); - - // vision.setDefaultCommand(new RunCommand(vision::runVision, vision)); - - // swerveDrive.setDefaultCommand( + turret.setDefaultCommand(new SpinTurret(turret, swerveDrive, vision, 2, 0)); - // new InstantCommand(() -> swerveDrive.drive(-m_driverController.getRawAxis(1) - // * Constants.SwerveDriveConstants.kMaxSpeedMetersPerSecond, - // -m_driverController.getRawAxis(0) - // * Constants.SwerveDriveConstants.kMaxSpeedMetersPerSecond, - // -m_driverController.getRawAxis(4) * (2 * Math.PI), true), swerveDrive)); } /** @@ -114,15 +94,48 @@ public RobotContainer() { private void configureButtonBindings() { JoystickButton butA = new JoystickButton(m_operatorController, 1); JoystickButton butB = new JoystickButton(m_operatorController, 2); - JoystickButton butY = new JoystickButton(m_operatorController, 3); - JoystickButton butpress = new JoystickButton(m_driverController, 8); - JoystickButton butXd = new JoystickButton(m_driverController, 8); + JoystickButton butX = new JoystickButton(m_operatorController, 3); + + JoystickButton butXd = new JoystickButton(m_driverController, 3); + JoystickButton butAd = new JoystickButton(m_driverController, 1); + JoystickButton butBd = new JoystickButton(m_driverController, 2); + + JoystickButton gyro = new JoystickButton(m_driverController, 8); + JoystickButton rBump = new JoystickButton(m_operatorController, 6); JoystickButton lBump = new JoystickButton(m_operatorController, 5); JoystickButton lAnal = new JoystickButton(m_operatorController, 9); JoystickButton rAnal = new JoystickButton(m_operatorController, 10); - JoystickButton gyro = new JoystickButton(m_driverController, 7); + + // B + JoystickButton turbo = new JoystickButton(m_driverController, 2); + // JoystickButton ok = new JoystickButton(m_driverController, 7); + + BarrelPath Barrel = new BarrelPath(swerveDrive, theta); + + SmartDashboard.putData(Scheduler.getInstance()); + SmartDashboard.putData("Barrel Path", Barrel); + + // GalacticA galA = new GalacticA(swerveDrive, theta); + + SmartDashboard.putData(Scheduler.getInstance()); + // SmartDashboard.putData("GalacticA", galA); + + + BouncePath Bounce = new BouncePath(swerveDrive, theta); + + SmartDashboard.putData(Scheduler.getInstance()); + SmartDashboard.putData("Bounce Path", Bounce); + + SlalomPath1 Slalom = new SlalomPath1(swerveDrive, theta); + + SmartDashboard.putData(Scheduler.getInstance()); + SmartDashboard.putData("Slalom Path", Slalom); + + SideStep SideStep = new SideStep(swerveDrive, theta); + butAd.whenPressed(SideStep); + // A button butA.whileHeld(new ExtendIntake(intake, m_operatorController)); @@ -133,40 +146,39 @@ private void configureButtonBindings() { // right bumper rBump.whileHeld(new RunShooter(shooter)); rBump.whenReleased(new StopShooter(shooter)); - + rBump.whileHeld(new FeedShooter(conveyor, shooter)); + rBump.whenReleased(new AutoIndexConveyor(conveyor)); + + // left bumper + lBump.whileHeld(new ControlConveyor(conveyor)); + lBump.whenReleased(new AutoIndexConveyor(conveyor)); + // left analog center - lAnal.whileHeld(new MoveHood(shooter, -1)); - + lAnal.whileHeld(new MoveHood(shooter, -1)); // right analog center rAnal.whileHeld(new MoveHood(shooter, 1)); - // right bumper - rBump.whileHeld(new FeedShooter(conveyor, shooter)); - rBump.whenReleased(new AutoIndexConveyor(conveyor)); - // left bumper - lBump.whileHeld(new ControlConveyor(conveyor)); - lBump.whenReleased(new AutoIndexConveyor(conveyor)); + // B button - butB.whileHeld(new SpinTurret(turret, true, 0.25)); - butB.whenReleased(new SpinTurret(turret, true, 0)); + butB.whileHeld(new SpinTurret(turret, swerveDrive, vision, 1, 0.25)); // Y button - butY.whileHeld(new SpinTurret(turret, true, -0.25)); - butY.whenReleased(new SpinTurret(turret, true, 0)); + butX.whileHeld(new SpinTurret(turret, swerveDrive, vision, 1, -0.25)); - // driver X button - butXd.whileHeld(new DefaultDrive(swerveDrive, m_driverController, true)); + // driver X button - slow + butXd.whileHeld(new DefaultDrive(swerveDrive, vision, m_driverController, 0.35)); + butAd.whenPressed(new InstantCommand(swerveDrive::zeroWheels)); - gyro.whenPressed(new InstantCommand(swerveDrive::zeroHeading)); + butBd.whenPressed(new SideStep(swerveDrive, theta)); - //new JoystickButton(m_operatorController, 4).whenPressed(new RunCommand(() -> conveyor.manualControl(-), conveyor)) - // .whenReleased(new RunCommand(conveyor::autoIndex, conveyor)); - // should be start button for camera to find target idk what number is so fix it - // new JoystickButton(m_operatorController, 7).whenHeld(new InstantCommand(turret::visionTurret, turret)); + turbo.whileHeld(new DefaultDrive(swerveDrive, vision, m_driverController, 2)); + + gyro.whenPressed(new InstantCommand(swerveDrive::zeroHeading)); + } public static String getCoords() { @@ -178,74 +190,12 @@ public static String getCoords() { * * @return the command to run in autonomous */ - public Command getAutonomousCommand() { - // Create config for trajectory - TrajectoryConfig config = new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond, - AutoConstants.kMaxAccelerationMetersPerSecondSquared) - // Add kinematics to ensure max speed is actually obeyed - .setKinematics(SwerveDriveConstants.kDriveKinematics); - - // An example trajectory to follow. All units in meters. - // Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( - // Start at the origin facing the +X direction - // new Pose2d(0, 0, new Rotation2d(0)), - // List.of(), - // End 3 meters straight ahead of where we started, facing forward - // new Pose2d(0, 3, new Rotation2d(0)), config); - - Trajectory newTrajectory = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(-Math.PI / 2)), List.of( - // keep in - new Translation2d(0, -1.2), // take out - new Translation2d(1.1, -1.9), - new Translation2d(1.88, -1.9), - - - //new Translation2d(1.88, -3.5), - //new Translation2d(1.88, -4), - // keep in - new Translation2d(1.88, -5.5), - new Translation2d(1.88, -6.25), - new Translation2d(0.3, -6.31), - new Translation2d(0.3, -7.9) - - - // previously commented out - //new Translation2d(1.324, -7.9), - //new Translation2d(1.724, -7.9), - // new Translation2d(1.6, -7.75), - // new Translation2d(1.6, -6.55), - // new Translation2d(0, -6.3) - //new Translation2d(1.78, -7.75) - ), - - new Pose2d(1.88, -7.75, new Rotation2d(-Math.PI / 2)), config); - // new Pose2d(0, -7, new Rotation2d(-Math.PI / 2)), config); - - - - SwerveControllerCommand swerveControllerCommand1 = new SwerveControllerCommand(Slalom.getTrajectory(), - (-Math.PI / 2), swerveDrive::getPose, // Functional interface to feed supplier - SwerveDriveConstants.kDriveKinematics, - - // Position controllers - new PIDController(AutoConstants.kPXController, 0, 0), - new PIDController(AutoConstants.kPYController, 0, 0), theta, - - swerveDrive::setModuleStates, - - swerveDrive - - ); - - // Command shootCommand = new Command(() -> shooter.runHood(.5), shooter) - // .andThen(shooter::runShooter, shooter) - // .andThen(new RunCommand(() -> conveyor.feedShooter(0.75, shooter.atSpeed()), conveyor)) - // .withTimeout(15).andThen(new InstantCommand(shooter::stopShooter, shooter)); - - + + public Command getAutonomousCommand(Trajectory trajectory) { + // GalacticA galA = new GalacticA(swerveDrive, theta); + BarrelPath bar = new BarrelPath(swerveDrive, theta); - // return swerveControllerCommand1; + return bar; return swerveControllerCommand1; } diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/GalacticA.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/GalacticA.java new file mode 100644 index 0000000..9274c16 --- /dev/null +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/GalacticA.java @@ -0,0 +1,68 @@ +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. + +// package frc.robot.commands.Auto; + +// import edu.wpi.first.wpilibj.command.Command; +// import edu.wpi.first.wpilibj.command.CommandGroup; +// import edu.wpi.first.wpilibj.command.InstantCommand; + +// import java.util.List; + +// import edu.wpi.first.wpilibj.geometry.Pose2d; +// import edu.wpi.first.wpilibj.geometry.Rotation2d; +// import edu.wpi.first.wpilibj.geometry.Translation2d; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj.trajectory.Trajectory; +// import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +// import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +// import frc.robot.commands.*; +// import edu.wpi.first.wpilibj.controller.PIDController; +// import edu.wpi.first.wpilibj.controller.ProfiledPIDController; +// import frc.robot.RobotContainer; +// import frc.robot.Constants.AutoConstants; +// import frc.robot.Constants.SwerveDriveConstants; +// import frc.robot.subsystems.*; + +// public class GalacticA extends CommandGroup { +// /** Add your docs here. */ +// public GalacticA(SwerveDrive swerveDrive, ProfiledPIDController theta) { + +// final RobotContainer m_robotContainer; +// final Intake intake; + +// // Create config for trajectory +// TrajectoryConfig config = new TrajectoryConfig(2.1, +// AutoConstants.kMaxAccelerationMetersPerSecondSquared) +// // Add kinematics to ensure max speed is actually obeyed +// // .setKinematics(SwerveDriveConstants.kDriveKinematics) +// .setEndVelocity(1.5); + +// Trajectory trajectory = TrajectoryGenerator +// .generateTrajectory(new Pose2d(0, 0, new Rotation2d(-Math.PI / 2)), List.of( + +// ), +// // direction robot moves +// new Pose2d(0, -3, new Rotation2d(Math.PI / 2)), config); + +// SwerveControllerCommand swerveControllerCommand1 = new SwerveControllerCommand(trajectory, (0), +// swerveDrive::getPose, // Functional interface to feed supplier +// SwerveDriveConstants.kDriveKinematics, + +// // Position controllers +// new PIDController(AutoConstants.kPXController, 1, AutoConstants.kDXController), +// new PIDController(AutoConstants.kPYController, 1, AutoConstants.kDYController), theta, + +// swerveDrive::setModuleStates, + +// swerveDrive + +// ); + +// addSequential(swerveControllerCommand1); + + +// } + +// } diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/SideStep.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/SideStep.java new file mode 100644 index 0000000..2d7bb25 --- /dev/null +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/SideStep.java @@ -0,0 +1,58 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.Auto; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.Scheduler; + +import java.util.List; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import frc.robot.commands.*; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.ProfiledPIDController; + +import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.SwerveDriveConstants; +import frc.robot.subsystems.*; + +public class SideStep extends CommandGroup { + /** Add your docs here. */ + public SideStep(SwerveDrive swerveDrive, ProfiledPIDController theta) { + + // Create config for trajectory + TrajectoryConfig config = new TrajectoryConfig(2.2, + AutoConstants.kMaxAccelerationMetersPerSecondSquared) + // Add kinematics to ensure max speed is actually obeyed + // .setKinematics(SwerveDriveConstants.kDriveKinematics) + .setEndVelocity(1.5); + + Trajectory traject = TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d()), List.of(), + //direction robot moves + new Pose2d(1, 0, new Rotation2d(Math.PI / 2)), config); + + + + SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(traject, + (0), swerveDrive::getPose, // Functional interface to feed supplier + SwerveDriveConstants.kDriveKinematics, + + // Position controllers + new PIDController(AutoConstants.kPXController, 1, AutoConstants.kDXController), + new PIDController(AutoConstants.kPYController, 1, AutoConstants.kDYController), theta, + + swerveDrive::setModuleStates, + + swerveDrive + ); + addSequential(swerveControllerCommand); + } +} diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/DefaultDrive.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/DefaultDrive.java index 93bb979..ece2a81 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/DefaultDrive.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/DefaultDrive.java @@ -23,8 +23,12 @@ public class DefaultDrive extends Command { private final boolean m_fieldRelative; private double heading; private PIDController pid = new PIDController(0.05, 0, 0.01); + private PIDController visionController = new PIDController(.1, 0, 0); private JoystickButton butX; private boolean slow; + private int mode; + private double multiplier; + private Vision m_vision; /** * Creates a new DefaultDrive. @@ -32,31 +36,31 @@ public class DefaultDrive extends Command { * @param subsystem The drive subsystem this command will run on * @param driver The joystick to be used for calculations in speed and rotation */ - public DefaultDrive(SwerveDrive subsystem, Joystick driver, boolean slow) { - requires(subsystem); + public DefaultDrive(SwerveDrive subsystem1, Vision subsystem2, Joystick driver, double multiplier) { + requires(subsystem1); + requires(subsystem2); // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; + m_drive = subsystem1; + m_vision = subsystem2; m_joystick = driver; m_xSpeed = 0; m_ySpeed = 0; m_rot = 0; m_fieldRelative = true; - this.slow = slow; - + this.multiplier = multiplier; } // Called when the command is initially scheduled. @Override public void initialize() { heading = m_drive.getAngle().getDegrees(); - butX = new JoystickButton(m_joystick, 2); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { // deadzone - + SmartDashboard.putNumber("MODE", mode); if (m_drive.getGyroReset()) { heading = m_drive.getAngle().getDegrees(); m_drive.setGyroReset(false); @@ -66,7 +70,9 @@ public void execute() { m_ySpeed = 0; m_rot = 0; - if (!slow) { + if (!(m_joystick.getRawButtonPressed(1))) { + // y should be 0 when robot is facing ^ (and intake is facing driver station) + // x should be negative when intake facing driver station % if (Math.abs(m_joystick.getRawAxis(0)) > 0.1) { m_ySpeed = m_joystick.getRawAxis(0) * 0.5 * Constants.SwerveDriveConstants.kMaxSpeedMetersPerSecond; } @@ -90,7 +96,6 @@ public void execute() { double curHead = m_drive.getAngle().getDegrees(); - double difference = heading - curHead; if (m_rot == 0) { m_drive.drive(m_xSpeed, m_ySpeed, pid.calculate(curHead, heading), m_fieldRelative); @@ -100,6 +105,19 @@ public void execute() { heading = m_drive.getAngle().getDegrees(); } + // fill with correct button + if (m_joystick.getRawButtonPressed(6)) { + // to zero all wheels + SmartDashboard.putNumber("BUTTON PRESSED", 5); + } + } + // A - for vision + else { + m_vision.updateTargets(); + double rot = visionController.calculate(m_vision.getYaw(), 0); + m_drive.drive(0, 0, m_rot, m_fieldRelative); + } + } // Returns true when the command should end. diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/SpinTurret.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/SpinTurret.java index 7f274e1..7bfb7bc 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/SpinTurret.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/SpinTurret.java @@ -8,15 +8,22 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.command.InstantCommand; +import frc.robot.subsystems.SwerveDrive; import frc.robot.subsystems.Turret; +import frc.robot.subsystems.Vision; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html public class SpinTurret extends InstantCommand { - public SpinTurret(Turret subsystem, boolean manual, double speed) { - super(subsystem, () -> {subsystem.spin(manual, speed);}); - requires(subsystem); + public SpinTurret(Turret subsystem1, SwerveDrive subsystem2, Vision subsystem3, int mode, double speed) { + // mode 1: manual + // mode 2: auto adjust (without vision) + // mode 3: vision + super(subsystem1, () -> {subsystem1.spin(subsystem2, subsystem3, mode, speed);}); + requires(subsystem1); + requires(subsystem2); + requires(subsystem3); // Use addRequirements() here to declare subsystem dependencies. } diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/VisionCommand.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/VisionCommand.java new file mode 100644 index 0000000..b5757a1 --- /dev/null +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/VisionCommand.java @@ -0,0 +1,53 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.controller.PIDController; +import frc.robot.subsystems.SwerveDrive; +import frc.robot.subsystems.Vision; + +public class VisionCommand extends Command { + private final SwerveDrive m_drive; + private final Vision m_vision; + private PIDController controller = new PIDController(.1, 0, 0); + private double rotationSpeed; + + public VisionCommand(SwerveDrive subsystem1, Vision subsystem2) { + requires(subsystem1); + m_drive = subsystem1; + m_vision = subsystem2; + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() {} + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + m_vision.updateTargets(); + rotationSpeed = controller.calculate(m_vision.getYaw(), 0); + + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() {} + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() {} +} diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Remote.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Remote.java new file mode 100644 index 0000000..93be34a --- /dev/null +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Remote.java @@ -0,0 +1,33 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.command.Subsystem; +import java.io.*; +import java.util.*; +import java.util.HashMap.*; +import org.json.simple.JSONObject; + +/** Add your docs here. */ +public class Remote extends Subsystem { + public static void main(String args) throws FileNotFoundException { + JSONObject obj = new JSONObject(); + + obj.put("name", "foo"); + obj.put("num", new Integer(100)); + obj.put("balance", new Double(1000.21)); + obj.put("is_vip", new Boolean(true)); + + System.out.print(obj); + } + + + + @Override + public void initDefaultCommand() { + // Set the default command for a subsystem here. + // setDefaultCommand(new MySpecialCommand()); + } +} diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveModule.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveModule.java index 0700059..8ab716c 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -145,4 +145,10 @@ public void resetEncoders() { m_turningEncoder.setPosition(0); } + /** + * Physically zeroes wheel. (i hope) + */ + public void resetWheel() { + m_pidController.setReference(0, ControlType.kPosition); + } } diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java index 304555e..5030d88 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java @@ -9,7 +9,6 @@ import com.revrobotics.CANDigitalInput; import com.revrobotics.CANEncoder; -import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax.SoftLimitDirection; @@ -22,6 +21,7 @@ import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; +import frc.robot.commands.SpinTurret; public class Turret extends Subsystem { @@ -29,21 +29,19 @@ public class Turret extends Subsystem { // transfer to robot container private static final int motor = 10; private final CANSparkMax turretMotor; - private final CANPIDController m_turretPIDController; + private final PIDController m_turretPIDController; private CANEncoder turretEncoder; public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, setPoint, rotations; private CANDigitalInput m_reverseLimit; private DigitalInput limitSwitch; - public Vision vision; + // public Vision vision; public SwerveDrive swerve; private PIDController visionController = new PIDController(Constants.Vision.turretKP, Constants.Vision.turretKI, Constants.Vision.turretKD); /** * Creates a new Turret. */ - public Turret(Vision vision, SwerveDrive swerve) { - this.vision = vision; - this.swerve = swerve; + public Turret() { turretMotor = new CANSparkMax(motor, MotorType.kBrushless); turretMotor.restoreFactoryDefaults(); @@ -62,7 +60,7 @@ public Turret(Vision vision, SwerveDrive swerve) { turretEncoder = turretMotor.getEncoder(); turretEncoder.setPosition(0); - m_turretPIDController = turretMotor.getPIDController(); + m_turretPIDController = new PIDController(0, 0, 0); // PID coefficients kP = 0.1; @@ -77,9 +75,9 @@ public Turret(Vision vision, SwerveDrive swerve) { m_turretPIDController.setP(kP); m_turretPIDController.setI(kI); m_turretPIDController.setD(kD); - m_turretPIDController.setIZone(kIz); - m_turretPIDController.setFF(kFF); - m_turretPIDController.setOutputRange(kMinOutput, kMaxOutput); + // m_turretPIDController.setIZone(kIz); + // m_turretPIDController.setFF(kFF); + // m_turretPIDController.setOutputRange(kMinOutput, kMaxOutput); // m_reverseLimit = turretMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); // m_reverseLimit.enableLimitSwitch(true); @@ -94,7 +92,8 @@ public void periodic() { } public void setPosition(double setpoint) { - m_turretPIDController.setReference(setpoint, ControlType.kPosition); + turretMotor.set(m_turretPIDController.calculate(turretEncoder.getPosition(), 0)); + SmartDashboard.putNumber("SetPoint", setpoint); SmartDashboard.putNumber("ProcessVariable", turretEncoder.getPosition()); } @@ -103,7 +102,13 @@ public void stop() { turretMotor.set(0); } - public void spin(boolean manual, double speed) { + public void spin(SwerveDrive swerve, Vision vision, int mode, double speed) { + // mode 1: manual + // mode 2: auto adjust (without vision) + // mode 3: vision + + this.swerve = swerve; + mode = 2; double robotHeading = swerve.getHeading(); double targetPosition = 0; @@ -114,11 +119,15 @@ public void spin(boolean manual, double speed) { targetPosition = 360 - robotHeading; } - if (manual) - turretMotor.set(speed); - else + if (mode == 1) { + turretMotor.set(speed); + } else if (mode == 2) { // turretMotor.set(0); - m_turretPIDController.setReference(targetPosition, ControlType.kPosition); + setPosition(0); + // m_turretPIDController.setReference(0, ControlType.kPosition); + } else if (mode == 3) { + // vision + } } @Override diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Vision.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Vision.java index 28a09a9..0b23283 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Vision.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Vision.java @@ -1,113 +1,107 @@ -package frc.robot.subsystems; +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. -import frc.robot.Robot; -import frc.robot.Constants; -import frc.robot.RobotContainer; -import frc.robot.subsystems.SwerveDrive; +package frc.robot.subsystems; import java.util.List; -import org.photonvision.PhotonCamera; -// import org.photonvision.PhotonCamera2; -// import org.photonvision.PhotonCamera.SimplePipelineResult; -import org.photonvision.PhotonPipelineResult; -import org.photonvision.PhotonTrackedTarget; -import org.photonvision.PhotonUtils; +import org.photonvision.*; -import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.geometry.Pose2d; -import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.geometry.Transform2d; -import edu.wpi.first.wpilibj.geometry.Translation2d; - +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +/** Add your docs here. */ public class Vision extends Subsystem { - public final PhotonCamera camera = new PhotonCamera("MyCamera"); - - - public Vision() { - // PhotonPipelineResult result = camera.getLatestResult(); - - - // List targets = result.getTargets(); - // PhotonTrackedTarget target = targets.get(0); - - // // Get the yaw, pitch, and area from the camera. - // double yaw = camera.getBestTargetYaw(); - // double pitch = camera.getBestTargetPitch(); - // double area = camera.getBestTargetArea(); - - - // camera.setDriverMode(true); - // camera.setPipelineIndex(2); - - // // Check if the latest result has any targets. - // boolean hasTargets = result.hasTargets(); - - // // Get a list of currently tracked targets. - // // Get the pipeline latency. - // double latencySeconds = result.getLatencyMillis() / 1000.0; - - // // Get information from target. - // double targetYaw = target.getYaw(); - // double targetPitch = target.getPitch(); - // double targetArea = target.getArea(); - // double targetSkew = target.getSkew(); - // Transform2d pose = target.getCameraToTarget(); - // // Pose2d pose = target.getRobotRelativePose(); - - - // // Get distance to target. - // double distanceMeters = PhotonUtils.calculateDistanceToTargetMeters( - // Constants.Vision.kCameraHeight, Constants.Vision.kTargetHeight, - // Constants.Vision.kCameraPitch, Math.toRadians(camera.getBestTargetPitch())); - // //Calculate a translation from the camera to the target. - // Translation2d translation = PhotonUtils.estimateTargetTranslation2d( - // distanceMeters, Rotation2d.fromDegrees(-camera.getBestTargetYaw())); - - - - } - - public double getTargetYaw() { - // PhotonPipelineResult result = camera.getLatestResult(); - // List targets = result.getTargets(); - // if (!targets.isEmpty()) { - // PhotonTrackedTarget target = targets.get(0); - - // double targetYaw = target.getYaw(); - - // return targetYaw; - // } - return 0; - } - - public double getTargetPitch() { - PhotonPipelineResult result = camera.getLatestResult(); - List targets = result.getTargets(); - if (!targets.isEmpty()) { - PhotonTrackedTarget target = targets.get(0); - - double targetPitch = target.getPitch(); - - return targetPitch; - } - return 0; - } - - public boolean hasTargets() { - PhotonPipelineResult result = camera.getLatestResult(); - - boolean hasTargets = result.hasTargets(); - - return hasTargets; - } - - @Override - protected void initDefaultCommand() { - // TODO Auto-generated method stub - - } - -} + private PhotonCamera _camera; + private List _targets; + private PhotonTrackedTarget _target; + private boolean _hasTargets; + private PIDController _controller; + private PhotonPipelineResult _result; + // Put methods for controlling this subsystem + // here. Call these from Commands. + public Vision(PhotonCamera camera) { + // photonvision.local:5800 + + _camera = camera; + // Set driver mode to on. + _camera.setDriverMode(true); + + // Change pipeline to 2 + _camera.setPipelineIndex(2); + + updateTargets(); + + _controller = new PIDController(.1, 0, 0); + } + + /* STANDARD FUNCTIONS */ + public void updateTargets() { + // Get the latest pipeline result. + _result = _camera.getLatestResult(); + + // Check if the latest result has any targets. + _hasTargets = _result.hasTargets(); + + // Get a list of currently tracked targets. + _targets = _result.getTargets(); + + // Get the current best target. + _target = _result.getBestTarget(); + + // Get the pipeline latency. + // double latencySeconds = getLatencyMillis() / 1000.0; // around 3 ms + + // Blink the LEDs. + _camera.setLED(LEDMode.kOff); + } + + public void periodic() { + SmartDashboard.putNumber("Yaw", getYaw()); + SmartDashboard.putNumber("Pitch", getPitch()); + SmartDashboard.putNumber("Area", getArea()); + SmartDashboard.putNumber("Skew", getSkew()); + } + + public double getYaw() { + double yaw = _target.getYaw(); + return yaw; + } + + public double getPitch() { + double pitch = _target.getPitch(); + return pitch; + } + + public double getArea() { + double area = _target.getArea(); + return area; + } + + public double getSkew() { + double skew = _target.getSkew(); + return skew; + } + + public Transform2d getPose() { + Transform2d pose = _target.getCameraToTarget(); + return pose; + } + /* ****************** */ + + /* TO BE USED FOR COMMANDS */ + public double getRotationSpeed() { + updateTargets(); + double rotationSpeed = _controller.calculate(_result.getBestTarget().getYaw(), 0); + return rotationSpeed; + } + + @Override + public void initDefaultCommand() { + // Set the default command for a subsystem here. + // setDefaultCommand(new MySpecialCommand()); + } +} diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/f.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/f.java new file mode 100644 index 0000000..271bbf2 --- /dev/null +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/f.java @@ -0,0 +1,19 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.command.Subsystem; + +/** Add your docs here. */ +public class f extends Subsystem { + // Put methods for controlling this subsystem + // here. Call these from Commands. + + @Override + public void initDefaultCommand() { + // Set the default command for a subsystem here. + // setDefaultCommand(new MySpecialCommand()); + } +}