Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

asdfghkl;

Browse Source
This commit is contained in:
Ryan Manley
2022-04-07 11:35:07 -06:00
parent 6253f97817
commit cce339dc56
5 changed files with 59 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+3 -2
View File
@@ -168,9 +168,10 @@ public final class Constants {
public static final double STORAGE_SPEED = 1.0;//0.9;
}
public static final class LEDConstants {
public static final int LED_SPARK_ID = 0;
public static final int LED_SPARK_ID = 3;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_BPM;
public static final LEDPatterns SHOOTING_PATTERN = LEDPatterns.SOLID_GREEN;
}
public static final class ClimberConstants {
src/main/java/frc4388/robot/RobotContainer.java
+34 -18
View File
@@ -55,6 +55,7 @@ import frc4388.robot.subsystems.Climber;
import frc4388.robot.subsystems.Extender;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Serializer;
import frc4388.robot.subsystems.Storage;
import frc4388.robot.subsystems.SwerveDrive;
@@ -87,7 +88,7 @@ public class RobotContainer {
public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor);
public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor);
// private final LED m_robotLED = new LED(m_robotMap.LEDController); // ! no LED makes aarav sad
private final LED m_robotLED = new LED(m_robotMap.LEDController); // ! no LED makes aarav sad
public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
@@ -352,7 +353,7 @@ public class RobotContainer {
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whenPressed(new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - 10, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0));
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); // * aim with turret to target);
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED)); // * aim with turret to target);
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
@@ -525,21 +526,21 @@ public class RobotContainer {
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs.
SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
new ParallelCommandGroup(
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0, true)
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
SequentialCommandGroup weirdAutoShootingGroup2 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
SequentialCommandGroup weirdAutoShootingGroup2 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
new ParallelCommandGroup(
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.3, true)
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
SequentialCommandGroup weirdAutoShootingGroup3 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
SequentialCommandGroup weirdAutoShootingGroup3 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
new ParallelCommandGroup(
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 4.0, true)
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
@@ -641,9 +642,11 @@ public class RobotContainer {
ParallelDeadlineGroup idleDrumUntilShootingFirstBall = new ParallelDeadlineGroup(extendThenAimTurret, new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom));
// return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Move Forward"));
return new SequentialCommandGroup(idleDrumUntilShootingFirstBall,
//new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true),
// extendWhileTurretIsAiming,//new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target
//! 2 BALL AUTO
return new SequentialCommandGroup(extendWhileTurretIsAiming,//new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target
weirdAutoShootingGroup, // * shoot
// extendWhileTurretIsAiming,
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true),
@@ -652,14 +655,27 @@ public class RobotContainer {
// intakeWithPath,
// weirdAutoShootingGroup2,
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true)); // * aim with turret to target); // * shoot
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true),
// intakeWithPath2,
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 1.0, true),
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 0.7, true),
// intakeWithPath2AndTrackTarget,
intakeWithPath2AndAimTurret,
weirdAutoShootingGroup3,
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true));
//! 3 BALL AUTO
// return new SequentialCommandGroup(idleDrumUntilShootingFirstBall,
// //new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true),
// // extendWhileTurretIsAiming,//new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target
// weirdAutoShootingGroup, // * shoot
// // extendWhileTurretIsAiming,
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true),
// // new ExtenderIntakeGroup(m_robotIntake, m_robotExtender),
// intakeWithPathAndTrackTarget,
// // intakeWithPath,
// // weirdAutoShootingGroup2,
// // new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true)); // * aim with turret to target); // * shoot
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true),
// // intakeWithPath2,
// // new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 1.0, true),
// // new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 0.7, true),
// // intakeWithPath2AndTrackTarget,
// intakeWithPath2AndAimTurret,
// weirdAutoShootingGroup3,
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true));
// return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Diamond"));
}
src/main/java/frc4388/robot/RobotMap.java
+2 -2
View File
@@ -53,9 +53,9 @@ public class RobotMap {
}
/* LED Subsystem */
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
// void configureLEDMotorControllers() {}
void configureLEDMotorControllers() {}
/* Swerve Subsystem */
src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java
+5 -4
View File
@@ -7,6 +7,7 @@ package frc4388.robot.commands.ShooterCommands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.VisionOdometry;
@@ -21,10 +22,10 @@ public class Seek extends SequentialCommandGroup {
* @author Aarav Shah
* @blame Aarav Shah (thomas did this)
*/
public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) {
public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, LED leds) {
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry));
addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry, leds));
}
/** Seeks.
@@ -32,9 +33,9 @@ public class Seek extends SequentialCommandGroup {
* @author Aarav Shah
* @blame Aarav Shah (thomas did this)
*/
public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, double[] fakeOdo) {
public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, LED leds, double[] fakeOdo) {
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, fakeOdo, false, false), new TrackTarget(turret, drum, hood, visionOdometry));
addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, fakeOdo, false, false), new TrackTarget(turret, drum, hood, visionOdometry, leds));
}
}
src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+15 -3
View File
@@ -17,14 +17,17 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.RobotContainer;
import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.Vision;
import frc4388.robot.subsystems.VisionOdometry;
import frc4388.utility.LEDPatterns;
import frc4388.utility.Vector2D;
import frc4388.utility.desmos.DesmosServer;
@@ -36,6 +39,7 @@ public class TrackTarget extends CommandBase {
VisionOdometry m_visionOdometry;
BoomBoom m_boomBoom;
Hood m_hood;
LED m_leds;
boolean isAuto;
@@ -57,11 +61,12 @@ public class TrackTarget extends CommandBase {
long startTime;
private double timeTolerance;
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, boolean isAuto) {
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, LED leds, boolean isAuto) {
m_turret = turret;
m_boomBoom = boomBoom;
m_hood = hood;
m_visionOdometry = visionOdometry;
m_leds = leds;
this.isAuto = isAuto;
this.timeTolerance = 1000;
@@ -71,8 +76,8 @@ public class TrackTarget extends CommandBase {
SmartDashboard.putBoolean("Target Locked", false);
}
public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
this(turret, boomBoom, hood, visionOdometry, false);
public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, LED leds) {
this(turret, boomBoom, hood, visionOdometry, leds, false);
}
// Called when the command is initially scheduled.
@@ -134,6 +139,13 @@ public class TrackTarget extends CommandBase {
SmartDashboard.putNumber("Hood Target Angle Track", hoodPosition);
SmartDashboard.putNumber("Vel Target Track", velocity);
SmartDashboard.putBoolean("Target Locked", targetLocked);
if (targetLocked){
m_leds.setPattern(LEDConstants.SHOOTING_PATTERN);
}
else{
m_leds.setPattern(LEDConstants.DEFAULT_PATTERN);
}
}
public ArrayList<Point> filterPoints(ArrayList<Point> points) {