diff --git a/RobotCode2021-Imported/.vscode/launch.json b/RobotCode2021-Imported/.vscode/launch.json index c9c9713..84c6c0d 100644 --- a/RobotCode2021-Imported/.vscode/launch.json +++ b/RobotCode2021-Imported/.vscode/launch.json @@ -4,18 +4,24 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - + { + "type": "java", + "name": "Launch Main", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "RobotCode2021-Imported" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java b/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java index 7c87f74..7fd9617 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java @@ -111,8 +111,8 @@ public void autonomousInit() { // DriverStation.reportError("Unable to open trajectory" + trajectoryJSON, ex.getStackTrace()); // } - - // m_autonomousCommand = m_robotContainer.getAutonomousCommand(null); +//enable for autos not on smart dashboard (barrel, bounce, slalom) + m_autonomousCommand = m_robotContainer.getAutonomousCommand(null); // schedule the autonomous command (example) if (m_autonomousCommand != null) { @@ -165,6 +165,8 @@ public void teleopInit() { m_autonomousCommand.cancel(); } RobotContainer.swerveDrive.zeroHeading(); + // RobotContainer.swerveDrive.resetEncoders(); + } double x = 0; diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java index e303d27..7bb74bb 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java @@ -64,8 +64,10 @@ public class RobotContainer { public final static Shooter shooter = new Shooter(); private final Climb climb = new Climb(); private final PhotonCamera camera = new PhotonCamera("TurretCamera"); + private final PhotonCamera camera1 = new PhotonCamera("OtherCamera"); private final Vision vision = new Vision(camera); - private final Turret turret = new Turret(swerveDrive); + private final Vision vision1 = new Vision(camera1); + private final Turret turret = new Turret(swerveDrive, vision); // The driver's controller public static Joystick m_driverController = new Joystick(OIConstants.kDriverControllerPort); public static Joystick m_operatorController = new Joystick(1); @@ -94,17 +96,9 @@ public RobotContainer() { swerveDrive.setDefaultCommand(new DefaultDrive(swerveDrive, 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)); + turret.setDefaultCommand(new SpinTurret(turret, vision, 1, 0)); + vision.setDefaultCommand(new RunVision(vision)); - // swerveDrive.setDefaultCommand( - - // 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)); } /** @@ -115,80 +109,82 @@ public RobotContainer() { * calling passing it to a {@link JoystickButton}. */ 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 butXd = new JoystickButton(m_driverController, 3); - JoystickButton rBump = new JoystickButton(m_operatorController, 6); + // new buttons 3/27/21 + JoystickButton butA = new JoystickButton(m_operatorController, 1); + JoystickButton butB = new JoystickButton(m_operatorController, 2); + JoystickButton butX = new JoystickButton(m_operatorController, 3); + JoystickButton butY = new JoystickButton(m_operatorController, 4); JoystickButton lBump = new JoystickButton(m_operatorController, 5); + JoystickButton rBump = new JoystickButton(m_operatorController, 6); + JoystickButton lWing = new JoystickButton(m_operatorController, 7); + JoystickButton rWing = new JoystickButton(m_operatorController, 8); JoystickButton lAnal = new JoystickButton(m_operatorController, 9); JoystickButton rAnal = new JoystickButton(m_operatorController, 10); - JoystickButton gyro = new JoystickButton(m_driverController, 7); - //JoystickButton ok = new JoystickButton(m_driverController, 7); + + + 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); + + butA.whenPressed(new ExtendIntake(intake, m_operatorController)); + // butA.whenReleased(new RetractIntake(intake)); + // butB.whileHeld(new SpinTurret(turret, vision, 1, 0.25)); + // butX.whileHeld(new SpinTurret(turret, vision, 1, -0.25)); + + //shooter and conveyor move together *** change for PowerPortChallenges + butY.whileHeld(new RunShooter(shooter)); + butY.whenReleased(new StopShooter(shooter)); + rBump.whileHeld(new ControlConveyor(conveyor, 1)); + rBump.whenReleased(new ControlConveyor(conveyor, 0)); + lBump.whileHeld(new ControlConveyor(conveyor, -1)); + lBump.whenReleased(new ControlConveyor(conveyor, 0)); + + // rWing for vision + lAnal.whileHeld(new MoveHood(shooter, 1)); + rAnal.whileHeld(new MoveHood(shooter, -1)); + + // 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); - 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); + // AccuracyChallenge accuracyChallenge1 = new AccuracyChallenge(swerveDrive, intake, shooter, theta); + // SmartDashboard.putData(Scheduler.getInstance()); + // SmartDashboard.putData("Accuracy Challenge", accuracyChallenge1); - - SlalomPath1 Slolam = new SlalomPath1(swerveDrive, theta); - + PowerPortChallenge ppcc = new PowerPortChallenge(swerveDrive, intake, shooter, theta); SmartDashboard.putData(Scheduler.getInstance()); - SmartDashboard.putData("Slalom Path", Slolam); + SmartDashboard.putData("PowerPortChallenge", ppcc); + GalacticPathA galA = new GalacticPathA(swerveDrive, intake, theta); + SmartDashboard.putData(Scheduler.getInstance()); + SmartDashboard.putData("Galactic Path", galA); - // A button - butA.whileHeld(new ExtendIntake(intake, m_operatorController)); - butA.whenReleased(new RetractIntake(intake)); - // right bumper - rBump.whileHeld(new RunShooter(shooter)); - rBump.whenReleased(new StopShooter(shooter)); - - // left analog center - lAnal.whileHeld(new MoveHood(shooter, -1)); - + // driver X button - slow + butXd.whileHeld(new DefaultDrive(swerveDrive, m_driverController, 0.35)); + // butAd.whenPressed(new InstantCommand(swerveDrive::zeroWheels)); - // 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)); - - // Y button - butY.whileHeld(new SpinTurret(turret, true, -0.25)); - butY.whenReleased(new SpinTurret(turret, true, 0)); - - // driver X button - slow - butXd.whileHeld(new DefaultDrive(swerveDrive, m_driverController, 0.35)); - turbo.whileHeld(new DefaultDrive(swerveDrive, m_driverController, 2)); - gyro.whenPressed(new InstantCommand(swerveDrive::zeroHeading)); - - //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)); + // butBd.whenPressed(new SideStep(swerveDrive, theta)); + + turbo.whileHeld(new DefaultDrive(swerveDrive, m_driverController, 2)); + // butAd.whileHeld(new SpinTurret(turret, vision, 2, 0)); + gyro.whenPressed(new InstantCommand(swerveDrive::zeroHeading)); } @@ -203,10 +199,10 @@ public static String getCoords() { */ public Command getAutonomousCommand(Trajectory trajectory) { - GalacticA galA = new GalacticA(swerveDrive, theta); - - return galA; + PowerPortChallenge ppc = new PowerPortChallenge(swerveDrive, intake, shooter, theta); + return ppc; + } } diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/AccuracyChallenge.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/AccuracyChallenge.java new file mode 100644 index 0000000..9aaa112 --- /dev/null +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/AccuracyChallenge.java @@ -0,0 +1,75 @@ +// 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.WaitCommand; + +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 AccuracyChallenge extends CommandGroup { + /** Add your docs here. */ + + private final SwerveDrive swerveDrive; + private final Intake intake; + + public AccuracyChallenge(SwerveDrive swerveDrive, Intake intake, ProfiledPIDController theta) { + requires(swerveDrive); + requires(intake); + + this.swerveDrive = swerveDrive; + this.intake = intake; + + // Create config for trajectory + TrajectoryConfig config = new TrajectoryConfig(1, + AutoConstants.kMaxAccelerationMetersPerSecondSquared) + // Add kinematics to ensure max speed is actually obeyed + // .setKinematics(SwerveDriveConstants.kDriveKinematics) + .setEndVelocity(0); + + Trajectory traject = TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(-Math.PI / 2)), List.of(), + + //direction robot moves + new Pose2d(-2, 0, new Rotation2d(-Math.PI / 2)), config); + + SwerveControllerCommand swerveControllerCommand1 = 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 + + ); +WaitCommand wait = new WaitCommand(15); +IntakeCommand intakeCommand = new IntakeCommand(intake); + + + addSequential(swerveControllerCommand1); + // addSequential(wait); + // addSequential(intakeCommand); + +// addSequential(swerveControllerCommand2); + } +} diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/BarrelPath.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/BarrelPath.java index b25e51a..99fd655 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/BarrelPath.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/BarrelPath.java @@ -31,7 +31,7 @@ public BarrelPath(SwerveDrive swerveDrive, ProfiledPIDController theta) { AutoConstants.kMaxAccelerationMetersPerSecondSquared) // Add kinematics to ensure max speed is actually obeyed // .setKinematics(SwerveDriveConstants.kDriveKinematics) - .setEndVelocity(1.5); + .setEndVelocity(2); // TrajectoryConfig config1 = new TrajectoryConfig(1, // AutoConstants.kMaxAccelerationMetersPerSecondSquared) @@ -47,12 +47,12 @@ public BarrelPath(SwerveDrive swerveDrive, ProfiledPIDController theta) { new Translation2d(-1.63, -3.224939), new Translation2d(-1.663700, -1.846359), new Translation2d(-.180766, -1.846359), - new Translation2d(-.180766, -5.230811), + new Translation2d(-.180766, -5.030811), new Translation2d(1.439370, -5.430811), new Translation2d(1.439370, -3.995877), new Translation2d(-1.827272, -3.995877), new Translation2d(-1.827272, -6.816515), - new Translation2d(0.15, -6.816515) + new Translation2d(0, -6.816515) ), 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 index 724133a..bb9ee88 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/GalacticA.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/GalacticA.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.CommandGroup; import edu.wpi.first.wpilibj.command.InstantCommand; +import edu.wpi.first.wpilibj.command.Scheduler; import java.util.List; @@ -25,44 +26,55 @@ 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); - + private final RobotContainer m_robotContainer; + // private final Intake m_intake; + + public GalacticA(SwerveDrive swerveDrive, ProfiledPIDController theta) { + m_robotContainer = new RobotContainer(); + Intake m_intake = new Intake(); + + + // Create config for trajectory + + //top speed: 2.1 + TrajectoryConfig config = new TrajectoryConfig(.5, + AutoConstants.kMaxAccelerationMetersPerSecondSquared) + // Add kinematics to ensure max speed is actually obeyed + // .setKinematics(SwerveDriveConstants.kDriveKinematics) + + //change to 1.5 + .setEndVelocity(.5); + + Trajectory trajectory = TrajectoryGenerator + .generateTrajectory(new Pose2d(0, 0, new Rotation2d(0)), List.of( + + ), + // direction robot moves + new Pose2d(0, -3, new Rotation2d(0)), 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 + ); + + m_intake.extendAuto(); + m_intake.runWheelsAuto(); + + IntakeCommand intakeCommand = new IntakeCommand(m_intake); + + + addParallel(swerveControllerCommand1); + addSequential(intakeCommand); } - -} +} \ No newline at end of file diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/GalacticPathA.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/GalacticPathA.java new file mode 100644 index 0000000..c81b051 --- /dev/null +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/GalacticPathA.java @@ -0,0 +1,75 @@ +// 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 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.RobotContainer; +import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.SwerveDriveConstants; +import frc.robot.subsystems.*; + +public class GalacticPathA extends CommandGroup { + /** Add your docs here. */ + private final SwerveDrive swerveDrive; + private final Intake intake; + + public GalacticPathA(SwerveDrive swerveDrive, Intake intake, ProfiledPIDController theta) { + // private final Intake m_intake; + requires(swerveDrive); + requires(intake); + + this.swerveDrive = swerveDrive; + this.intake = intake; + // Create config for trajectory + // top speed: 2.1 + TrajectoryConfig config = new TrajectoryConfig(.5, AutoConstants.kMaxAccelerationMetersPerSecondSquared) + // Add kinematics to ensure max speed is actually obeyed + // .setKinematics(SwerveDriveConstants.kDriveKinematics) + // set end: 1.5 + .setEndVelocity(.5); + + Trajectory traject = TrajectoryGenerator.generateTrajectory( + + new Pose2d(0, 0, new Rotation2d(0)), List.of( + + + ), + //direction robot moves + new Pose2d(1.01, 0, new Rotation2d(-Math.PI / 2)), config); + + SwerveControllerCommand swerveControllerCommand1 = new SwerveControllerCommand(traject, (0), swerveDrive::getPose, + // Functional + // feed + // supplier + SwerveDriveConstants.kDriveKinematics, + + // Position controllers + new PIDController(AutoConstants.kPXController, 1, AutoConstants.kDXController), + new PIDController(AutoConstants.kPYController, 1, AutoConstants.kDYController), theta, + + swerveDrive::setModuleStates, + + swerveDrive + + ); + + addParallel(swerveControllerCommand1); + IntakeCommand intakeCommand = new IntakeCommand(intake); + addSequential(intakeCommand); + } +} \ No newline at end of file diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/PowerPortChallenge.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/PowerPortChallenge.java new file mode 100644 index 0000000..4832891 --- /dev/null +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/PowerPortChallenge.java @@ -0,0 +1,108 @@ +// 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.WaitCommand; + +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 PowerPortChallenge extends CommandGroup { + /** Add your docs here. */ + + private final SwerveDrive swerveDrive; + private final Intake intake; + private final Shooter shoot; + + public PowerPortChallenge(SwerveDrive swerveDrive, Intake intake, Shooter shoot, ProfiledPIDController theta) { + requires(swerveDrive); + requires(shoot); + requires(intake); + + this.swerveDrive = swerveDrive; + this.intake = intake; + this.shoot = shoot; + + // Create config for trajectory + TrajectoryConfig config = new TrajectoryConfig(1, + AutoConstants.kMaxAccelerationMetersPerSecondSquared) + // Add kinematics to ensure max speed is actually obeyed + // .setKinematics(SwerveDriveConstants.kDriveKinematics) + .setEndVelocity(0); + + Trajectory traject = TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(-Math.PI / 2)), List.of(), + + //direction robot moves + new Pose2d(-2, 0, new Rotation2d(-Math.PI / 2)), config); + + SwerveControllerCommand swerveControllerCommand1 = 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 + + ); + + TrajectoryConfig config2 = new TrajectoryConfig(1, + AutoConstants.kMaxAccelerationMetersPerSecondSquared) + // Add kinematics to ensure max speed is actually obeyed + // .setKinematics(SwerveDriveConstants.kDriveKinematics) + .setStartVelocity(1); + +Trajectory traject2 = TrajectoryGenerator.generateTrajectory( +new Pose2d(0, 0, new Rotation2d(-Math.PI / 2)), List.of(), + + //direction robot moves +new Pose2d(-2, 0, new Rotation2d(-Math.PI / 2)), config); + +SwerveControllerCommand swerveControllerCommand2 = 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 + +); +WaitCommand wait = new WaitCommand(15); +IntakeCommand intakeCommand = new IntakeCommand(intake); +RunShooter runShoot = new RunShooter(shoot); + + addParallel(intakeCommand); + addSequential(swerveControllerCommand1); + // addSequential(wait); + // addSequential(swerveControllerCommand2); + // addSequential(wait); + // addSequential(swerveControllerCommand1); + // addSequential(wait); + // addSequential(swerveControllerCommand2); + // addSequential(wait); + } +} diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/SlalomPath1.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/SlalomPath1.java index 89fc103..a9c442d 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/SlalomPath1.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/SlalomPath1.java @@ -31,22 +31,22 @@ public SlalomPath1(SwerveDrive swerveDrive, ProfiledPIDController theta) { AutoConstants.kMaxAccelerationMetersPerSecondSquared) // Add kinematics to ensure max speed is actually obeyed // .setKinematics(SwerveDriveConstants.kDriveKinematics) - .setEndVelocity(1.5); + .setEndVelocity(2); TrajectoryConfig config1 = new TrajectoryConfig(2.2, AutoConstants.kMaxAccelerationMetersPerSecondSquared) // Add kinematics to ensure max speed is actually obeyed // .setKinematics(SwerveDriveConstants.kDriveKinematics) - .setStartVelocity(1.5); + .setStartVelocity(2); Trajectory traject = TrajectoryGenerator.generateTrajectory( new Pose2d(0, 0, new Rotation2d(0)), List.of( //start s-shape - new Translation2d(0, -1.1), - new Translation2d(1.1, -1.6), - new Translation2d(1.88, -1.9), + new Translation2d(-.25, -1.4), + new Translation2d(1.1, -1.7), + new Translation2d(1.88, -2), new Translation2d(1.9, -4), @@ -66,11 +66,11 @@ public SlalomPath1(SwerveDrive swerveDrive, ProfiledPIDController theta) { Trajectory traject1 = TrajectoryGenerator.generateTrajectory( //direction robot moves new Pose2d(-.5, -5.5, new Rotation2d(Math.PI / 2)), List.of( - new Translation2d(-.5, -1.45), - new Translation2d(1.1, -1.5) + new Translation2d(-.5, -1.55), + new Translation2d(1.5, -1.65) ), - new Pose2d(1.8, 0, new Rotation2d(Math.PI / 2)), config1); + new Pose2d(2.1, 0, new Rotation2d(Math.PI / 2)), config1); // new Pose2d(0, -7, new Rotation2d(-Math.PI / 2)), config); // String trajectoryJSON = "../paths/Slalom.wpilib.json"; diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/ControlConveyor.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/ControlConveyor.java index de17209..d24cc7e 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/ControlConveyor.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/ControlConveyor.java @@ -16,10 +16,10 @@ public class ControlConveyor extends CommandGroup { /** * Creates a new ControlConveyor. */ - public ControlConveyor(Conveyor subsystem) { + public ControlConveyor(Conveyor subsystem, int speed) { requires(subsystem); m_conveyor = subsystem; - m_speed = 1; + m_speed = speed; // Use addRequirements() here to declare subsystem dependencies. } @@ -31,7 +31,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_conveyor.manualControl(m_speed); + m_conveyor.manualControl(m_speed / 1.5); } 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 2b872f2..93c8195 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/DefaultDrive.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/DefaultDrive.java @@ -76,11 +76,11 @@ public void execute() { m_ySpeed = 0; m_rot = 0; - if (Math.abs(m_joystick.getRawAxis(1)) > 0.1) { - m_ySpeed = -m_joystick.getRawAxis(1) * 0.5 * multiplier * Constants.SwerveDriveConstants.kMaxSpeedMetersPerSecond; - } if (Math.abs(m_joystick.getRawAxis(0)) > 0.1) { - m_xSpeed = m_joystick.getRawAxis(0) * 0.5 * multiplier * Constants.SwerveDriveConstants.kMaxSpeedMetersPerSecond; + m_ySpeed = -m_joystick.getRawAxis(0) * 0.5 * multiplier * Constants.SwerveDriveConstants.kMaxSpeedMetersPerSecond; + } + if (Math.abs(m_joystick.getRawAxis(1)) > 0.1) { + m_xSpeed = -m_joystick.getRawAxis(1) * 0.5 * multiplier * Constants.SwerveDriveConstants.kMaxSpeedMetersPerSecond; } if (Math.abs(m_joystick.getRawAxis(4)) > 0.2) { m_rot = m_joystick.getRawAxis(4) * 0.5 * multiplier * (Math.PI); diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/IntakeCommand.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/IntakeCommand.java new file mode 100644 index 0000000..6b1b306 --- /dev/null +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/IntakeCommand.java @@ -0,0 +1,37 @@ +// 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.command.CommandGroup; +import frc.robot.subsystems.Intake; + +public class IntakeCommand extends CommandGroup { + /** Creates a new IntakeCommand. */ + private final Intake m_intake; + + public IntakeCommand(Intake subsystem) { + requires(subsystem); + m_intake = subsystem; + // Use addRequirements() here to declare subsystem dependencies. + } + + +// Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_intake.runWheelsAuto(); + m_intake.extendAuto(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/MoveHood.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/MoveHood.java index a951194..5419e6d 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/MoveHood.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/MoveHood.java @@ -20,6 +20,7 @@ public class MoveHood extends CommandGroup { public MoveHood(Shooter subsystem, double pos) { requires(subsystem); m_shooter = subsystem; + m_pos = pos; // Use addRequirements() here to declare subsystem dependencies. } diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/RunVision.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/RunVision.java new file mode 100644 index 0000000..0c93242 --- /dev/null +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/RunVision.java @@ -0,0 +1,27 @@ +// 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.command.InstantCommand; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.Vision; + +/** Add your docs here. */ +public class RunVision extends InstantCommand { + private Vision vision; + /** Add your docs here. */ + public RunVision(Vision vision) { + super(); + requires(vision); + this.vision = vision; + } + + // Called once when the command executes + @Override + protected void initialize() { + vision.updateTargets(); + SmartDashboard.putNumber("PITCH", vision.getPitch()); + } +} 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..3fddb15 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/SpinTurret.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/SpinTurret.java @@ -9,14 +9,16 @@ import edu.wpi.first.wpilibj.command.InstantCommand; 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 subsystem, Vision subsystem2, int mode, double speed) { + super(subsystem, () -> {subsystem.spin(mode, speed);}); + requires(subsystem); + requires(subsystem2); // Use addRequirements() here to declare subsystem dependencies. } @@ -24,4 +26,4 @@ public SpinTurret(Turret subsystem, boolean manual, double speed) { @Override public void initialize() { } -} +} \ No newline at end of file diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Intake.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Intake.java index 420da8b..02dd688 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Intake.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Intake.java @@ -29,17 +29,18 @@ public Intake() { public void extend(Joystick joystick) { intakeSolenoid.set(Value.kForward); setExtended(true); - double speedIn = joystick.getRawAxis(2); - double speedOut = joystick.getRawAxis(3); + double speedIn = joystick.getRawAxis(3); + double speedOut = joystick.getRawAxis(2); if (speedIn > 0.1) - intakeMotor.set(speedIn / 3.5); + intakeMotor.set(speedIn / 1.5); else if (speedOut > 0.1) intakeMotor.set(-speedOut); else intakeMotor.set(0); } + public void retract() { intakeSolenoid.set(Value.kReverse); setExtended(false); @@ -50,7 +51,7 @@ public void runWheels(Joystick joystick) { double speedIn = joystick.getRawAxis(3); if (getExtended()) { if (speedIn > 0.1) - intakeMotor.set(speedIn / 3.5); + intakeMotor.set(speedIn / 2); else if (speedOut > 0.1) intakeMotor.set(-speedOut); else @@ -58,6 +59,17 @@ else if (speedOut > 0.1) } } + //auto intakes + public void runWheelsAuto() { + intakeMotor.set(.5); + } + + public void extendAuto() { + intakeSolenoid.set(Value.kForward); + setExtended(true); + intakeMotor.set(.5); + } + public boolean getExtended() { return extended; } diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Shooter.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Shooter.java index 567f5fd..af61d8d 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Shooter.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Shooter.java @@ -73,7 +73,7 @@ public Shooter() { } public void runShooter() { - setPoint = SmartDashboard.getNumber("Shooter Velocity", 4550); + setPoint = SmartDashboard.getNumber("Shooter Velocity", 1000); m_pidController.setP(30); m_pidController.setReference(setPoint, ControlType.kVelocity); @@ -110,7 +110,8 @@ public void periodic() { } public boolean atSpeed() { - return Math.abs(setPoint - m_encoder.getVelocity()) < 300; // play with the number (go up to 1,000) + //was 300 + return Math.abs(setPoint - m_encoder.getVelocity()) < 1000; // play with the number (go up to 1,000) } @Override diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveDrive.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveDrive.java index 6a725fc..e3c5f1e 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveDrive.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveDrive.java @@ -178,6 +178,7 @@ public void resetEncoders() { */ public void zeroHeading() { navX.reset(); + // resetEncoders(); gyroReset = true; } 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 6dd49b5..b934757 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -143,7 +143,7 @@ public void setDesiredState(SwerveModuleState state) { public void resetEncoders() { m_driveEncoder.setPosition(0); // m_turningEncoder.setPosition(Math.PI-m_absoluteEncoder.getPosition()); - m_turningEncoder.setPosition(0); + // m_turningEncoder.setPosition(0); } /** 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 d78f721..5435f3f 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java @@ -30,20 +30,20 @@ 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(SwerveDrive swerve) { - // this.vision = vision; + public Turret(SwerveDrive swerve, Vision vision) { + this.vision = vision; this.swerve = swerve; turretMotor = new CANSparkMax(motor, MotorType.kBrushless); @@ -63,7 +63,7 @@ public Turret(SwerveDrive swerve) { turretEncoder = turretMotor.getEncoder(); turretEncoder.setPosition(0); - m_turretPIDController = turretMotor.getPIDController(); + m_turretPIDController = new PIDController(0, 0, 0); // PID coefficients kP = 0.1; @@ -75,12 +75,9 @@ public Turret(SwerveDrive swerve) { kMinOutput = -1; // // set PID coefficients - 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.setP(kP); + // m_turretPIDController.setI(kI); + // m_turretPIDController.setD(kD); // m_reverseLimit = turretMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); // m_reverseLimit.enableLimitSwitch(true); @@ -95,7 +92,7 @@ public void periodic() { } public void setPosition(double setpoint) { - m_turretPIDController.setReference(setpoint, ControlType.kPosition); + m_turretPIDController.calculate(turretEncoder.getPosition(), setpoint); SmartDashboard.putNumber("SetPoint", setpoint); SmartDashboard.putNumber("ProcessVariable", turretEncoder.getPosition()); } @@ -104,7 +101,7 @@ public void stop() { turretMotor.set(0); } - public void spin(boolean manual, double speed) { + public void spin(int mode, double speed) { double robotHeading = swerve.getHeading(); double targetPosition = 0; @@ -115,11 +112,18 @@ public void spin(boolean manual, double speed) { targetPosition = 360 - robotHeading; } - if (manual) + if (mode == 1) { turretMotor.set(speed); - else + } else { + if (vision.getHasTargets()) { + double pitch = vision.getPitch(); + SmartDashboard.putNumber("PITCH", pitch); + turretMotor.set(vision.getRotationSpeed()); + } + + } // turretMotor.set(0); - setPosition(targetPosition); + // setPosition(targetPosition); // m_turretPIDController.setReference(targetPosition, ControlType.kPosition); } 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 09df2ed..33243ef 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Vision.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Vision.java @@ -49,12 +49,14 @@ public void updateTargets() { if (_hasTargets) { // Get a list of currently tracked targets. _targets = _result.getTargets(); + SmartDashboard.putBoolean("TARGET ACQUIRED", true); // Get the current best target. _target = _result.getBestTarget(); } else { _targets = null; _target = null; + SmartDashboard.putBoolean("TARGET ACQUIRED", false); } // Get the pipeline latency. @@ -84,7 +86,7 @@ public double getPitch() { double pitch = _target.getPitch(); return pitch; } - return 0.0; + return 11.0; } public double getArea() { @@ -110,6 +112,11 @@ public Transform2d getPose() { } return new Transform2d(); } + + public boolean getHasTargets() { + return _hasTargets; + } + /* ****************** */ /* TO BE USED FOR COMMANDS */