From 6aff397f4fbd074b69547734b6790e6d6a7a2115 Mon Sep 17 00:00:00 2001 From: Team 1533 Driver Station <32420432+team1533@users.noreply.github.com> Date: Sat, 13 Mar 2021 16:56:25 -0500 Subject: [PATCH 01/11] intake command and galactic --- .../main/java/frc/robot/RobotContainer.java | 6 +- .../frc/robot/commands/Auto/GalacticA.java | 69 ++++++++++--------- .../frc/robot/commands/IntakeCommand.java | 37 ++++++++++ .../java/frc/robot/subsystems/Intake.java | 12 ++++ 4 files changed, 88 insertions(+), 36 deletions(-) create mode 100644 RobotCode2021-Imported/src/main/java/frc/robot/commands/IntakeCommand.java diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java index e303d27..945ebf0 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java @@ -203,10 +203,12 @@ public static String getCoords() { */ public Command getAutonomousCommand(Trajectory trajectory) { - GalacticA galA = new GalacticA(swerveDrive, theta); + // GalacticA galA = new GalacticA(swerveDrive, theta); - return galA; + // return galA; + BouncePath galA = new BouncePath(swerveDrive, theta); + return galA; } } 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..adb1bbe 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 @@ -27,42 +27,43 @@ 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 - - ); + private final RobotContainer m_robotContainer; + private final Intake m_intake; + + public GalacticA(SwerveDrive swerveDrive, ProfiledPIDController theta) { + m_robotContainer = new RobotContainer(); + m_intake = new 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(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 + ); + IntakeCommand intakeCommand = new IntakeCommand(m_intake); addSequential(swerveControllerCommand1); - + // addSequential(intakeCommand); } - -} +} \ No newline at end of file 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/subsystems/Intake.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Intake.java index 420da8b..7944e0b 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Intake.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Intake.java @@ -40,6 +40,7 @@ else if (speedOut > 0.1) intakeMotor.set(0); } + public void retract() { intakeSolenoid.set(Value.kReverse); setExtended(false); @@ -58,6 +59,17 @@ else if (speedOut > 0.1) } } + //auto intakes + public void runWheelsAuto() { + intakeMotor.set(.15); + } + + public void extendAuto() { + intakeSolenoid.set(Value.kForward); + setExtended(true); + intakeMotor.set(.15); + } + public boolean getExtended() { return extended; } From fe8a81eddd2b217cff9a513ca3f63f8e2322be4e Mon Sep 17 00:00:00 2001 From: Team 1533 Driver Station <32420432+team1533@users.noreply.github.com> Date: Tue, 16 Mar 2021 21:02:31 -0400 Subject: [PATCH 02/11] intake and drive at the same time --- .../main/java/frc/robot/RobotContainer.java | 16 ++-- .../frc/robot/commands/Auto/GalacticA.java | 23 ++++-- .../robot/commands/Auto/GalacticPathA.java | 75 +++++++++++++++++++ .../java/frc/robot/subsystems/Intake.java | 12 +-- .../java/frc/robot/subsystems/Shooter.java | 2 +- .../frc/robot/subsystems/SwerveDrive.java | 1 + 6 files changed, 107 insertions(+), 22 deletions(-) create mode 100644 RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/GalacticPathA.java diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java index 945ebf0..4923925 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java @@ -64,7 +64,9 @@ 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 Vision vision1 = new Vision(camera1); private final Turret turret = new Turret(swerveDrive); // The driver's controller public static Joystick m_driverController = new Joystick(OIConstants.kDriverControllerPort); @@ -131,23 +133,21 @@ private void configureButtonBindings() { // 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 Slolam = new SlalomPath1(swerveDrive, theta); SmartDashboard.putData(Scheduler.getInstance()); SmartDashboard.putData("Slalom Path", Slolam); + GalacticPathA galA = new GalacticPathA(swerveDrive, intake, theta); + SmartDashboard.putData(Scheduler.getInstance()); + SmartDashboard.putData("GalacticA", galA); // A button butA.whileHeld(new ExtendIntake(intake, m_operatorController)); @@ -203,12 +203,10 @@ public static String getCoords() { */ public Command getAutonomousCommand(Trajectory trajectory) { - // GalacticA galA = new GalacticA(swerveDrive, theta); - - // return galA; - BouncePath galA = new BouncePath(swerveDrive, theta); + GalacticA galA = new GalacticA(swerveDrive, theta); return galA; + } } 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 adb1bbe..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,23 +26,28 @@ import frc.robot.Constants.SwerveDriveConstants; import frc.robot.subsystems.*; + public class GalacticA extends CommandGroup { /** Add your docs here. */ private final RobotContainer m_robotContainer; - private final Intake m_intake; + // private final Intake m_intake; public GalacticA(SwerveDrive swerveDrive, ProfiledPIDController theta) { m_robotContainer = new RobotContainer(); - m_intake = new Intake(); + Intake m_intake = new Intake(); // Create config for trajectory - TrajectoryConfig config = new TrajectoryConfig(2.1, + + //top speed: 2.1 + TrajectoryConfig config = new TrajectoryConfig(.5, AutoConstants.kMaxAccelerationMetersPerSecondSquared) // Add kinematics to ensure max speed is actually obeyed // .setKinematics(SwerveDriveConstants.kDriveKinematics) - .setEndVelocity(1.5); + + //change to 1.5 + .setEndVelocity(.5); Trajectory trajectory = TrajectoryGenerator .generateTrajectory(new Pose2d(0, 0, new Rotation2d(0)), List.of( @@ -60,10 +66,15 @@ public GalacticA(SwerveDrive swerveDrive, ProfiledPIDController theta) { swerveDrive::setModuleStates, swerveDrive ); + + m_intake.extendAuto(); + m_intake.runWheelsAuto(); + IntakeCommand intakeCommand = new IntakeCommand(m_intake); + - addSequential(swerveControllerCommand1); - // addSequential(intakeCommand); + 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..12fd9c5 --- /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(1, AutoConstants.kMaxAccelerationMetersPerSecondSquared) + // Add kinematics to ensure max speed is actually obeyed + // .setKinematics(SwerveDriveConstants.kDriveKinematics) + // set end: 1.5 + .setEndVelocity(1); + + Trajectory traject = TrajectoryGenerator.generateTrajectory( + + new Pose2d(0, 0, new Rotation2d(-Math.PI/2)), List.of( + + + ), + //direction robot moves + new Pose2d(5, 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/subsystems/Intake.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Intake.java index 7944e0b..221a007 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Intake.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Intake.java @@ -29,11 +29,11 @@ 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 / 2); else if (speedOut > 0.1) intakeMotor.set(-speedOut); else @@ -51,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 @@ -61,13 +61,13 @@ else if (speedOut > 0.1) //auto intakes public void runWheelsAuto() { - intakeMotor.set(.15); + intakeMotor.set(.5); } public void extendAuto() { intakeSolenoid.set(Value.kForward); setExtended(true); - intakeMotor.set(.15); + intakeMotor.set(.5); } public boolean getExtended() { 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..b869d97 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); 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..2b5f9d2 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; } From c1ab50f8d95faef9bd0d26e6e2028ac08062fc05 Mon Sep 17 00:00:00 2001 From: Team 1533 Driver Station <32420432+team1533@users.noreply.github.com> Date: Sat, 20 Mar 2021 16:13:32 -0400 Subject: [PATCH 03/11] change path on galacticA --- .../src/main/java/frc/robot/RobotContainer.java | 2 +- .../main/java/frc/robot/commands/Auto/GalacticPathA.java | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java index 4923925..8859ea5 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java @@ -203,7 +203,7 @@ public static String getCoords() { */ public Command getAutonomousCommand(Trajectory trajectory) { - GalacticA galA = new GalacticA(swerveDrive, theta); + GalacticPathA galA = new GalacticPathA(swerveDrive, intake, theta); return galA; 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 index 12fd9c5..d485e33 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/GalacticPathA.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/GalacticPathA.java @@ -37,11 +37,11 @@ public GalacticPathA(SwerveDrive swerveDrive, Intake intake, ProfiledPIDControll this.intake = intake; // Create config for trajectory // top speed: 2.1 - TrajectoryConfig config = new TrajectoryConfig(1, AutoConstants.kMaxAccelerationMetersPerSecondSquared) + TrajectoryConfig config = new TrajectoryConfig(.5, AutoConstants.kMaxAccelerationMetersPerSecondSquared) // Add kinematics to ensure max speed is actually obeyed // .setKinematics(SwerveDriveConstants.kDriveKinematics) // set end: 1.5 - .setEndVelocity(1); + .setEndVelocity(.5); Trajectory traject = TrajectoryGenerator.generateTrajectory( @@ -50,7 +50,7 @@ public GalacticPathA(SwerveDrive swerveDrive, Intake intake, ProfiledPIDControll ), //direction robot moves - new Pose2d(5, 0, new Rotation2d(-Math.PI / 2)), config); + new Pose2d(1.01, 0, new Rotation2d(-Math.PI / 2)), config); SwerveControllerCommand swerveControllerCommand1 = new SwerveControllerCommand(traject, (0), swerveDrive::getPose, // Functional From d0ecf6c893343c8594997f98ac93c9d8398c80cb Mon Sep 17 00:00:00 2001 From: Team 1533 Driver Station <32420432+team1533@users.noreply.github.com> Date: Sat, 27 Mar 2021 17:22:11 -0400 Subject: [PATCH 04/11] teleop, buttons, hood, joysticks --- .../src/main/java/frc/robot/Robot.java | 2 + .../main/java/frc/robot/RobotContainer.java | 94 +++++++++--------- .../commands/Auto/AccuracyChallenge.java | 97 +++++++++++++++++++ .../frc/robot/commands/ControlConveyor.java | 4 +- .../java/frc/robot/commands/MoveHood.java | 1 + .../java/frc/robot/commands/SpinTurret.java | 8 +- .../frc/robot/subsystems/SwerveDrive.java | 2 +- .../frc/robot/subsystems/SwerveModule.java | 2 +- .../java/frc/robot/subsystems/Turret.java | 4 +- 9 files changed, 155 insertions(+), 59 deletions(-) create mode 100644 RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/AccuracyChallenge.java diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java b/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java index 7c87f74..aae2159 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java @@ -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 8859ea5..cd05d5c 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java @@ -96,7 +96,7 @@ 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)); + turret.setDefaultCommand(new SpinTurret(turret, vision,1, 0)); // vision.setDefaultCommand(new RunCommand(vision::runVision, vision)); @@ -117,16 +117,41 @@ 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)); + 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); @@ -139,56 +164,25 @@ private void configureButtonBindings() { BouncePath Bounce = new BouncePath(swerveDrive, theta); SmartDashboard.putData(Scheduler.getInstance()); SmartDashboard.putData("Bounce Path", Bounce); - + SlalomPath1 Slolam = new SlalomPath1(swerveDrive, theta); - SmartDashboard.putData(Scheduler.getInstance()); SmartDashboard.putData("Slalom Path", Slolam); - GalacticPathA galA = new GalacticPathA(swerveDrive, intake, theta); + AccuracyChallenge accuracyChallenge1 = new AccuracyChallenge(swerveDrive, intake, theta); SmartDashboard.putData(Scheduler.getInstance()); - SmartDashboard.putData("GalacticA", galA); + SmartDashboard.putData("Accuracy Challenge", accuracyChallenge1); - // 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)); + + gyro.whenPressed(new InstantCommand(swerveDrive::zeroHeading)); } 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..b2038a4 --- /dev/null +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/AccuracyChallenge.java @@ -0,0 +1,97 @@ +// 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); + +// TrajectoryConfig config1 = new TrajectoryConfig(1, +// AutoConstants.kMaxAccelerationMetersPerSecondSquared) +// // Add kinematics to ensure max speed is actually obeyed +// // .setKinematics(SwerveDriveConstants.kDriveKinematics) +// .setStartVelocity(1.5); + + + + Trajectory traject = TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(-Math.PI / 2)), List.of(), + + //direction robot moves + new Pose2d(1, 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 + + ); +// SwerveControllerCommand swerveControllerCommand2 = new SwerveControllerCommand(traject1, +// (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/ControlConveyor.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/ControlConveyor.java index de17209..8070393 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. } 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/SpinTurret.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/SpinTurret.java index 7f274e1..62e2ba7 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);}); + 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/SwerveDrive.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveDrive.java index 2b5f9d2..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,7 +178,7 @@ public void resetEncoders() { */ public void zeroHeading() { navX.reset(); - resetEncoders(); + // 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..5278264 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java @@ -104,7 +104,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,7 +115,7 @@ public void spin(boolean manual, double speed) { targetPosition = 360 - robotHeading; } - if (manual) + if (mode == 1) turretMotor.set(speed); else // turretMotor.set(0); From 7613ac27f108ca4abf313564d2b290f935f51730 Mon Sep 17 00:00:00 2001 From: Team 1533 Driver Station <32420432+team1533@users.noreply.github.com> Date: Tue, 30 Mar 2021 21:01:40 -0400 Subject: [PATCH 05/11] conveyor is slower, button changes, auto changes --- .../src/main/java/frc/robot/Robot.java | 4 +-- .../main/java/frc/robot/RobotContainer.java | 10 +++++-- .../commands/Auto/AccuracyChallenge.java | 28 ++----------------- .../robot/commands/Auto/GalacticPathA.java | 2 +- .../frc/robot/commands/ControlConveyor.java | 2 +- .../java/frc/robot/subsystems/Intake.java | 2 +- 6 files changed, 16 insertions(+), 32 deletions(-) diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java b/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java index aae2159..ea7608f 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java @@ -78,8 +78,8 @@ public void disabledPeriodic() { @Override public void autonomousInit() { - RobotContainer.swerveDrive.resetOdometry(new Pose2d(0, 0, new Rotation2d(0))); //0.053, .8539 - RobotContainer.theta.reset(0); + RobotContainer.swerveDrive.resetOdometry(new Pose2d(0, 0, new Rotation2d(-Math.PI / 2))); //0.053, .8539 + RobotContainer.theta.reset(-Math.PI / 2); try { diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java index cd05d5c..de34990 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java @@ -96,7 +96,7 @@ 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, vision,1, 0)); + turret.setDefaultCommand(new SpinTurret(turret, vision, 1, 0)); // vision.setDefaultCommand(new RunCommand(vision::runVision, vision)); @@ -137,9 +137,11 @@ private void configureButtonBindings() { JoystickButton gyro = new JoystickButton(m_driverController, 8); butA.whenPressed(new ExtendIntake(intake, m_operatorController)); - butA.whenReleased(new RetractIntake(intake)); + // 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 butY.whileHeld(new RunShooter(shooter)); butY.whenReleased(new StopShooter(shooter)); rBump.whileHeld(new ControlConveyor(conveyor, 1)); @@ -173,6 +175,10 @@ private void configureButtonBindings() { SmartDashboard.putData(Scheduler.getInstance()); SmartDashboard.putData("Accuracy Challenge", accuracyChallenge1); + GalacticPathA galA = new GalacticPathA(swerveDrive, intake, theta); + SmartDashboard.putData(Scheduler.getInstance()); + SmartDashboard.putData("Galactic Path", galA); + // driver X button - slow butXd.whileHeld(new DefaultDrive(swerveDrive, m_driverController, 0.35)); 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 index b2038a4..9aaa112 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/AccuracyChallenge.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/AccuracyChallenge.java @@ -43,19 +43,11 @@ public AccuracyChallenge(SwerveDrive swerveDrive, Intake intake, ProfiledPIDCont // .setKinematics(SwerveDriveConstants.kDriveKinematics) .setEndVelocity(0); -// TrajectoryConfig config1 = new TrajectoryConfig(1, -// AutoConstants.kMaxAccelerationMetersPerSecondSquared) -// // Add kinematics to ensure max speed is actually obeyed -// // .setKinematics(SwerveDriveConstants.kDriveKinematics) -// .setStartVelocity(1.5); - - - Trajectory traject = TrajectoryGenerator.generateTrajectory( new Pose2d(0, 0, new Rotation2d(-Math.PI / 2)), List.of(), //direction robot moves - new Pose2d(1, 0, new Rotation2d(-Math.PI / 2)), config); + new Pose2d(-2, 0, new Rotation2d(-Math.PI / 2)), config); SwerveControllerCommand swerveControllerCommand1 = new SwerveControllerCommand(traject, (0), swerveDrive::getPose, // Functional interface to feed supplier @@ -70,27 +62,13 @@ public AccuracyChallenge(SwerveDrive swerveDrive, Intake intake, ProfiledPIDCont swerveDrive ); -// SwerveControllerCommand swerveControllerCommand2 = new SwerveControllerCommand(traject1, -// (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(wait); + // addSequential(intakeCommand); // addSequential(swerveControllerCommand2); } 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 index d485e33..c81b051 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/GalacticPathA.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/GalacticPathA.java @@ -45,7 +45,7 @@ public GalacticPathA(SwerveDrive swerveDrive, Intake intake, ProfiledPIDControll Trajectory traject = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(-Math.PI/2)), List.of( + new Pose2d(0, 0, new Rotation2d(0)), List.of( ), 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 8070393..d24cc7e 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/ControlConveyor.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/ControlConveyor.java @@ -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/subsystems/Intake.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Intake.java index 221a007..02dd688 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Intake.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Intake.java @@ -33,7 +33,7 @@ public void extend(Joystick joystick) { double speedOut = joystick.getRawAxis(2); if (speedIn > 0.1) - intakeMotor.set(speedIn / 2); + intakeMotor.set(speedIn / 1.5); else if (speedOut > 0.1) intakeMotor.set(-speedOut); else From 221c2e76800d9523c9ef21dcb910c8c998d57251 Mon Sep 17 00:00:00 2001 From: Lori Brown <34607662+Lori2018@users.noreply.github.com> Date: Thu, 1 Apr 2021 19:44:54 -0400 Subject: [PATCH 06/11] Max did nothing --- .../main/java/frc/robot/RobotContainer.java | 4 +-- .../java/frc/robot/subsystems/Turret.java | 31 ++++++++++--------- 2 files changed, 18 insertions(+), 17 deletions(-) diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java index de34990..4c21184 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java @@ -67,7 +67,7 @@ public class RobotContainer { private final PhotonCamera camera1 = new PhotonCamera("OtherCamera"); private final Vision vision = new Vision(camera); private final Vision vision1 = new Vision(camera1); - private final Turret turret = new Turret(swerveDrive); + 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); @@ -96,7 +96,7 @@ 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, vision, 1, 0)); + turret.setDefaultCommand(new SpinTurret(turret, vision, 2, 0)); // vision.setDefaultCommand(new RunCommand(vision::runVision, vision)); 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 5278264..7fff3c6 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()); } @@ -115,11 +112,15 @@ public void spin(int mode, double speed) { targetPosition = 360 - robotHeading; } - if (mode == 1) + if (mode == 1) { turretMotor.set(speed); - else + } else { + vision.updateTargets(); + double pitch = vision.getPitch(); + setPosition(pitch); + } // turretMotor.set(0); - setPosition(targetPosition); + // setPosition(targetPosition); // m_turretPIDController.setReference(targetPosition, ControlType.kPosition); } From cf1aecfd0a57fba6854c83bed833f08fb7b39c27 Mon Sep 17 00:00:00 2001 From: Lori Brown <34607662+Lori2018@users.noreply.github.com> Date: Thu, 1 Apr 2021 21:04:44 -0400 Subject: [PATCH 07/11] update --- .../src/main/java/frc/robot/RobotContainer.java | 2 +- .../src/main/java/frc/robot/commands/SpinTurret.java | 2 +- .../src/main/java/frc/robot/subsystems/Shooter.java | 3 ++- .../src/main/java/frc/robot/subsystems/Vision.java | 1 + 4 files changed, 5 insertions(+), 3 deletions(-) diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java index 4c21184..4e8528d 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java @@ -96,7 +96,7 @@ 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, vision, 2, 0)); + turret.setDefaultCommand(new SpinTurret(turret, vision, 1, 0)); // vision.setDefaultCommand(new RunCommand(vision::runVision, vision)); 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 62e2ba7..3fddb15 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/SpinTurret.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/SpinTurret.java @@ -17,7 +17,7 @@ public class SpinTurret extends InstantCommand { public SpinTurret(Turret subsystem, Vision subsystem2, int mode, double speed) { super(subsystem, () -> {subsystem.spin(mode, speed);}); - requires(subsystem); + requires(subsystem); requires(subsystem2); // Use addRequirements() here to declare subsystem dependencies. } 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 b869d97..af61d8d 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Shooter.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Shooter.java @@ -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/Vision.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Vision.java index 09df2ed..8e2c01f 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Vision.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Vision.java @@ -82,6 +82,7 @@ public double getYaw() { public double getPitch() { if (_target != null) { double pitch = _target.getPitch(); + SmartDashboard.putNumber("Pitch", pitch); return pitch; } return 0.0; From 28971bd6f2de75a5c4fc89b21a7d3cb160fbf38b Mon Sep 17 00:00:00 2001 From: Lori Brown <34607662+Lori2018@users.noreply.github.com> Date: Fri, 2 Apr 2021 19:01:38 -0400 Subject: [PATCH 08/11] tried to fix vision - not sure if it works --- RobotCode2021-Imported/.vscode/launch.json | 12 ++++++--- .../main/java/frc/robot/RobotContainer.java | 12 ++------- .../java/frc/robot/commands/RunVision.java | 25 +++++++++++++++++++ .../java/frc/robot/subsystems/Turret.java | 9 ++++--- .../java/frc/robot/subsystems/Vision.java | 5 ++++ 5 files changed, 47 insertions(+), 16 deletions(-) create mode 100644 RobotCode2021-Imported/src/main/java/frc/robot/commands/RunVision.java 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/RobotContainer.java b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java index 4e8528d..e0785b2 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java @@ -96,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, vision, 1, 0)); - - // vision.setDefaultCommand(new RunCommand(vision::runVision, vision)); + turret.setDefaultCommand(new SpinTurret(turret, vision, 2, 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)); } /** 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..dd1c738 --- /dev/null +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/RunVision.java @@ -0,0 +1,25 @@ +// 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 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(); + } +} 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 7fff3c6..5435f3f 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java @@ -115,9 +115,12 @@ public void spin(int mode, double speed) { if (mode == 1) { turretMotor.set(speed); } else { - vision.updateTargets(); - double pitch = vision.getPitch(); - setPosition(pitch); + if (vision.getHasTargets()) { + double pitch = vision.getPitch(); + SmartDashboard.putNumber("PITCH", pitch); + turretMotor.set(vision.getRotationSpeed()); + } + } // turretMotor.set(0); // setPosition(targetPosition); 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 8e2c01f..3d2bbd6 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Vision.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Vision.java @@ -111,6 +111,11 @@ public Transform2d getPose() { } return new Transform2d(); } + + public boolean getHasTargets() { + return _hasTargets; + } + /* ****************** */ /* TO BE USED FOR COMMANDS */ From 3a84b5262070742951fecfe16dc4a087ee92a041 Mon Sep 17 00:00:00 2001 From: Team 1533 Driver Station <32420432+team1533@users.noreply.github.com> Date: Sat, 3 Apr 2021 17:03:51 -0400 Subject: [PATCH 09/11] auto updates, vision, teleop changes --- .../src/main/java/frc/robot/Robot.java | 6 +- .../main/java/frc/robot/RobotContainer.java | 10 +-- .../frc/robot/commands/Auto/BarrelPath.java | 6 +- .../commands/Auto/PowerPortChallenge.java | 75 +++++++++++++++++++ .../frc/robot/commands/Auto/SlalomPath1.java | 16 ++-- .../java/frc/robot/commands/RunVision.java | 2 + .../java/frc/robot/subsystems/Vision.java | 5 +- 7 files changed, 99 insertions(+), 21 deletions(-) create mode 100644 RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/PowerPortChallenge.java diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java b/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java index ea7608f..16918c4 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java @@ -78,8 +78,8 @@ public void disabledPeriodic() { @Override public void autonomousInit() { - RobotContainer.swerveDrive.resetOdometry(new Pose2d(0, 0, new Rotation2d(-Math.PI / 2))); //0.053, .8539 - RobotContainer.theta.reset(-Math.PI / 2); + RobotContainer.swerveDrive.resetOdometry(new Pose2d(0, 0, new Rotation2d(0))); //0.053, .8539 + RobotContainer.theta.reset(0); try { @@ -111,7 +111,7 @@ public void autonomousInit() { // DriverStation.reportError("Unable to open trajectory" + trajectoryJSON, ex.getStackTrace()); // } - +//enable for autos not on smart dashboard (barrel, bounce, slalom) // m_autonomousCommand = m_robotContainer.getAutonomousCommand(null); // schedule the autonomous command (example) diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java index e0785b2..dfdc572 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java @@ -96,7 +96,7 @@ 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, vision, 2, 0)); + turret.setDefaultCommand(new SpinTurret(turret, vision, 1, 0)); vision.setDefaultCommand(new RunVision(vision)); } @@ -159,9 +159,9 @@ private void configureButtonBindings() { SmartDashboard.putData(Scheduler.getInstance()); SmartDashboard.putData("Bounce Path", Bounce); - SlalomPath1 Slolam = new SlalomPath1(swerveDrive, theta); + SlalomPath1 Slalom = new SlalomPath1(swerveDrive, theta); SmartDashboard.putData(Scheduler.getInstance()); - SmartDashboard.putData("Slalom Path", Slolam); + SmartDashboard.putData("Slalom Path", Slalom); AccuracyChallenge accuracyChallenge1 = new AccuracyChallenge(swerveDrive, intake, theta); SmartDashboard.putData(Scheduler.getInstance()); @@ -195,9 +195,9 @@ public static String getCoords() { */ public Command getAutonomousCommand(Trajectory trajectory) { - GalacticPathA galA = new GalacticPathA(swerveDrive, intake, theta); + BarrelPath bar = new BarrelPath(swerveDrive, theta); - return galA; + return bar; } 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/PowerPortChallenge.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/PowerPortChallenge.java new file mode 100644 index 0000000..309031d --- /dev/null +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/PowerPortChallenge.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 PowerPortChallenge extends CommandGroup { + /** Add your docs here. */ + private final SwerveDrive swerveDrive; + private final Intake intake; + + public PowerPortChallenge(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/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/RunVision.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/RunVision.java index dd1c738..0c93242 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/RunVision.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/RunVision.java @@ -5,6 +5,7 @@ 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. */ @@ -21,5 +22,6 @@ public RunVision(Vision vision) { @Override protected void initialize() { vision.updateTargets(); + SmartDashboard.putNumber("PITCH", vision.getPitch()); } } 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 3d2bbd6..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. @@ -82,10 +84,9 @@ public double getYaw() { public double getPitch() { if (_target != null) { double pitch = _target.getPitch(); - SmartDashboard.putNumber("Pitch", pitch); return pitch; } - return 0.0; + return 11.0; } public double getArea() { From 67cdab8b22c601faa69a6df7397fe3ed0edd279e Mon Sep 17 00:00:00 2001 From: Team 1533 Driver Station <32420432+team1533@users.noreply.github.com> Date: Sun, 4 Apr 2021 16:45:39 -0400 Subject: [PATCH 10/11] power port challenge --- .../src/main/java/frc/robot/RobotContainer.java | 10 +++++----- .../frc/robot/commands/Auto/PowerPortChallenge.java | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java index dfdc572..78ad8d5 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java @@ -136,8 +136,8 @@ private void configureButtonBindings() { //shooter and conveyor move together butY.whileHeld(new RunShooter(shooter)); butY.whenReleased(new StopShooter(shooter)); - rBump.whileHeld(new ControlConveyor(conveyor, 1)); - rBump.whenReleased(new ControlConveyor(conveyor, 0)); + butY.whileHeld(new ControlConveyor(conveyor, 1)); + butY.whenReleased(new ControlConveyor(conveyor, 0)); lBump.whileHeld(new ControlConveyor(conveyor, -1)); lBump.whenReleased(new ControlConveyor(conveyor, 0)); @@ -179,7 +179,7 @@ private void configureButtonBindings() { // 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)); } @@ -195,9 +195,9 @@ public static String getCoords() { */ public Command getAutonomousCommand(Trajectory trajectory) { - BarrelPath bar = new BarrelPath(swerveDrive, theta); + PowerPortChallenge ppc = new PowerPortChallenge(swerveDrive, intake, theta); - return bar; + return ppc; } 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 index 309031d..7f5692a 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/PowerPortChallenge.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/PowerPortChallenge.java @@ -50,7 +50,7 @@ public PowerPortChallenge(SwerveDrive swerveDrive, Intake intake, ProfiledPIDCon ), //direction robot moves - new Pose2d(1.01, 0, new Rotation2d(-Math.PI / 2)), config); + new Pose2d(3.112242, 0, new Rotation2d(0)), config); SwerveControllerCommand swerveControllerCommand1 = new SwerveControllerCommand(traject, (0), swerveDrive::getPose, // Functional @@ -68,7 +68,7 @@ public PowerPortChallenge(SwerveDrive swerveDrive, Intake intake, ProfiledPIDCon ); - // addParallel(swerveControllerCommand1); + addSequential(swerveControllerCommand1); IntakeCommand intakeCommand = new IntakeCommand(intake); addSequential(intakeCommand); } From 6cab4f55096cbfef6bfcb9de0e73745caa152c87 Mon Sep 17 00:00:00 2001 From: Team 1533 Driver Station <32420432+team1533@users.noreply.github.com> Date: Mon, 5 Apr 2021 21:04:13 -0400 Subject: [PATCH 11/11] jj --- .../src/main/java/frc/robot/Robot.java | 2 +- .../main/java/frc/robot/RobotContainer.java | 20 ++-- .../commands/Auto/PowerPortChallenge.java | 97 +++++++++++++------ .../java/frc/robot/commands/DefaultDrive.java | 8 +- 4 files changed, 82 insertions(+), 45 deletions(-) diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java b/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java index 16918c4..7fd9617 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/Robot.java @@ -112,7 +112,7 @@ public void autonomousInit() { // } //enable for autos not on smart dashboard (barrel, bounce, slalom) - // m_autonomousCommand = m_robotContainer.getAutonomousCommand(null); + m_autonomousCommand = m_robotContainer.getAutonomousCommand(null); // schedule the autonomous command (example) if (m_autonomousCommand != null) { diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java index 78ad8d5..7bb74bb 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java @@ -130,14 +130,14 @@ private void configureButtonBindings() { 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)); + // butB.whileHeld(new SpinTurret(turret, vision, 1, 0.25)); + // butX.whileHeld(new SpinTurret(turret, vision, 1, -0.25)); - //shooter and conveyor move together + //shooter and conveyor move together *** change for PowerPortChallenges butY.whileHeld(new RunShooter(shooter)); butY.whenReleased(new StopShooter(shooter)); - butY.whileHeld(new ControlConveyor(conveyor, 1)); - butY.whenReleased(new ControlConveyor(conveyor, 0)); + rBump.whileHeld(new ControlConveyor(conveyor, 1)); + rBump.whenReleased(new ControlConveyor(conveyor, 0)); lBump.whileHeld(new ControlConveyor(conveyor, -1)); lBump.whenReleased(new ControlConveyor(conveyor, 0)); @@ -163,9 +163,13 @@ private void configureButtonBindings() { SmartDashboard.putData(Scheduler.getInstance()); SmartDashboard.putData("Slalom Path", Slalom); - AccuracyChallenge accuracyChallenge1 = new AccuracyChallenge(swerveDrive, intake, theta); + // AccuracyChallenge accuracyChallenge1 = new AccuracyChallenge(swerveDrive, intake, shooter, theta); + // SmartDashboard.putData(Scheduler.getInstance()); + // SmartDashboard.putData("Accuracy Challenge", accuracyChallenge1); + + PowerPortChallenge ppcc = new PowerPortChallenge(swerveDrive, intake, shooter, theta); SmartDashboard.putData(Scheduler.getInstance()); - SmartDashboard.putData("Accuracy Challenge", accuracyChallenge1); + SmartDashboard.putData("PowerPortChallenge", ppcc); GalacticPathA galA = new GalacticPathA(swerveDrive, intake, theta); SmartDashboard.putData(Scheduler.getInstance()); @@ -195,7 +199,7 @@ public static String getCoords() { */ public Command getAutonomousCommand(Trajectory trajectory) { - PowerPortChallenge ppc = new PowerPortChallenge(swerveDrive, intake, theta); + PowerPortChallenge ppc = new PowerPortChallenge(swerveDrive, intake, shooter, theta); return ppc; 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 index 7f5692a..4832891 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/PowerPortChallenge.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/PowerPortChallenge.java @@ -4,8 +4,8 @@ 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.WaitCommand; import java.util.List; @@ -18,58 +18,91 @@ 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 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, ProfiledPIDController theta) { - // private final Intake m_intake; + public PowerPortChallenge(SwerveDrive swerveDrive, Intake intake, Shooter shoot, ProfiledPIDController theta) { requires(swerveDrive); + requires(shoot); 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); + 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(0)), List.of( - + 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(3.112242, 0, new Rotation2d(0)), config); + //direction robot moves +new Pose2d(-2, 0, new Rotation2d(-Math.PI / 2)), config); - SwerveControllerCommand swerveControllerCommand1 = new SwerveControllerCommand(traject, (0), swerveDrive::getPose, - // Functional - // feed - // supplier - SwerveDriveConstants.kDriveKinematics, +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, +// Position controllers +new PIDController(AutoConstants.kPXController, 1, AutoConstants.kDXController), +new PIDController(AutoConstants.kPYController, 1, AutoConstants.kDYController), theta, - swerveDrive::setModuleStates, +swerveDrive::setModuleStates, - swerveDrive +swerveDrive - ); +); +WaitCommand wait = new WaitCommand(15); +IntakeCommand intakeCommand = new IntakeCommand(intake); +RunShooter runShoot = new RunShooter(shoot); - addSequential(swerveControllerCommand1); - IntakeCommand intakeCommand = new IntakeCommand(intake); - addSequential(intakeCommand); + addParallel(intakeCommand); + addSequential(swerveControllerCommand1); + // addSequential(wait); + // addSequential(swerveControllerCommand2); + // addSequential(wait); + // addSequential(swerveControllerCommand1); + // addSequential(wait); + // addSequential(swerveControllerCommand2); + // addSequential(wait); } -} \ No newline at end of file +} 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);