Skip to content
This repository has been archived by the owner on Jan 16, 2024. It is now read-only.

Commit

Permalink
feat: initial driving code, sans turret :D
Browse files Browse the repository at this point in the history
Co-authored-by: DaviTheDinosaur <[email protected]>
  • Loading branch information
mimizh2418 and DaviTheDinosaur committed Dec 15, 2023
1 parent 2eaa1ef commit 8485533
Show file tree
Hide file tree
Showing 13 changed files with 316 additions and 56 deletions.
65 changes: 39 additions & 26 deletions src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -9,27 +9,34 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.StartEndCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
import org.team1540.bunnybotTank2023.commands.auto.AutoShoot5RamTotes;
import org.team1540.bunnybotTank2023.commands.drivetrain.ArcadeDriveCommand;
import org.team1540.bunnybotTank2023.commands.drivetrain.Drivetrain;
import org.team1540.bunnybotTank2023.commands.indexer.Indexer;
import org.team1540.bunnybotTank2023.commands.indexer.IndexerCommand;
import org.team1540.bunnybotTank2023.commands.indexer.IndexerIdleCommand;
import org.team1540.bunnybotTank2023.commands.intake.Intake;
import org.team1540.bunnybotTank2023.commands.intake.IntakeCommand;
import org.team1540.bunnybotTank2023.commands.shooter.ShootSequenceCommand;
import org.team1540.bunnybotTank2023.commands.shooter.Shooter;
import org.team1540.bunnybotTank2023.commands.turret.Turret;
import org.team1540.bunnybotTank2023.commands.turret.TurretManualCommand;
import org.team1540.bunnybotTank2023.commands.turret.TurretSetpointCommand;
import org.team1540.bunnybotTank2023.commands.turret.TurretZeroSequenceCommand;
import org.team1540.bunnybotTank2023.commands.turret.*;
import org.team1540.bunnybotTank2023.io.drivetrain.DrivetrainIOSim;
import org.team1540.bunnybotTank2023.io.drivetrain.DrivetrainIOReal;
import org.team1540.bunnybotTank2023.io.indexer.IndexerIOReal;
import org.team1540.bunnybotTank2023.io.intake.IntakeIOReal;
import org.team1540.bunnybotTank2023.io.shooter.ShooterIOReal;
import org.team1540.bunnybotTank2023.io.shooter.ShooterIOSim;
import org.team1540.bunnybotTank2023.io.turret.TurretIOReal;
import org.team1540.bunnybotTank2023.io.vision.Limelight;
import org.team1540.bunnybotTank2023.io.vision.LimelightIO;
import org.team1540.bunnybotTank2023.io.vision.LimelightIOReal;

import java.util.ArrayList;

import static org.team1540.bunnybotTank2023.Constants.*;

