diff --git a/lib199 b/lib199 index a2a01f6..a7dea30 160000 --- a/lib199 +++ b/lib199 @@ -1 +1 @@ -Subproject commit a2a01f6e6c1338f86e01ee6e86b8a13da554ceaa +Subproject commit a7dea3012bb9c120a16ef09e0eeb03f7a8b278e8 diff --git a/src/main/deploy/PathWeaver/Paths/CBall1.path b/src/main/deploy/PathWeaver/Paths/CBall1.path new file mode 100644 index 0000000..055cc34 --- /dev/null +++ b/src/main/deploy/PathWeaver/Paths/CBall1.path @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name +3.39,-2.4,1.0,0.0,true,false, +5.0,-0.7,1.0,0.0,true,false, diff --git a/src/main/deploy/PathWeaver/Paths/LBall1.path b/src/main/deploy/PathWeaver/Paths/LBall1.path new file mode 100644 index 0000000..913f451 --- /dev/null +++ b/src/main/deploy/PathWeaver/Paths/LBall1.path @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name +3.39,-3.4,1.0,0.0,true,false, +5.0,-0.7,1.0,0.0,true,false, diff --git a/src/main/deploy/PathWeaver/Paths/RBall1.path b/src/main/deploy/PathWeaver/Paths/RBall1.path new file mode 100644 index 0000000..7b11d7e --- /dev/null +++ b/src/main/deploy/PathWeaver/Paths/RBall1.path @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name +3.39,-1.4,1.0,0.0,true,false, +5.0,-0.7,1.0,0.0,true,false, diff --git a/src/main/java/org/team199/robot2021/Constants.java b/src/main/java/org/team199/robot2021/Constants.java index 2d53310..2ef8b3c 100644 --- a/src/main/java/org/team199/robot2021/Constants.java +++ b/src/main/java/org/team199/robot2021/Constants.java @@ -106,8 +106,13 @@ public static final class DriveConstants { // TODO: Find experimental value for current draw public static final double intakeCurrentDraw = 8.0; + // TODO: change these to current for 2021 robot public static final double cameraHeight = Units.inchesToMeters(40.0); public static final double cameraMountingAngleDeg = 29.5; + + // TODO: Find distances between intake and limelight + public static final double xLimeDistIntake = 0.0; + public static final double zLimeDistIntake = 0.0; } public static final class DrivePorts { @@ -153,6 +158,7 @@ public static final class DrivePorts { public static final int kAutoPathSwitch1Port = 0; public static final int kAutoPathSwitch2Port = 1; + public static final int kLimelightServoPort = 0; //TODO: set proper channel } /** @@ -235,6 +241,8 @@ public static final class Controller { // intake/feeder public static final int kIntakeButton = X; public static final int kOuttakeButton = Y; + // limelight toggle + public static final int kLimelightToggleButton = LT; //Limelight Toggle button set to Left Button //public static final int kRegurgitateButton = B; } } diff --git a/src/main/java/org/team199/robot2021/RobotContainer.java b/src/main/java/org/team199/robot2021/RobotContainer.java index fb43399..4d51c26 100644 --- a/src/main/java/org/team199/robot2021/RobotContainer.java +++ b/src/main/java/org/team199/robot2021/RobotContainer.java @@ -28,11 +28,14 @@ import org.team199.lib.RobotPath; import org.team199.robot2021.Constants.OI; +import org.team199.robot2021.commands.BallSearchAndPickup; import org.team199.robot2021.commands.GalacticSearchCommand; import org.team199.robot2021.commands.TeleopDrive; import org.team199.robot2021.commands.ToggleIntake; import org.team199.robot2021.subsystems.Drivetrain; +import org.team199.robot2021.subsystems.Feeder; import org.team199.robot2021.subsystems.Intake; +import org.team199.robot2021.subsystems.Shooter; import frc.robot.lib.Limelight; @@ -46,9 +49,9 @@ public class RobotContainer { final Drivetrain drivetrain = new Drivetrain(); private final Limelight lime = new Limelight(); - //private final Shooter shooter = new Shooter(lime); + private final Shooter shooter = new Shooter(lime); private final Intake intake = new Intake(); - //private final Feeder feeder = new Feeder(); + private final Feeder feeder = new Feeder(); private final Joystick leftJoy = new Joystick(Constants.OI.LeftJoy.port); private final Joystick rightJoy = new Joystick(Constants.OI.RightJoy.port); private final Joystick controller = new Joystick(Constants.OI.Controller.port); @@ -192,8 +195,12 @@ private void configureButtonBindingsController() { new JoystickButton(controller, Constants.OI.Controller.A).whenPressed(new InstantCommand(() -> {drivetrain.resetHeading();})); new JoystickButton(controller, Constants.OI.Controller.B).whenPressed(new InstantCommand(() -> { SmartDashboard.putBoolean("Field Oriented", !SmartDashboard.getBoolean("Field Oriented", true)); })); // Intake toggle button - new JoystickButton(controller, Constants.OI.Controller.kIntakeButton).whenPressed(new ToggleIntake(intake)); - + // new JoystickButton(controller, Constants.OI.Controller.kIntakeButton).whenPressed(new ToggleIntake(intake)); + new JoystickButton(controller, Constants.OI.Controller.kIntakeButton).whenPressed(new BallSearchAndPickup(drivetrain, intake, feeder, shooter, lime)); + new JoystickButton(controller, Constants.OI.Controller.kLimelightToggleButton).whenPressed(new InstantCommand(() -> { + intake.setLimelightSearching(!intake.isLimelightSearching()); + + })); // Power cell regurgitate button //new JoystickButton(controller, Constants.OI.Controller.kRegurgitateButton).whileHeld(new Regurgitate(intake, feeder)); diff --git a/src/main/java/org/team199/robot2021/commands/AutoShootAndDrive.java b/src/main/java/org/team199/robot2021/commands/AutoShootAndDrive.java index 3223427..a2f51ce 100644 --- a/src/main/java/org/team199/robot2021/commands/AutoShootAndDrive.java +++ b/src/main/java/org/team199/robot2021/commands/AutoShootAndDrive.java @@ -23,8 +23,9 @@ public AutoShootAndDrive(Drivetrain drivetrain, Intake intake, Feeder feeder, Sh TimeOfFlight shooterDistanceSensor = feeder.getShooterDistanceSensor(); ShooterHorizontalAim aim = new ShooterHorizontalAim(drivetrain, lime); AutoShoot shoot = new AutoShoot(feeder, shooter, shooterDistanceSensor, 3); + BallSearchAndPickup search = new BallSearchAndPickup(drivetrain, intake, feeder, shooter, lime); - /* + addCommands( aim, new InstantCommand(() -> { @@ -36,7 +37,7 @@ public AutoShootAndDrive(Drivetrain drivetrain, Intake intake, Feeder feeder, Sh intake.intake(); intake.doTheFlop(); }, intake), - path.getPathCommand(), + path.getPathCommand(true, true), new InstantCommand(() -> { intake.retract(); intake.stop(); @@ -45,8 +46,9 @@ public AutoShootAndDrive(Drivetrain drivetrain, Intake intake, Feeder feeder, Sh new InstantCommand(() -> { SmartDashboard.putNumber("Shooter.kTargetSpeed", linearInterpol.calculate(drivetrain.getOdometry().getPoseMeters().getTranslation().getDistance(target))); }), - shoot + shoot, + search ); - */ + } } \ No newline at end of file diff --git a/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java b/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java new file mode 100644 index 0000000..72540b7 --- /dev/null +++ b/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java @@ -0,0 +1,67 @@ +// 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 org.team199.robot2021.commands; + +import frc.robot.lib.Limelight; +import org.team199.robot2021.subsystems.Drivetrain; +import org.team199.robot2021.subsystems.Feeder; +import org.team199.robot2021.subsystems.Intake; +import org.team199.robot2021.subsystems.Shooter; +import org.team199.robot2021.Constants; +import org.team199.robot2021.commands.RotateAngle; +import org.team199.robot2021.commands.DriveDistance; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class BallSearchAndPickup extends SequentialCommandGroup { + private Drivetrain drivetrain; + private Intake intake; + private Feeder feeder; + private Shooter shooter; + private Limelight lime; + + /** Creates a new BallSearchAndPickup. */ + public BallSearchAndPickup(Drivetrain drivetrain, Intake intake, Feeder feeder, Shooter shooter, + Limelight lime) { + this.lime = lime; + this.drivetrain = drivetrain; + addRequirements(drivetrain, intake, feeder, shooter); + RotateAngle check = new RotateAngle(drivetrain, Math.PI/5); + + for (int i = 0; i < 10; i++) { + addCommands(check); + if (SmartDashboard.getNumber("Found Vision Target", 0) != 0) { + break; + } + } + + if (SmartDashboard.getNumber("Found Vision Target", 0) != 0) { + // Use addRequirements() here to declare subsystem dependencies. + //Search for Balls + double[] distComponents = lime.determineObjectDist(Constants.DriveConstants.cameraHeight, 0.0, Constants.DriveConstants.cameraMountingAngleDeg); + double xOffset = Constants.DriveConstants.xLimeDistIntake; + double zOffset = Constants.DriveConstants.zLimeDistIntake; + //(Make sure to subtract the horizontal dist. b/t limelight and center of intake from the left right dist) + distComponents[0] = distComponents[0] - xOffset; + distComponents[1] = distComponents[1] - zOffset; + //Find angle and distance from balls using trigonometry and getObjectDistance method in Limelight class (0 is forward, 1 is strafe) + double angle = Math.atan2(distComponents[0], distComponents[1]); + double distance = Math.hypot(distComponents[0], distComponents[1]); + RotateAngle rotate = new RotateAngle(drivetrain, angle); + DriveDistance drive = new DriveDistance(drivetrain, distance); + //Rotate Robot along angle till it's facing ball + //Create new command to do this where isfinished checks if the goal angle is the same as the current angle + //Run the intak + addCommands(rotate, new InstantCommand(() -> intake.deploy()), new InstantCommand(() -> intake.intake()), drive, new InstantCommand(() -> intake.stop()), new InstantCommand(() -> intake.retract())); + } + } + + // Called when the command is initially scheduled. + +} diff --git a/src/main/java/org/team199/robot2021/commands/DriveDistance.java b/src/main/java/org/team199/robot2021/commands/DriveDistance.java new file mode 100644 index 0000000..24ab004 --- /dev/null +++ b/src/main/java/org/team199/robot2021/commands/DriveDistance.java @@ -0,0 +1,49 @@ +// 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 org.team199.robot2021.commands; + +import org.team199.robot2021.subsystems.Drivetrain; + +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class DriveDistance extends CommandBase { + private Drivetrain drivetrain; + private double distance; + private Translation2d initDist; + private double hasDriven; + /** Creates a new DriveDistance. */ + public DriveDistance(Drivetrain drivetrain, double distance) { + this.drivetrain = drivetrain; + this.distance = distance; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + drivetrain.drive(1, 0, 0); + //Sets initial Distance + initDist = drivetrain.getOdometry().getPoseMeters().getTranslation(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + //Keep Going forward (Set speed of wheels or whatever that is) + drivetrain.drive(1,0,0); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + hasDriven = drivetrain.getOdometry().getPoseMeters().getTranslation().getDistance(initDist); + return (hasDriven >= distance); + } +} diff --git a/src/main/java/org/team199/robot2021/commands/RotateAngle.java b/src/main/java/org/team199/robot2021/commands/RotateAngle.java new file mode 100644 index 0000000..cf72b28 --- /dev/null +++ b/src/main/java/org/team199/robot2021/commands/RotateAngle.java @@ -0,0 +1,58 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.team199.robot2021.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import org.team199.robot2021.subsystems.Drivetrain; + +public class RotateAngle extends CommandBase { + private Drivetrain drivetrain; + private double rotateAngle; + private double currentAngle; + private double goalAngle; + /** Creates a new RotateAngle. */ + public RotateAngle(Drivetrain drivetrain, double rotateAngle) { + // Use addRequirements() here to declare subsystem dependencies. + this.drivetrain = drivetrain; + this.rotateAngle = rotateAngle; + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + //TODO: determine speed angle rotates + //Rotates based on if rotateAngle is positive or negative + //TODO: define goalAngle as the final angle based on odometry values + //TODO: ensure getHeading returns an angle + goalAngle = drivetrain.getHeading() *Math.PI/180 + rotateAngle; + if (Math.abs(rotateAngle) != rotateAngle) { + drivetrain.drive(0, 0, -0.1); + } else { + drivetrain.drive(0, 0, 0.1); + } + //TODO: use getHeading to find the angle we want the drivetrain to rotate. + //TODO: Change goalAngle to turnAngle, with goalAngle being the angle we want the drivetrian to rotate + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + currentAngle = drivetrain.getHeading()*Math.PI/180; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drivetrain.drive(0, 0 , 0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + // TODO: Determine margin of error + return (Math.abs(goalAngle - currentAngle) < 5); + } +} diff --git a/src/main/java/org/team199/robot2021/subsystems/Intake.java b/src/main/java/org/team199/robot2021/subsystems/Intake.java index d4199e3..98ca611 100644 --- a/src/main/java/org/team199/robot2021/subsystems/Intake.java +++ b/src/main/java/org/team199/robot2021/subsystems/Intake.java @@ -11,17 +11,24 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.lib.MotorControllerFactory; +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.networktables.NetworkTableInstance; + public class Intake extends SubsystemBase { // TODO: find good values and then set to final private static double kIntakeSpeed = .6; private static double kSlowSpeed = -0.1; + private static double kLimeBottomAngleDegs = 0; //TODO: set correct angle + private static double kLimeTopAngleDegs = 0; //TODO: set correct angle private final CANSparkMax rollerMotor = MotorControllerFactory.createSparkMax(Constants.DrivePorts.kIntakeRoller); + private final Servo limelightServo = new Servo(Constants.DrivePorts.kLimelightServoPort); private final DoubleSolenoid intakePistons1 = new DoubleSolenoid(Constants.DrivePorts.kIntakePistons[2], Constants.DrivePorts.kIntakePistons[3]); private final DoubleSolenoid intakePistons2 = new DoubleSolenoid(Constants.DrivePorts.kIntakePistons[0], Constants.DrivePorts.kIntakePistons[1]); private boolean deployed = false; + private boolean isLimelightSearching = false; /** * Vectored intake that rolls balls through the bumper gap and into feeder. @@ -77,5 +84,21 @@ public void doTheFlop() { public boolean isDeployed() { return deployed; } + public boolean isLimelightSearching() { + return isLimelightSearching; + } + public void setLimelightSearching(boolean status) { + isLimelightSearching = status; + if (isLimelightSearching) { + limelightServo.setAngle(kLimeBottomAngleDegs); // TODO: set correct angle + NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(1); + } else { + limelightServo.setAngle(kLimeTopAngleDegs); // TODO: set correct angle + NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(0); + + } + + } + } \ No newline at end of file