Skip to content

Commit

Permalink
more integration
Browse files Browse the repository at this point in the history 
shooter for now will not use PID, might have to add back for feeding @ diff distances

right now there will only be two sensors
  • Loading branch information
@Wi11iamYuan
Wi11iamYuan committed Oct 6, 2024
1 parent e260e01 commit c51ddba
Show file tree
Hide file tree
Showing 10 changed files with 266 additions and 232 deletions.
42 changes: 3 additions & 39 deletions src/main/java/frc/team3128/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -187,42 +187,6 @@ public static class VisionConstants {

public static final Matrix<N3,N1> SVR_VISION_MEASUREMENT_STD = VecBuilder.fill(0.5,0.5,Units.degreesToRadians(5));

public static final HashMap<Integer,Pose2d> APRIL_TAG_POS = new HashMap<Integer,Pose2d>();

static {
APRIL_TAG_POS.put(1, new Pose2d(
new Translation2d(Units.inchesToMeters(610.77), Units.inchesToMeters(42.19)),
Rotation2d.fromDegrees(180))
);
APRIL_TAG_POS.put(2, new Pose2d(
new Translation2d(Units.inchesToMeters(610.77), Units.inchesToMeters(108.19)),
Rotation2d.fromDegrees(180))
);
APRIL_TAG_POS.put(3, new Pose2d(
new Translation2d(Units.inchesToMeters(610.77), Units.inchesToMeters(174.19)),
Rotation2d.fromDegrees(180))
);
APRIL_TAG_POS.put(4, new Pose2d(
new Translation2d(Units.inchesToMeters(636.96), Units.inchesToMeters(265.74)),
Rotation2d.fromDegrees(180))
);
APRIL_TAG_POS.put(5, new Pose2d(
new Translation2d(Units.inchesToMeters(14.25), Units.inchesToMeters(265.74)),
Rotation2d.fromDegrees(0))
);
APRIL_TAG_POS.put(6, new Pose2d(
new Translation2d( Units.inchesToMeters(40.45), Units.inchesToMeters(174.19)),
Rotation2d.fromDegrees(0))
);
APRIL_TAG_POS.put(7, new Pose2d(
new Translation2d(Units.inchesToMeters(40.45), Units.inchesToMeters(108.19)),
Rotation2d.fromDegrees(0))
);
APRIL_TAG_POS.put(8, new Pose2d(
new Translation2d(Units.inchesToMeters(40.45), Units.inchesToMeters(42.19)),
Rotation2d.fromDegrees(0))
);
}
}