/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
Expand All @@ -39,11 +46,12 @@
*/
public class RobotContainer {
// Subsystems
Drivetrain drivetrain;
Shooter shooter;
Indexer indexer;
Intake intake;
Turret turret;
final Drivetrain drivetrain;
final Shooter shooter;
final Indexer indexer;
final Intake intake;
final Turret turret;
final Limelight limelight;

// Controllers
CommandXboxController driver = new CommandXboxController(0);
Expand All @@ -59,13 +67,15 @@ public RobotContainer() {
indexer = new Indexer(new IndexerIOReal());
intake = new Intake(new IntakeIOReal());
turret = new Turret(new TurretIOReal());
limelight = new Limelight(new LimelightIOReal());
} else {
// Initialize subsystems with simulation IO
drivetrain = new Drivetrain(new DrivetrainIOSim());
shooter = new Shooter(new ShooterIOSim());
indexer = null;
intake = null;
turret = null;
limelight = null;
}
setDefaultCommands();
configureButtonBindings();
Expand All @@ -74,31 +84,34 @@ public RobotContainer() {

/** Use this method to define your trigger->command mappings. */
private void configureButtonBindings() {
copilot.a()
.whileTrue(new InstantCommand(() -> shooter.setVelocity(turretSetpoint.get()))
.andThen(new IndexerCommand(indexer)))
.onFalse(new InstantCommand(() -> {
shooter.stop();
indexer.stop();
}));
copilot.a().whileTrue(
Commands.parallel(
new IntakeCommand(intake),
new IndexerCommand(indexer)
)
);

// copilot.b().whileTrue(new InstantCommand(() -> intake.setFold(false)).andThen(new IndexerCommand(indexer))).whileFalse(new InstantCommand(() -> intake.setFold(true)));
copilot.b().whileTrue(Commands.parallel(new IntakeCommand(intake), new IndexerCommand(indexer)));
copilot.x().whileTrue(new TurretSetpointCommand(turret, Rotation2d.fromDegrees(turretSetpoint.get())));
copilot.y().whileTrue(new TurretZeroSequenceCommand(turret));
copilot.a().onTrue(new InstantCommand( () -> intake.setFold(false))).onFalse(new InstantCommand(() -> intake.setFold(true)));
copilot.rightTrigger().onTrue(new ShootSequenceCommand(shooter, indexer, 3000));

// copilot.x().onTrue(new InstantCommand(() -> intake.setFold(false))).onFalse(new InstantCommand(() -> intake.setFold(true)));
}

private void setDefaultCommands() {
// drivetrain.setDefaultCommand(new ArcadeDriveCommand(drivetrain, driver));
// shooter.setDefaultCommand(new StartEndCommand(
// () -> shooter.setVelocity(ShooterConstants.SHOOTER_IDLE_RPM),
// () -> {},
// shooter
// ));
drivetrain.setDefaultCommand(new ArcadeDriveCommand(drivetrain, driver));
shooter.setDefaultCommand(new StartEndCommand(
() -> shooter.setVelocity(ShooterConstants.SHOOTER_IDLE_RPM),
() -> {},
shooter
));
turret.setDefaultCommand(new TurretManualCommand(turret, copilot));
// indexer.setDefaultCommand(new IndexerIdleCommand(indexer));
indexer.setDefaultCommand(new IndexerIdleCommand(indexer));
intake.setDefaultCommand(new StartEndCommand(
() -> intake.setMotorSpeed(0.3),
() -> {},
intake
));
}


Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ public IndexerCommand(Indexer indexer) {
@Override
public void initialize() {
indexer.setBottomSpeed(0.50);
indexer.setTopSpeed(1);
indexer.setTopSpeed(-0.2);
}

@Override
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ public IntakeCommand(Intake intake) {
@Override
public void initialize() {
intake.setFold(false);
intake.setMotorSpeed(0.5);
intake.setMotorSpeed(1);
}

@Override
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
package org.team1540.bunnybotTank2023.commands.shooter;

import edu.wpi.first.wpilibj2.command.*;
import org.team1540.bunnybotTank2023.commands.indexer.Indexer;

public class ShootSequenceCommand extends SequentialCommandGroup {
public ShootSequenceCommand(Shooter shooter, Indexer indexer, double shooterVelocityRPM) {
addCommands(
new InstantCommand(() -> {
shooter.setVelocity(shooterVelocityRPM);
indexer.setBottomSpeed(0.5);
}),
new WaitUntilCommand(shooter::isAtSetpoint),
new InstantCommand(() -> indexer.setTopSpeed(1)),
new WaitCommand(0.1),
new InstantCommand(() -> indexer.setTopSpeed(-0.2)),
new WaitCommand(0.5)
);
addRequirements(shooter, indexer);
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,10 @@ public boolean isAtSetpoint() {
return Math.abs(inputs.turretSetPointDegrees - averageFilter.getAverage()) < 0.5;
}

public Rotation2d getPosition() {
return Rotation2d.fromDegrees(inputs.turretCurrentPositionDegrees);
}

public void resetToEncoder(Rotation2d position) {
io.resetEncoder(position);
}
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
package org.team1540.bunnybotTank2023.commands.turret;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import org.team1540.bunnybotTank2023.io.vision.Limelight;

public class TurretTrackTargetCommand extends CommandBase {
private final Turret turret;
private final Limelight limelight;

public TurretTrackTargetCommand(Turret turret, Limelight limelight) {
this.turret = turret;
this.limelight = limelight;
addRequirements(turret);
}

@Override
public void execute() {
Rotation2d turretSetpoint = Rotation2d.fromDegrees(limelight.getTx()).plus(turret.getPosition());
turret.autoSetPosition(turretSetpoint);
}

@Override
public void end(boolean interrupted) {
turret.stop();
}
}
104 changes: 95 additions & 9 deletions src/main/java/org/team1540/bunnybotTank2023/io/vision/Limelight.java
Original file line number Diff line number Diff line change
@@ -1,18 +1,104 @@
package org.team1540.bunnybotTank2023.io.vision;

import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import org.team1540.bunnybotTank2023.utils.vision.LEDState;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.littletonrobotics.junction.Logger;

public class Limelight implements TargetVisionIO {
private final NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
import java.util.ArrayList;

@Override
public void updateInputs(VisionInputs inputs) {
public class Limelight extends SubsystemBase {
private final LimelightIO io;
private final LimelightIOInputsAutoLogged inputs = new LimelightIOInputsAutoLogged();
private double[] target;

private final double mountingAngleDegrees = 10.0;
private final double limelightLensHeightInches = 20.0;//TODO Find this
private final double goalHeightInches = 25.0;//TODO Find this
private static final double HORIZONTAL_FOV = Math.toRadians(63.3);
private static final double VERTICAL_FOV = Math.toRadians(49.7);

public Limelight(LimelightIO io) {
this.io = io;
}

@Override
public void setLEDState(LEDState ledState) {
public void periodic() {
io.updateInputs(inputs);
Logger.getInstance().processInputs("Limelight", inputs);


if(getTv()) target = bestTargetCentral();

Logger.getInstance().recordOutput("Limelight/targetDistance", getDistance());
Logger.getInstance().recordOutput("Limelight/targetX", getTx());
Logger.getInstance().recordOutput("Limelight/targetY", getTy());
Logger.getInstance().recordOutput("Limelight/validTargetsLength", io.getValidTargets().size());
Logger.getInstance().recordOutput("Limelight/allTargetsLength", io.getAllTargets().length);
}

public double getTa() {
return target[2];
}

public double getTx() {
if(getTv()) return target[3];
return 0;
}
public double getTx(double[] target){
return target[3];
}

public double getTy() {
if(getTv()) return target[4];
return 0;
}

public boolean getTv() {
return inputs.validTarget && io.getValidTargets().size() > 0;
}

public double getDistance(){
if(getTv()) return (goalHeightInches - limelightLensHeightInches)/Math.tan(Math.toRadians(getTy() + mountingAngleDegrees));
return 0;
}
public double getDistance(double[] target){
if(getTv()) return (goalHeightInches - limelightLensHeightInches)/Math.tan(Math.toRadians(target[4] + mountingAngleDegrees));
return 0;
}

private double[] bestTargetMinDist(){
if (!getTv()) return null ;
ArrayList<double[]> targets = io.getValidTargets();
double[] minTarget = targets.get(0);
double min = getDistance(minTarget);
for (int i = 1; i < targets.size(); i++) {
if(getDistance(targets.get(i)) < min){
min = getDistance(targets.get(i));
minTarget = targets.get(i);
}
}
return minTarget;
}

private double[] bestTargetCentral(){
if (!getTv()) return null;
ArrayList<double[]> targets = io.getValidTargets();
double[] minTarget = targets.get(0);
double min = getTx(targets.get(0));
for (int i = 1; i < targets.size(); i++) {
if(getTx(targets.get(i)) < min){
min = getTx(targets.get(i));
minTarget = targets.get(i);
}
}
return minTarget;

}

public boolean aimed(){
return getTx() < 1;
}

public double getHorizontalFov() {
return HORIZONTAL_FOV;
}
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
package org.team1540.bunnybotTank2023.io.vision;

import org.littletonrobotics.junction.AutoLog;

import java.util.ArrayList;

public interface LimelightIO {
public double[][] allTargets = {};
public ArrayList<double[]> validTargets = new ArrayList<>();

@AutoLog
public static class LimelightIOInputs{
public boolean validTarget = false;

public double captureLatencyMs = 0.0;
public double pipelineLatencyMs = 0.0;
public double parseLatencyMs = 0.0;
public double totalLatencyMs = 0.0;
public double timestampMs = 0.0;
public double pipeline = 0;
}

default void updateInputs(LimelightIOInputs inputs) {}

default double[][] getAllTargets() {
return new double[0][0];
}
default ArrayList<double[]> getValidTargets() {
return new ArrayList<>();
}

}
Loading

0 comments on commit 8485533

Please sign in to comment.