Skip to content

Commit

Permalink
voltage shot
Browse files Browse the repository at this point in the history
  • Loading branch information
@Ani-8712
Ani-8712 committed Sep 12, 2024
1 parent b83bb59 commit 64bbc1c
Show file tree
Hide file tree
Showing 14 changed files with 130 additions and 116 deletions.
111 changes: 64 additions & 47 deletions src/main/java/team3647/frc2024/auto/AutoCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveFeedforward;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
Expand All @@ -22,9 +21,7 @@
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.Iterator;
import java.util.List;
import java.util.Map;
import java.util.Set;
Expand All @@ -41,7 +38,7 @@
import team3647.frc2024.util.TargetingUtil;
import team3647.lib.team9442.AllianceObserver;

public class AutoCommands implements AllianceObserver {
public class AutoCommands implements AllianceObserver {
private final SwerveDrive swerve;
private final Supplier<Twist2d> autoDriveVelocities;
private final Superstructure superstructure;
Expand Down Expand Up @@ -150,7 +147,7 @@ public class AutoCommands implements AllianceObserver {
public final AutonomousMode redTest;

public List<AutonomousMode> redAutoModes;

public List<AutonomousMode> blueAutoModes;

private MidlineNote lastNote = MidlineNote.ONE;
Expand Down Expand Up @@ -198,18 +195,25 @@ public AutoCommands(
// AllianceFlip.flipForPP(getInitial(s1_to_n1_to_f1)));

this.blueTest_S15 =
new AutonomousMode(testS1(Alliance.Blue), getInitial(s15_straight_forward), "blue test");
new AutonomousMode(
testS1(Alliance.Blue), getInitial(s15_straight_forward), "blue test");

this.redTwo_S2F3 =
new AutonomousMode(
two_S2F3(Alliance.Red), AllianceFlip.flipForPP(getInitial(s2_to_f3)), "red two note mid");
two_S2F3(Alliance.Red),
AllianceFlip.flipForPP(getInitial(s2_to_f3)),
"red two note mid");
this.redFour_S3F5F4F3 =
new AutonomousMode(
four_S3F5F4F3(Alliance.Red), AllianceFlip.flipForPP(getInitial(s35_to_f5)), "red 4 source");
four_S3F5F4F3(Alliance.Red),
AllianceFlip.flipForPP(getInitial(s35_to_f5)),
"red 4 source");

this.redFour_S1N1N2N3 =
new AutonomousMode(
four_S1N1N2N3(Alliance.Red), AllianceFlip.flipForPP(getInitial(s1_to_n1)), "red 4 amp near");
four_S1N1N2N3(Alliance.Red),
AllianceFlip.flipForPP(getInitial(s1_to_n1)),
"red 4 amp near");

// this.redFive_S1N1F1F2F3 = // DONT USE
// new AutonomousMode(
Expand All @@ -218,66 +222,81 @@ public AutoCommands(

this.redFour_S1F1F2F3 =
new AutonomousMode(
four_S1F1F2F3(Alliance.Red), AllianceFlip.flipForPP(getInitial(s15_to_f1)), "red 4 amp far");
four_S1F1F2F3(Alliance.Red),
AllianceFlip.flipForPP(getInitial(s15_to_f1)),
"red 4 amp far");

this.redSix_S1F1F2N1N2N3 =
new AutonomousMode(
six_S1F1F2N1N2N3(Alliance.Red),
AllianceFlip.flipForPP(getInitial(s15_to_f1)), "red six note");
AllianceFlip.flipForPP(getInitial(s15_to_f1)),
"red six note");

this.blueFour_S1N1N2N3 =
new AutonomousMode(four_S1N1N2N3(Alliance.Blue), getInitial(s1_to_n1), "blue 4 amp near");
new AutonomousMode(
four_S1N1N2N3(Alliance.Blue), getInitial(s1_to_n1), "blue 4 amp near");

this.blueSix_S1F1F2N1N2N3 =
new AutonomousMode(six_S1F1F2N1N2N3(Alliance.Blue), getInitial(s15_to_f1), "blue six note");
new AutonomousMode(
six_S1F1F2N1N2N3(Alliance.Blue), getInitial(s15_to_f1), "blue six note");

this.blueFour_S3F5F4F3 =
new AutonomousMode(four_S3F5F4F3(Alliance.Blue), getInitial(s35_to_f5), "blue 4 source far");
new AutonomousMode(
four_S3F5F4F3(Alliance.Blue), getInitial(s35_to_f5), "blue 4 source far");

this.blueFour_S1F1F2F3 =
new AutonomousMode(four_S1F1F2F3(Alliance.Blue), getInitial(s15_to_f1), "blue 4 amp far");
new AutonomousMode(
four_S1F1F2F3(Alliance.Blue), getInitial(s15_to_f1), "blue 4 amp far");

this.blueFullCenterS1 =
new AutonomousMode(fullCenterS1(Alliance.Blue), getInitial(s15_to_f1), "blue all mid amp");
new AutonomousMode(
fullCenterS1(Alliance.Blue), getInitial(s15_to_f1), "blue all mid amp");