public static class FieldConstants{
Expand Down Expand Up @@ -342,7 +306,7 @@ public static class IntakeConstants {
public static final double UNIT_CONV_FACTOR = GEAR_RATIO * 360;

public static final int ROLLER_MOTOR_ID = 30;
public static final NAR_CANSpark ROLLER_MOTOR = new NAR_CANSpark(ROLLER_MOTOR_ID);
public static final NAR_CANSpark ROLLER_MOTOR = new NAR_CANSpark(ROLLER_MOTOR_ID, ControllerType.CAN_SPARK_FLEX);

public static final double STALL_CURRENT = 50;
public static final double STALL_POWER = .05;
Expand Down Expand Up @@ -420,7 +384,7 @@ public enum Colors {
}
public static class ClimberConstants {
public static final int CLIMB_MOTOR_ID = 50;
public static final NAR_CANSpark CLIMB_MOTOR = new NAR_CANSpark(CLIMB_MOTOR_ID);
// public static final NAR_CANSpark CLIMB_MOTOR = new NAR_CANSpark(CLIMB_MOTOR_ID);

public static final PIDFFConfig PIDConstants = new PIDFFConfig(2, 0, 0, 0.18, 0, 0, 0.3);//240
public static final double MAX_VELOCTIY = 10000000;
Expand All @@ -441,7 +405,7 @@ public static class ClimberConstants {
public static class HopperConstants {
public static final int HOPPER_MOTOR_ID = 40;
public static final NAR_CANSpark HOPPER_MOTOR = new NAR_CANSpark(HOPPER_MOTOR_ID);
public static final int HOPPER_FRONT_SENSOR_ID = 1;
public static final int HOPPER_FRONT_SENSOR_ID = 0;
public static final int HOPPER_BACK_SENSOR_ID = 2;

public static final double STALL_CURRENT = 50;
Expand Down
22 changes: 11 additions & 11 deletions src/main/java/frc/team3128/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,9 +91,9 @@ public void driverStationConnected() {
Log.info("State", "DS Connected");
Log.info("Alliance", getAlliance().toString());
if (getAlliance() == Alliance.Red) {
Camera.addTags(3, 4, 5, 11, 12);
Camera.addIgnoredTags(3, 4, 5, 11, 12);
} else {
Camera.addTags(6, 7, 8, 15, 16);
Camera.addIgnoredTags(6, 7, 8, 15, 16);
}
if (!NAR_Robot.logWithAdvantageKit) return;
if(DriverStation.getMatchType() != MatchType.None){
Expand All @@ -110,15 +110,15 @@ public void driverStationConnected() {
public void robotPeriodic(){
Camera.updateAll();
// TODO: this may break everything
if (Hopper.getInstance().hasObjectPresent()) {
if (timer.hasElapsed(2.5)) {
// CommandScheduler.getInstance().cancel(CommandScheduler.getInstance().requiring(Hopper.getInstance()));
CommandScheduler.getInstance().schedule(CmdManager.hopperOuttake());
timer.reset();
}
return;
}
timer.reset();
// if (Hopper.getInstance().hasObjectPresent()) {
// if (timer.hasElapsed(2.5)) {
// // CommandScheduler.getInstance().cancel(CommandScheduler.getInstance().requiring(Hopper.getInstance()));
// CommandScheduler.getInstance().schedule(CmdManager.hopperOuttake());
// timer.reset();
// }
// return;
// }
// timer.reset();
}

@Override
Expand Down
60 changes: 46 additions & 14 deletions src/main/java/frc/team3128/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,11 @@
import common.utility.shuffleboard.NAR_Shuffleboard;
import common.utility.sysid.CmdSysId;
import common.utility.tester.Tester;
import frc.team3128.subsystems.Hopper;
import frc.team3128.subsystems.Intake;
// import common.utility.tester.Tester.UnitTest;
import frc.team3128.subsystems.Leds;
import frc.team3128.subsystems.Shooter;
import frc.team3128.subsystems.Swerve;
import java.util.ArrayList;
import edu.wpi.first.apriltag.AprilTagFields;
Expand Down Expand Up @@ -77,7 +80,7 @@ public RobotContainer() {
buttonPad = new NAR_ButtonBoard(3);

//uncomment line below to enable driving
CommandScheduler.getInstance().setDefaultCommand(swerve, new CmdSwerveDrive(controller::getLeftX,controller::getLeftY, controller::getRightX, true));
CommandScheduler.getInstance().setDefaultCommand(Swerve.getInstance(), new CmdSwerveDrive(controller::getLeftX,controller::getLeftY, controller::getRightX, true));

initRobotTest();

Expand All @@ -86,8 +89,8 @@ public RobotContainer() {

configureButtonBindings();

NAR_Shuffleboard.addData("Limelight", "ValidTarget", ()-> limelight.hasValidTarget(), 0, 0);
NAR_Shuffleboard.addData("Limelight", "TX", ()-> limelight.getValue(LimelightKey.HORIZONTAL_OFFSET), 0, 1);
// NAR_Shuffleboard.addData("Limelight", "ValidTarget", ()-> limelight.hasValidTarget(), 0, 0);
// NAR_Shuffleboard.addData("Limelight", "TX", ()-> limelight.getValue(LimelightKey.HORIZONTAL_OFFSET), 0, 1);
}

private void configureButtonBindings() {
Expand All @@ -106,30 +109,59 @@ private void configureButtonBindings() {
controller.getLeftPOVButton().onTrue(runOnce(()-> {
CmdSwerveDrive.setTurnSetpoint(Robot.getAlliance() == Alliance.Red ? 270 : 90);
}));

controller.getButton(XboxButton.kLeftBumper).onTrue(intake(Intake.Setpoint.GROUND));
controller.getButton(XboxButton.kRightBumper).onTrue(retractIntake());

controller.getButton(XboxButton.kA).onTrue(Shooter.getInstance().runShooter(0.8));
controller.getButton(XboxButton.kY).onTrue(Shooter.getInstance().runShooter(0));
controller.getButton(XboxButton.kB).onTrue(Shooter.getInstance().runKickMotor(0.8)).onFalse(Shooter.getInstance().runKickMotor(0));

controller.getButton(XboxButton.kY).whileTrue(ampUp()).onFalse(ampFinAndDown());


// new Trigger(()->true).onTrue(queueNote());

new Trigger(()-> Intake.getInstance().getMeasurement() > 90)
.and(()->!Hopper.getInstance().hasObjectPresent())
.onTrue(Hopper.getInstance().runManipulator(0.8))
.onFalse(Hopper.getInstance().runManipulator(0));

new Trigger(()-> Shooter.getInstance().noteInRollers()).negate()
.and(()->Hopper.getInstance().hasObjectPresent())
.onTrue(sequence(
Shooter.getInstance().runKickMotor(0.8),
Hopper.getInstance().runManipulator(0.8)
))
.onFalse(sequence(
Shooter.getInstance().runKickMotor(0),
Hopper.getInstance().runManipulator(0)
));

}

@SuppressWarnings("unused")
public void initCameras() {
Camera.disableAll();
Camera.configCameras(AprilTagFields.k2024Crescendo, PoseStrategy.LOWEST_AMBIGUITY, (pose, time) -> swerve.addVisionMeasurement(pose, time), () -> swerve.getPose());
Camera.setAmbiguityThreshold(0.3);
Camera.overrideThreshold = 30;
Camera.validDist = 0.5;
Camera.setResources(()-> swerve.getYaw(), (pose, time) -> swerve.addVisionMeasurement(pose, time), AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(), () -> swerve.getPose());
Camera.setThresholds(5, 0.5);
// Camera.overrideThreshold = 30;
// Camera.validDist = 0.5;
// Camera.addIgnoredTags(13.0, 14.0);

if (Robot.isReal()) {
final Camera camera = new Camera("FRONT_LEFT", Units.inchesToMeters(10.055), Units.inchesToMeters(9.79), Units.degreesToRadians(30), Units.degreesToRadians(-28.125), 0);
final Camera camera2 = new Camera("FRONT_RIGHT", Units.inchesToMeters(10.055), -Units.inchesToMeters(9.79), Units.degreesToRadians(-30), Units.degreesToRadians(-28.125), 0);
camera.setCamDistanceThreshold(3.5);
camera2.setCamDistanceThreshold(5);
// final Camera camera = new Camera("FRONT_LEFT", Units.inchesToMeters(10.055), Units.inchesToMeters(9.79), Units.degreesToRadians(30), Units.degreesToRadians(-28.125), 0);
// final Camera camera2 = new Camera("FRONT_RIGHT", Units.inchesToMeters(10.055), -Units.inchesToMeters(9.79), Units.degreesToRadians(-30), Units.degreesToRadians(-28.125), 0);
// camera.setCamDistanceThreshold(3.5);
// camera2.setCamDistanceThreshold(5);
}
// final Camera camera3 = new Camera("LEFT", Units.inchesToMeters(-3.1), Units.inchesToMeters(12.635), Units.degreesToRadians(90), Units.degreesToRadians(-10), 0);
// final Camera camera4 = new Camera("RIGHT", Units.inchesToMeters(-3.1), Units.inchesToMeters(-12.635), Units.degreesToRadians(-90), Units.degreesToRadians(0), 0);

// sideCams.add(camera3);
// sideCams.add(camera4);

limelight = new Limelight("limelight-mason", 0, 0, 0);
// limelight = new Limelight("limelight-mason", 0, 0, 0);
}

public static void toggleSideCams(boolean enable) {
Expand Down Expand Up @@ -172,7 +204,7 @@ public boolean isConnected() {
}

private void initRobotTest() {
Tester tester = Tester.getInstance();
tester.getTest("Robot").setTimeBetweenTests(0.5);
// Tester tester = Tester.getInstance();
// tester.getTest("Robot").setTimeBetweenTests(0.5);
}
}
Loading

0 comments on commit c51ddba

Please sign in to comment.