180 ok

Scorpions-Robotics · Mar 19, 2024 · 03edb7f · 03edb7f
1 parent 3fbd955
commit 03edb7f
Show file tree

Hide file tree

Showing 5 changed files with 78 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -25,7 +25,9 @@ public void robotInit() {
     m_robotContainer = new RobotContainer();
     auto_chooser.setDefaultOption("1 Note", 0);
     auto_chooser.setDefaultOption("2 Note", 1);
-    auto_chooser.addOption("3 Note", 2);
+    auto_chooser.addOption("3 Note For RED", 2);
+    auto_chooser.addOption("3 Note For BLUE", 3);
+
     SmartDashboard.putData(auto_chooser);
   }
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -44,6 +44,7 @@
 import frc.robot.commands.Intake.RunTillSwitch;
 import frc.robot.commands.Shooter.PidSetRpm;
 import frc.robot.commands.Shooter.ShooterModeChange;
+import frc.robot.commands.Shooter.ShooterSet180;
 import frc.robot.commands.Shooter.ShooterSetDegree;
 import frc.robot.commands.Shooter.TurnForShooter;
 import frc.robot.commands.Shooter.VisionShooter;
@@ -174,8 +175,8 @@ private void configureBindings() {
 new JoystickButton(subJoytick, 4).whileFalse(new InstantCommand(()-> m_shooter.ShooterThrow1MotorStop()));
  new JoystickButton(subJoytick, 4).whileFalse(new InstantCommand(()-> m_shooter.ShooterThrow2MotorStop()));
 //button 5 Amfi 
- new JoystickButton(subJoytick, 5).whileTrue(new ShooterSetDegree(m_shooter, ()->180.0));
- new JoystickButton(subJoytick, 6).whileTrue(new ShooterSetDegree(m_shooter, ()->70.0));
+ new JoystickButton(subJoytick, 5).whileTrue(new ShooterSet180(m_shooter));
+ new JoystickButton(subJoytick, 6).whileTrue(new ShooterSetDegree(m_shooter, ()->73.0));
  new JoystickButton(subJoytick, 6).whileTrue(new InstantCommand(()-> m_shooter.ShooterThrowMotorOutput(-1)));
  new JoystickButton(subJoytick, 6).whileFalse(new InstantCommand(()-> m_shooter.ShooterThrow1MotorStop()));
  new JoystickButton(subJoytick, 6).whileFalse(new InstantCommand(()-> m_shooter.ShooterThrow2MotorStop()));
@@ -388,6 +389,16 @@ public Command getAutonomousCommand(int mode) {
                           new Pose2d(-0.1,0, new Rotation2d(0)),
                         trajectoryConfig);
 
+
+                        var note3pathforblue =
+                        TrajectoryGenerator.generateTrajectory(
+                          new Pose2d(0, 0, new Rotation2d(0)),
+                          List.of(new Translation2d(-0.5, 1),new Translation2d(-1.0, 1.6),
+                          new Translation2d(-1.58, 1.6)
+                          ),
+                          new Pose2d(-1.28,0,new Rotation2d(0)),
+                        trajectoryConfig);
+
                 var note3path =
                         TrajectoryGenerator.generateTrajectory(
                           new Pose2d(0, 0, new Rotation2d(0)),
@@ -477,7 +488,30 @@ public Command getAutonomousCommand(int mode) {
         //        .andThen(new InstantCommand(()-> m_shooter.throwStop()))
         //        .andThen(new InstantCommand(()-> m_intake.StopNoteMotor()));
 
-               if(mode == 2){
+        if(mode == 3){
+                return new AutoNoteShoot(m_shooter, m_feeder, () -> 64).withTimeout(2)
+                .andThen(pathCommand(trajectoryOne).alongWith(new pickupforAuto(m_intake, m_joystick).withTimeout(3.75))
+                .withTimeout(4.75)
+                .andThen(new InstantCommand(()-> m_intake.StopNoteMotor())
+                .andThen(new InstantCommand(()-> swerveSubsystem.stopModules()))))
+                .andThen(new otofeedleme(m_intake, m_feeder, m_joystick, m_shooter)).withTimeout(6.75)
+                .andThen(new InstantCommand(()-> m_intake.StopNoteMotor()))
+                .andThen(new AutoNoteShoot(m_shooter, m_feeder, ()-> 70)).withTimeout(8)
+                .andThen(pathCommand(note3pathforblue).alongWith(new pickupforAuto(m_intake, m_joystick).withTimeout(9.5))
+                .alongWith(new InstantCommand(()-> m_intake.StopNoteMotor())
+                .andThen(new InstantCommand(()-> m_shooter.throwStop()))
+                .andThen(new InstantCommand(()-> swerveSubsystem.stopModules())
+                .withTimeout(10.5)))).
+                withTimeout(11)
+                .andThen(new InstantCommand(()-> swerveSubsystem.stopModules()))
+                .andThen(new otofeedleme(m_intake, m_feeder, m_joystick, m_shooter)).withTimeout(13.5)
+                .andThen(new AutoNoteShoot(m_shooter,m_feeder,() -> 47))
+                .withTimeout(14.6)
+                .andThen(new InstantCommand(()-> m_shooter.throwStop()))
+                .andThen(new InstantCommand(()-> m_intake.StopNoteMotor()));
+               }     
+
+        else if(mode == 2){
                 return new AutoNoteShoot(m_shooter, m_feeder, () -> 64).withTimeout(2)
                 .andThen(pathCommand(trajectoryOne).alongWith(new pickupforAuto(m_intake, m_joystick).withTimeout(3.75))
                 .withTimeout(4.75)

diff --git a/src/main/java/frc/robot/commands/Shooter/ShooterSet180.java b/src/main/java/frc/robot/commands/Shooter/ShooterSet180.java
@@ -0,0 +1,33 @@
+package frc.robot.commands.Shooter;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+import frc.robot.Constants;
+import frc.robot.subsystems.ShooterSubsystem;
+
+public class ShooterSet180 extends PIDCommand {
+
+  public ShooterSet180(ShooterSubsystem m_shooter) {
+    super(
+
+       // new PIDController(0.04,
+        new PIDController(0.035,
+            0.003,
+            Constants.values.shooter.PidShooterAngleKD),
+        () -> m_shooter.getMappedOutput(),
+        () -> 180,
+        output -> {
+
+          m_shooter.ShooterAngleMotorOutput(output * -.13);
+
+        });
+    addRequirements(m_shooter);
+    getController().setTolerance(Constants.values.shooter.PidShooterAngleTolerance);
+
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/Shooter/ShooterSetDegree.java b/src/main/java/frc/robot/commands/Shooter/ShooterSetDegree.java
@@ -14,6 +14,8 @@ public ShooterSetDegree(ShooterSubsystem m_shooter, DoubleSupplier angle) {
     super(
 
         new PIDController(0.04,
+        //new PIDController(0.025,
+
             0.03,
             Constants.values.shooter.PidShooterAngleKD),
         () -> m_shooter.getMappedOutput(),

diff --git a/src/main/java/frc/robot/commands/Shooter/VisionShooter.java b/src/main/java/frc/robot/commands/Shooter/VisionShooter.java
@@ -27,7 +27,9 @@ public VisionShooter(ShooterSubsystem m_shooter, NetworkSubsystem m_network) {
             e.printStackTrace();
         }*/
 
-  m_shooter.ShooterAngleMotorOutput(output * .13);
+        m_shooter.ShooterAngleMotorOutput(output * .13);
+
+ // m_shooter.ShooterAngleMotorOutput(output * .09);
 });
 
     addRequirements(m_shooter);