this.redFullCenterS1 =
new AutonomousMode(
fullCenterS1(Alliance.Red), AllianceFlip.flipForPP(getInitial(s15_to_f1)), "red all mid amp");
fullCenterS1(Alliance.Red),
AllianceFlip.flipForPP(getInitial(s15_to_f1)),
"red all mid amp");

this.blueFullCenterS3 =
new AutonomousMode(fullCenterS3(Alliance.Blue), getInitial(s35_to_f5), "blue all mid source");
new AutonomousMode(
fullCenterS3(Alliance.Blue), getInitial(s35_to_f5), "blue all mid source");

this.redFullCenterS3 =
new AutonomousMode(
fullCenterS3(Alliance.Red), AllianceFlip.flipForPP(getInitial(s35_to_f5)), "red all mid source");

this.blueForwardTest =
fullCenterS3(Alliance.Red),
AllianceFlip.flipForPP(getInitial(s35_to_f5)),
"red all mid source");

this.blueForwardTest =
new AutonomousMode(testForward(), getInitial("test Forward"), "blue forward test");
this.blueRightTest =
this.blueRightTest =
new AutonomousMode(testRight(), getInitial("test right"), "blue right test");
this.redTest =
this.redTest =
new AutonomousMode(testForward(), AllianceFlip.flipForPP(getInitial(f1_to_f2)));

redAutoModes = new ArrayList<AutonomousMode>(Set.of(
redFullCenterS1,
redFullCenterS3,
redFour_S1F1F2F3,
redFour_S1N1N2N3,
redFour_S3F5F4F3,
redSix_S1F1F2N1N2N3,
redTwo_S2F3));

blueAutoModes = new ArrayList<AutonomousMode>(Set.of(
blueFour_S1F1F2F3,
blueFour_S1N1N2N3,
blueFour_S3F5F4F3,
blueFullCenterS1,
blueFullCenterS3,
blueSix_S1F1F2N1N2N3,
blueForwardTest,
blueRightTest));


redAutoModes =
new ArrayList<AutonomousMode>(
Set.of(
redFullCenterS1,
redFullCenterS3,
redFour_S1F1F2F3,
redFour_S1N1N2N3,
redFour_S3F5F4F3,
redSix_S1F1F2N1N2N3,
redTwo_S2F3));

blueAutoModes =
new ArrayList<AutonomousMode>(
Set.of(
blueFour_S1F1F2F3,
blueFour_S1N1N2N3,
blueFour_S3F5F4F3,
blueFullCenterS1,
blueFullCenterS3,
blueSix_S1F1F2N1N2N3,
blueForwardTest,
blueRightTest));
}

public enum MidlineNote {
Expand Down Expand Up @@ -374,11 +393,11 @@ public Command getScoringSequenceF5F4(Alliance color) {
followChoreoPathWithOverrideFast(shoot4_to_f4, color));
}

public Command testForward(){
public Command testForward() {
return followChoreoPath("test Forward", Alliance.Blue);
}

public Command testRight(){
public Command testRight() {
return followChoreoPath("test right", Alliance.Blue);
}

Expand Down Expand Up @@ -961,6 +980,4 @@ public Command followChoreoPath(String path, Alliance color) {
() -> mirror,
swerve);
}


}
4 changes: 2 additions & 2 deletions src/main/java/team3647/frc2024/auto/AutonomousMode.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ public AutonomousMode(Command autoCommand, Pose2d ppinitial, String name) {
this.name = name;
}

