Skip to content

Commit

Permalink
auto alliance flipping acutally works confirmed
Browse files Browse the repository at this point in the history
  • Loading branch information
@Ani-8712
Ani-8712 committed Oct 22, 2024
1 parent 7751048 commit 3520e0c
Show file tree
Hide file tree
Showing 11 changed files with 125 additions and 148 deletions.
122 changes: 60 additions & 62 deletions src/main/java/team3647/frc2024/auto/AutoCommands.java
View file
Original file line number Diff line number Diff line change
@@ -1,24 +1,12 @@
package team3647.frc2024.auto;

import java.util.ArrayList;
import java.util.List;
import java.util.Map;
import java.util.Set;
import java.util.function.BooleanSupplier;
import java.util.function.Consumer;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;

import org.littletonrobotics.junction.Logger;

import com.choreo.lib.Choreo;
import com.choreo.lib.ChoreoControlFunction;
import com.choreo.lib.ChoreoTrajectory;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.path.PathPlannerPath;
import com.pathplanner.lib.util.PathPlannerLogging;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
Expand All @@ -33,6 +21,15 @@
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
import edu.wpi.first.wpilibj2.command.SelectCommand;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import java.util.ArrayList;
import java.util.List;
import java.util.Map;
import java.util.Set;
import java.util.function.BooleanSupplier;
import java.util.function.Consumer;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;
import team3647.frc2024.constants.AutoConstants;
import team3647.frc2024.constants.FieldConstants;
import team3647.frc2024.subsystems.Superstructure;
Expand Down Expand Up @@ -96,13 +93,10 @@ public class AutoCommands implements AllianceObserver {
private final String shoot2_to_f5 = "shoot2 to f5";
private final String f4_to_shoot2 = "f4 to shoot2";


private final String s15_straight_forward = "s15 straight forward";
private final String trap_test = "trap test 2";
private final String s3_preload_move = "s3 preload and move";



public final Trigger currentYes;

private boolean hasPiece = true;
Expand Down Expand Up @@ -298,17 +292,25 @@ public AutoCommands(
this.blueRightTest =
new AutonomousMode(testRight(), getInitial("test right"), "blue right test");
this.redTest =
new AutonomousMode(testForward(), AllianceFlip.flipForPP(getInitial(s15_straight_forward)));
this.doNothing =
new AutonomousMode(Commands.none(), AllianceFlip.flipForPP(getInitial(s15_straight_forward), color == Alliance.Red));
this.redPreloadOnly =
new AutonomousMode(preloadOnly(), AllianceFlip.flipForPP(getInitial(s1_to_n1)), "red preload only");
this.bluePreloadOnly =
new AutonomousMode(
testForward(), AllianceFlip.flipForPP(getInitial(s15_straight_forward)));
this.doNothing =
new AutonomousMode(
Commands.none(),
AllianceFlip.flipForPP(
getInitial(s15_straight_forward), color == Alliance.Red));
this.redPreloadOnly =
new AutonomousMode(
preloadOnly(),
AllianceFlip.flipForPP(getInitial(s1_to_n1)),
"red preload only");
this.bluePreloadOnly =
new AutonomousMode(preloadOnly(), getInitial(s1_to_n1), "blue preload only");
this.preloadMove =
new AutonomousMode(preloadMove(), AllianceFlip.flipForPP(getInitial(s3_preload_move), color == Alliance.Red));


this.preloadMove =
new AutonomousMode(
preloadMove(),
AllianceFlip.flipForPP(getInitial(s3_preload_move), color == Alliance.Red));

redAutoModes =
new ArrayList<AutonomousMode>(
Set.of(
Expand All @@ -335,7 +337,6 @@ public AutoCommands(
blueForwardTest,
blueRightTest,
doNothing));

}

public enum MidlineNote {
Expand Down Expand Up @@ -463,19 +464,13 @@ public Command fullCenterS1(Alliance color) {
.repeatedly()));
}

public Command preloadOnly(){
return Commands.parallel(
target().withTimeout(10),
scorePreload()
);
public Command preloadOnly() {
return Commands.parallel(target().withTimeout(10), scorePreload());
}

public Command preloadMove(){
public Command preloadMove() {
return Commands.sequence(
scorePreload(),
Commands.waitSeconds(2),
followChoreoPath(s3_preload_move, color)
);
scorePreload(), Commands.waitSeconds(2), followChoreoPath(s3_preload_move, color));
}

public Command fullCenterS3(Alliance color) {
Expand Down Expand Up @@ -728,12 +723,12 @@ public Command poopPreload() {
.withTimeout(1.4);
}

public Command scorePreload(){
public Command scorePreload() {
return Commands.parallel(
superstructure.spinUp(),
superstructure.prep(),
Commands.sequence(Commands.waitSeconds(1), superstructure.feed()))
.withTimeout(1.4);
.withTimeout(1.4);
}

public Pose2d getInitial(String path) {
Expand Down Expand Up @@ -928,32 +923,35 @@ public Command followChoreoPathWithOverrideNoverrideFast(String path, Alliance c
AutoConstants.kXController,
AutoConstants.kYController,
AutoConstants.kRotController),
(ChassisSpeeds speeds) ->
{
var motionRotComponent = deeTheta();
var motionXComponent = speeds.vxMetersPerSecond;
var motionYComponent = speeds.vyMetersPerSecond;

var posexthresholdlow = color == Alliance.Blue ? 5 : FieldConstants.kFieldLength - 8.75;
var posexThresholdHigh = color == Alliance.Blue ? 8.75 : FieldConstants.kFieldLength - 5;

var isInPose = swerve.getOdoPose().getX() > posexthresholdlow && swerve.getOdoPose().getX() < posexThresholdHigh;

if(!this.hasPiece && hasTarget.getAsBoolean() && isInPose){
motionXComponent = autoDriveVelocities.get().dx;
motionYComponent = autoDriveVelocities.get().dy;
motionRotComponent = autoDriveVelocities.get().dtheta;
}

if(isInPose){
motionRotComponent = speeds.omegaRadiansPerSecond;
}
swerve.drive(motionXComponent, motionYComponent, motionRotComponent);},
(ChassisSpeeds speeds) -> {
var motionRotComponent = deeTheta();
var motionXComponent = speeds.vxMetersPerSecond;
var motionYComponent = speeds.vyMetersPerSecond;

var posexthresholdlow =
color == Alliance.Blue ? 5 : FieldConstants.kFieldLength - 8.75;
var posexThresholdHigh =
color == Alliance.Blue ? 8.75 : FieldConstants.kFieldLength - 5;

var isInPose =
swerve.getOdoPose().getX() > posexthresholdlow
&& swerve.getOdoPose().getX() < posexThresholdHigh;

if (!this.hasPiece && hasTarget.getAsBoolean() && isInPose) {
motionXComponent = autoDriveVelocities.get().dx;
motionYComponent = autoDriveVelocities.get().dy;
motionRotComponent = autoDriveVelocities.get().dtheta;
}

if (isInPose) {
motionRotComponent = speeds.omegaRadiansPerSecond;
}
swerve.drive(motionXComponent, motionYComponent, motionRotComponent);
},
() -> mirror)
.andThen(Commands.runOnce(() -> swerve.drive(0, 0, 0), swerve));
}


public Command followChoreoPathWithOverrideLongTimer(String path, Alliance color) {
ChoreoTrajectory traj = Choreo.getTrajectory(path);
boolean mirror = color == Alliance.Red;
Expand All @@ -978,6 +976,7 @@ public Command followChoreoPathWithOverrideLongTimer(String path, Alliance color
() -> mirror)
.andThen(Commands.runOnce(() -> swerve.drive(0, 0, 0), swerve));
}

public Command customChoreoFolloweForOverride(
ChoreoTrajectory trajectory,
Supplier<Pose2d> poseSupplier,
Expand Down Expand Up @@ -1052,7 +1051,7 @@ public Command customChoreoFolloweForOverrideLongTimer(
outputChassisSpeeds.accept(new ChassisSpeeds());
},
() ->
timer.hasElapsed(0.9)//gives the note more time to get in the intake;
timer.hasElapsed(0.9) // gives the note more time to get in the intake;
&& ((swerve.getVel() < 0.2
& swerve.getOdoPose()
.minus(
Expand All @@ -1072,7 +1071,6 @@ public Command customChoreoFolloweForOverrideLongTimer(
swerve);
}


public Command customChoreoFolloweForOverrideSlow(
ChoreoTrajectory trajectory,
Supplier<Pose2d> poseSupplier,
Expand Down
4 changes: 1 addition & 3 deletions src/main/java/team3647/frc2024/constants/PivotConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,6 @@
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 @@ -78,8 +77,7 @@ public class PivotConstants {
public static final InterpolatingDoubleTreeMap kMasterTrapMap =
new InterpolatingDoubleTreeMap();

public static final InterpolatingDoubleTreeMap feedmap =
new InterpolatingDoubleTreeMap();
public static final InterpolatingDoubleTreeMap feedmap = new InterpolatingDoubleTreeMap();

static {
// kMasterSpeakerMap.put(0.0, 60.0);
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/team3647/frc2024/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,8 @@

import com.ctre.phoenix6.SignalLogger;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
Expand Down
28 changes: 13 additions & 15 deletions src/main/java/team3647/frc2024/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import java.util.function.BooleanSupplier;
import team3647.frc2024.auto.AutoCommands;
import team3647.frc2024.commands.ClimbCommands;
import team3647.frc2024.commands.DrivetrainCommands;
Expand Down Expand Up @@ -247,11 +246,12 @@ private void configureButtonBindings() {
climbing.onTrue(superstructure.setIsClimbing());
climbing.onFalse(superstructure.setIsNotClimbing());

coController.leftJoyStickPress.and(coController.rightJoyStickPress)
.onTrue(superstructure.stowChurro());
coController
.leftJoyStickPress
.and(coController.rightJoyStickPress)
.onTrue(superstructure.stowChurro());

coController.leftTrigger.and(coController.rightTrigger)
.onTrue(superstructure.stowAll());
coController.leftTrigger.and(coController.rightTrigger).onTrue(superstructure.stowAll());

// characterization

Expand Down Expand Up @@ -312,7 +312,7 @@ public void teleopInit() {}
void configTestCommands() {}

public void configureSmartDashboardLogging() {
//logging offset for interp
// logging offset for interp
printer.addDouble("offset", targetingUtil::getOffset);
printer.addBoolean("note seen", () -> !autoCommands.noteNotSeen.getAsBoolean());
// SmartDashboard.putNumber("pivot output", 0);
Expand Down Expand Up @@ -407,11 +407,14 @@ 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());
new Trigger(
() ->
mainController.leftBumper.getAsBoolean()
&& !DriverStation.isAutonomous());

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

public final Pivot pivot =
new Pivot(
PivotConstants.kMaster,
Expand Down Expand Up @@ -484,8 +487,6 @@ public Command getAutonomousCommand() {
new AprilTagPhotonVision(VisionConstants.zoom, VisionConstants.robotToZoom)
.withPriority(true);



public AllianceChecker allianceChecker = new AllianceChecker();

private final VisionController visionController =
Expand Down Expand Up @@ -566,8 +567,7 @@ public Command getAutonomousCommand() {

private final Trigger piece = new Trigger(() -> superstructure.getPiece());

private final Trigger climbing =
new Trigger(() -> climbRight.getPosition() > 1);
private final Trigger climbing = new Trigger(() -> climbRight.getPosition() > 1);

private final Trigger goodToAmp =
new Trigger(
Expand All @@ -583,6 +583,4 @@ public Command getAutonomousCommand() {
new Trigger(() -> superstructure.current() && wrist.getAngle() < 5) // 41
.debounce(0.06)
.or(mainController.buttonB);


}
6 changes: 2 additions & 4 deletions src/main/java/team3647/frc2024/subsystems/ShooterRight.java
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,8 @@
package team3647.frc2024.subsystems;


import org.littletonrobotics.junction.Logger;

import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import org.littletonrobotics.junction.Logger;
import team3647.lib.TalonFXSubsystem;

public class ShooterRight extends TalonFXSubsystem {
Expand All @@ -24,13 +22,13 @@ public ShooterRight(
this.ff = ff;
}


@Override
public void periodic() {
// TODO Auto-generated method stub
super.periodic();
Logger.recordOutput("realrightshootdemand", velocityVoltage.Velocity);
}

public boolean velocityGreater(double setpoint) {
return super.getVelocity() > setpoint;
}
Expand Down
Loading

0 comments on commit 3520e0c

Please sign in to comment.