Skip to content

Commit

Permalink
gg pre batbcode
Browse files Browse the repository at this point in the history
  • Loading branch information
@Ani-8712
Ani-8712 committed Oct 11, 2024
1 parent 7d8426b commit 0bcef47
Show file tree
Hide file tree
Showing 7 changed files with 83 additions and 44 deletions.
2 changes: 1 addition & 1 deletion src/main/java/team3647/frc2024/commands/IntakeCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
public class IntakeCommands {

public Command intake() {
return Commands.run(() -> intake.openLoop(0.8), intake);
return Commands.run(() -> intake.openLoop(1), intake);
}

public Command outtake() {
Expand Down
35 changes: 21 additions & 14 deletions src/main/java/team3647/frc2024/constants/PivotConstants.java
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,17 @@
package team3647.frc2024.constants;

import com.ctre.phoenix6.configs.*;
import com.ctre.phoenix6.controls.*;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.MotionMagicConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfigurator;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.*;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.playingwithfusion.TimeOfFlight;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform2d;
Expand Down Expand Up @@ -91,18 +98,18 @@ public class PivotConstants {
// kMasterSpeakerMap.put(5.75, 27.7);
// kMasterSpeakerMap.put(6.0, 27.4);
// kMasterSpeakerMap.put(20.0, 26.5);
kMasterSpeakerMap.put(0.0, 60.0);
kMasterSpeakerMap.put(1.0, 60.0);
kMasterSpeakerMap.put(1.5, 60.0); //
kMasterSpeakerMap.put(2.0, 49.0); //
kMasterSpeakerMap.put(0.0, 60.0+1.4);
kMasterSpeakerMap.put(1.0, 60.0+1.4);
kMasterSpeakerMap.put(1.5, 60.0+1.4); //
kMasterSpeakerMap.put(2.0, 49.0+1.4); //
kMasterSpeakerMap.put(2.5, 45.0); //
kMasterSpeakerMap.put(3.0, 35.0); //
kMasterSpeakerMap.put(3.5, 31.7); //
kMasterSpeakerMap.put(4.0, 30.0); //
kMasterSpeakerMap.put(4.5, 27.9); //
kMasterSpeakerMap.put(5.0, 26.5); //
kMasterSpeakerMap.put(5.5, 26.0); //
kMasterSpeakerMap.put(6.0, 23.4); //
kMasterSpeakerMap.put(3.0, 35.0+1.4); //
kMasterSpeakerMap.put(3.5, 31.7+2.2); //
kMasterSpeakerMap.put(4.0, 30.0+0.6); //
kMasterSpeakerMap.put(4.5, 27.9-1.4); //
kMasterSpeakerMap.put(5.0, 26.5+0.6); //
kMasterSpeakerMap.put(5.5, 26.0-2.2); //
kMasterSpeakerMap.put(6.0, 23.4+0.6); //
kMasterSpeakerMap.put(6.52, 22.35); //
kMasterSpeakerMap.put(7.04, 22.0); //
kMasterSpeakerMap.put(7.4961, 20.7); // it stops working here
Expand Down
15 changes: 11 additions & 4 deletions src/main/java/team3647/frc2024/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +95,7 @@ public RobotContainer() {
}

private void configAllianceChecker() {
allianceChecker.registerObservers(autoCommands, autoChooser);
allianceChecker.registerObservers(autoCommands, autoChooser, tracker);
}

private void configureButtonBindings() {
Expand Down Expand Up @@ -400,7 +400,12 @@ public Command getAutonomousCommand() {

public final Intake intake =
new Intake(IntakeConstants.kMaster, 1, 1, IntakeConstants.kNominalVoltage, 0.02);
private final Trigger isIntaking =
new Trigger(() -> mainController.leftBumper.getAsBoolean() && !DriverStation.isAutonomous());

private final Trigger isNotIntaking =
new Trigger(() -> !isIntaking.getAsBoolean()).debounce(0.1);

public final Pivot pivot =
new Pivot(
PivotConstants.kMaster,
Expand All @@ -415,7 +420,8 @@ public Command getAutonomousCommand() {
PivotConstants.maxKG,
0.02,
PivotConstants.tofBack,
PivotConstants.tofFront);
PivotConstants.tofFront,
isIntaking);

private final ClimbLeft climbLeft =
new ClimbLeft(
Expand Down Expand Up @@ -472,6 +478,8 @@ public Command getAutonomousCommand() {
new AprilTagPhotonVision(VisionConstants.zoom, VisionConstants.robotToZoom)
.withPriority(true);



public AllianceChecker allianceChecker = new AllianceChecker();

private final VisionController visionController =
Expand Down Expand Up @@ -570,6 +578,5 @@ public Command getAutonomousCommand() {
.debounce(0.06)
.or(mainController.buttonB);

private final BooleanSupplier isIntaking =
() -> mainController.leftBumper.getAsBoolean() && !DriverStation.isAutonomous();

}
5 changes: 4 additions & 1 deletion src/main/java/team3647/frc2024/subsystems/Pivot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@ public class Pivot extends TalonFXSubsystem {
private final double maxAngleUnderStage;

BooleanSupplier underStage;
BooleanSupplier isIntaking;

private final double maxKG;

Expand All @@ -39,7 +40,8 @@ public Pivot(
double maxKG,
double kDt,
TimeOfFlight tofBack,
TimeOfFlight tofFront) {
TimeOfFlight tofFront,
BooleanSupplier isIntaking) {
super(master, ticksToMetersPerSec, ticksToMeters, nominalVoltage, kDt);
super.addFollower(slave, false);
this.minAngle = minAngle;
Expand All @@ -50,6 +52,7 @@ public Pivot(
this.maxKG = maxKG;
this.tofBack = tofBack;
this.tofFront = tofFront;
this.isIntaking = isIntaking;

master.getPosition().setUpdateFrequency(250);
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/team3647/frc2024/subsystems/Superstructure.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,7 +138,7 @@ public boolean isClimbing() {

public Command spinUp() {
return shooterCommands.setVelocityIndep(
() -> shooterSpeedSupplierRight.getAsDouble() + 4,
() -> shooterSpeedSupplierRight.getAsDouble() + 1,
() -> shooterSpeedSupplierLeft.getAsDouble());
}

Expand Down
63 changes: 41 additions & 22 deletions src/main/java/team3647/frc2024/util/RobotTracker.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,10 +5,14 @@
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;

import java.sql.Driver;
import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;
import team3647.lib.team6328.VirtualSubsystem;
import team3647.lib.team9442.AllianceChecker;
import team3647.lib.team9442.AllianceObserver;

public class RobotTracker extends VirtualSubsystem implements AllianceObserver {
Expand Down Expand Up @@ -41,31 +45,42 @@ public RobotTracker(
Supplier<Pose2d> drivePose,
Supplier<ChassisSpeeds> driveSpeeds,
InterpolatingDoubleTreeMap shootSpeedMap) {
this.color = Alliance.Red;
this.color = Alliance.Blue;




DriverStation.getAlliance()
.ifPresent(
(alliance) ->
this.speakerPose =
alliance == Alliance.Red
? AllianceFlip.flipForPP(speakerPose)
: speakerPose);

DriverStation.getAlliance()
.ifPresent(
(alliance) ->
this.ampPose =
alliance == Alliance.Red
? AllianceFlip.flipForPP(ampPose)
: ampPose);
DriverStation.getAlliance()
.ifPresent(
(alliance) ->
this.ampPose =
alliance == Alliance.Red
? AllianceFlip.flipForPP(feedPose)
: feedPose);

this.speakerPose = AllianceFlip.flipForPP(speakerPose, this.color == Alliance.Red);
this.ampPose = color.equals(Alliance.Red) ? AllianceFlip.flipForPP(ampPose) : ampPose;
this.robotToShooter = robotToShooter;
this.feedPose = AllianceFlip.flipForPP(feedPose, color == Alliance.Red);
this.drivePose = drivePose;
this.driveSpeeds = driveSpeeds;
this.shootSpeedMap = shootSpeedMap;


// DriverStation.getAlliance()
// .ifPresent(
// (alliance) ->
// this.speakerPose =
// alliance == Alliance.Red
// ? AllianceFlip.flipForPP(speakerPose)
// : speakerPose);

// DriverStation.getAlliance()
// .ifPresent(
// (alliance) ->
// this.ampPose =
// alliance == Alliance.Red
// ? AllianceFlip.flipForPP(ampPose)
// : ampPose);
}

@Override
Expand Down Expand Up @@ -161,10 +176,14 @@ public Pose2d getFeed() {

@Override
public void onAllianceFound(Alliance color) {
this.color = color;
this.ampPose = color.equals(Alliance.Red) ? AllianceFlip.flipForPP(ampPose) : ampPose;
this.speakerPose = AllianceFlip.flipForPP(speakerPose, color == Alliance.Red);
this.feedPose = AllianceFlip.flipForPP(feedPose, color == Alliance.Red);

// for(int i = 0; i <5; i++){
// DriverStation.reportError("METHODRUN" + DriverStation.getAlliance().isPresent(), false);
// }
// this.color = color;
// DriverStation.reportError(color.name(), false);
// this.ampPose = color.equals(Alliance.Red) ? AllianceFlip.flipForPP(ampPose) : ampPose;
// this.speakerPose = color.equals(Alliance.Red) ? AllianceFlip.flipForPP(speakerPose) : speakerPose;
// DriverStation.reportError(String.valueOf(speakerPose.getX()), false);
// this.feedPose = color.equals(Alliance.Red) ? AllianceFlip.flipForPP(feedPose) : feedPose;
}
}
5 changes: 4 additions & 1 deletion src/main/java/team3647/lib/team9442/AllianceChecker.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,10 +6,13 @@
import java.util.List;
import java.util.Optional;

import org.littletonrobotics.junction.Logger;

public class AllianceChecker {
private final List<AllianceObserver> observers = new ArrayList<>();
private Optional<Alliance> alliance = DriverStation.getAlliance();


public void registerObserver(AllianceObserver observer) {
observers.add(observer);
}
Expand All @@ -22,7 +25,7 @@ public void registerObservers(AllianceObserver... addObservers) {

public void periodic() {
alliance = DriverStation.getAlliance();

alliance.ifPresent(color -> observers.forEach(observer -> observer.onAllianceFound(color)));
}
}

0 comments on commit 0bcef47

Please sign in to comment.