public AutonomousMode(Command autoCommand, Pose2d ppinitial){
public AutonomousMode(Command autoCommand, Pose2d ppinitial) {
this(autoCommand, ppinitial, "no name");
}

Expand All @@ -28,7 +28,7 @@ public Pose2d getPathplannerPose2d() {

private final String name;

public String getName(){
public String getName() {
return name;
}
}
4 changes: 2 additions & 2 deletions src/main/java/team3647/frc2024/constants/PivotConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ public class PivotConstants {
kMasterSpeakerMap.put(6.0, 23.4); //
kMasterSpeakerMap.put(6.52, 22.35); //
kMasterSpeakerMap.put(7.04, 22.0); //
kMasterSpeakerMap.put(7.4961, 20.7); //it stops working here
kMasterSpeakerMap.put(7.4961, 20.7); // it stops working here
kMasterSpeakerMap.put(8.0, 20.7); //
kMasterSpeakerMap.put(8.5, 20.7); //
kMasterSpeakerMap.put(20.0, 20.7); // //
Expand Down Expand Up @@ -144,7 +144,7 @@ public class PivotConstants {
kMasterConfigurator.apply(kMasterSoftLimit);
kSlaveConfigurator.apply(kMasterSlot0);
kSlaveConfigurator.apply(kMasterCurrent);
kSlaveConfigurator.apply(kMasterMotionMagic);
kSlaveConfigurator.apply(kMasterMotionMagic);
kSlaveConfigurator.apply(kMasterMotorOutput);
kSlaveConfigurator.apply(kMasterSoftLimit);
}
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/team3647/frc2024/constants/ShooterConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,8 +42,8 @@ public class ShooterConstants {

public static final SimpleMotorFeedforward ff = new SimpleMotorFeedforward(kS, kV, kA);

public static final double leftKP = 6;
public static final double rightKP = 6;
public static final double leftKP = 0.3;
public static final double rightKP = 0.3;
public static final double masterKI = 0;
public static final double masterKD = 0;

Expand Down
9 changes: 1 addition & 8 deletions src/main/java/team3647/frc2024/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import org.littletonrobotics.junction.AutoLogOutputManager;
Expand All @@ -19,7 +18,6 @@
import org.littletonrobotics.junction.wpilog.WPILOGReader;
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
import team3647.lib.team6328.VirtualSubsystem;
import team3647.lib.team9442.AllianceChecker;

/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
Expand All @@ -32,10 +30,7 @@ public class Robot extends LoggedRobot {
public static final double kTenMSLoopTime = 0.01;
public static final double kTwentyMSLoopTime = 0.02;


private RobotContainer robotContainer = new RobotContainer();



public Robot() {
super(.02);
Expand Down Expand Up @@ -104,8 +99,6 @@ public void robotPeriodic() {
// robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();


}

/** This function is called once each time the robot enters Disabled mode. */
Expand All @@ -114,7 +107,7 @@ public void disabledInit() {}

@Override
public void disabledPeriodic() {

robotContainer.allianceChecker.periodic();
}

Expand Down
17 changes: 6 additions & 11 deletions src/main/java/team3647/frc2024/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,6 @@
import edu.wpi.first.wpilibj2.command.button.Trigger;
import java.util.function.BooleanSupplier;
import team3647.frc2024.auto.AutoCommands;
import team3647.frc2024.auto.AutonomousMode;
import team3647.frc2024.commands.ClimbCommands;
import team3647.frc2024.commands.DrivetrainCommands;
import team3647.frc2024.constants.ChurroConstants;
Expand Down Expand Up @@ -60,8 +59,6 @@
*/
public class RobotContainer {



/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {

Expand All @@ -85,21 +82,22 @@ public RobotContainer() {
configureSmartDashboardLogging();
autoCommands.registerCommands();
configAllianceChecker();

pivot.setEncoder(PivotConstants.kInitialAngle);
wrist.setEncoder(WristConstants.kInitialDegree);
climbLeft.setEncoder(ClimbConstants.kInitialLength);
climbRight.setEncoder(ClimbConstants.kInitialLength);
churro.setEncoder(ChurroConstants.kInitialDegree);
autoChooser.addAutos();
SmartDashboard.putData("Autos",autoChooser);
autoChooser.addAutos();
SmartDashboard.putData("Autos", autoChooser);

RobotController.setBrownoutVoltage(5.5);
}

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

private void configureButtonBindings() {

ledTriggers.inOutTrigger.onTrue(mainController.rumble());
Expand Down Expand Up @@ -473,7 +471,6 @@ public Command getAutonomousCommand() {

public AllianceChecker allianceChecker = new AllianceChecker();


private final VisionController visionController =
new VisionController(
swerve::addVisionData,
Expand Down Expand Up @@ -532,8 +529,6 @@ public Command getAutonomousCommand() {
private final team3647.frc2024.subsystems.LEDs LEDs =
new team3647.frc2024.subsystems.LEDs(LEDConstants.m_candle, ledTriggers);



public final AutoCommands autoCommands =
new AutoCommands(
swerve,
Expand All @@ -547,7 +542,7 @@ public Command getAutonomousCommand() {
detector::hasTarget,
autoDrive::getMode);

public final AutoChooser autoChooser = new AutoChooser(autoCommands, swerve::setRobotPose);
public final AutoChooser autoChooser = new AutoChooser(autoCommands, swerve::setRobotPose);

private final CommandScheduler scheduler = CommandScheduler.getInstance();

Expand Down
10 changes: 9 additions & 1 deletion src/main/java/team3647/frc2024/subsystems/ShooterLeft.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import org.littletonrobotics.junction.Logger;
import team3647.frc2024.util.ModifiedSignalLogger;
import team3647.lib.TalonFXSubsystem;

Expand Down Expand Up @@ -58,7 +59,7 @@ public void setTorque(double torque) {
}

public void setVelocity(double velocity) {
super.setVelocityFOC(velocity, 0);
super.setVelocityVoltage(velocity, 0);
}

public void setVoltage(double voltage) {
Expand All @@ -73,6 +74,13 @@ public boolean velocityGreater(double setpoint) {
return super.getVelocity() > setpoint;
}

@Override
public void periodic() {
super.periodic();
Logger.recordOutput(
"real shoot demadn", super.velocityVoltage.Velocity * velocityConversion);
}

@Override
public String getName() {
return "Shooter Left";
Expand Down
Loading

0 comments on commit 64bbc1c

Please sign in to comment.