PID + Shooter

Co-Authored-By: Nahuelpoet <[email protected]>

src/main/java/frc/robot/Constants.java

@@ -22,6 +22,9 @@ public static class OperatorConstants {
 
     public final int DeviceNumberClimberDroit = 0;
     public final int DeviceNumberClimberGauche = 0;
+
+    public final int DeviceNumberShooterGauche = 0;
+    public final int DeviceNumberShooterDroit = 0;
   }
 
   public static class PCM{

src/main/java/frc/robot/RobotContainer.java

@@ -10,11 +10,12 @@
 import frc.robot.commands.ClimberInABoxCommand;
 import frc.robot.commands.OctocanumDrivetrainCommand;
 import frc.robot.commands.ResetGryoCommand;
+import frc.robot.commands.ShooterActivateCommand;
+import frc.robot.commands.ShooterDisactivateCommand;
 import frc.robot.subsystems.ClimberInAnBox;
 import frc.robot.subsystems.DriveTrainSwitch;
 import frc.robot.subsystems.Drivetrain;
-
-
+import frc.robot.subsystems.Shooter;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
@@ -32,6 +33,7 @@ public class RobotContainer {
 private Drivetrain drivetrain;
 private DriveTrainSwitch driveTrainSwitch;
 private ClimberInAnBox climberInAnBox;
+private Shooter shooter;
 private CommandXboxController xboxController = new CommandXboxController(0);
 private CommandXboxController xboxController1 = new CommandXboxController(1);
 
@@ -65,6 +67,9 @@ private void configureBindings() {
     xboxController.a().onTrue(new ActivateDrivetrainCommand(driveTrainSwitch, drivetrain));
     xboxController.b().onTrue(new ActivateMecanumCommand(driveTrainSwitch, drivetrain));
     xboxController.x().onTrue(new ResetGryoCommand(drivetrain));
+    xboxController1.rightBumper().onTrue(new ShooterActivateCommand(shooter));
+    xboxController1.rightBumper().onFalse(new ShooterDisactivateCommand(shooter));
+
 
  }
 

src/main/java/frc/robot/commands/AvancerXmCommand.java

@@ -6,6 +6,7 @@
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.PIDCommand;
+import frc.robot.subsystems.DriveTrainSwitch;
 import frc.robot.subsystems.Drivetrain;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
@@ -15,28 +16,32 @@
 
 public class AvancerXmCommand extends PIDCommand {
   private Drivetrain drivetrain;
+  public DriveTrainSwitch driveTrainSwitch;
 
 
 
   /** Creates a new Avancer1mCommand. */
-  public AvancerXmCommand(Drivetrain drivetrain, double metre) {
+  public AvancerXmCommand(Drivetrain drivetrain,DriveTrainSwitch driveTrainSwitch, double metre) {
 
     super(
         // The controller that the command will use
         new PIDController(1, 0, 0),
         // This should return the measurement
-        () -> encoderValue[], // TODO erreur ici
+        () -> drivetrain.getEncoder()[0], // TODO erreur ici
         // This should return the setpoint (can also be a constant)
         () -> metre / 0.058,
         // This uses the output
         output -> {
-          // Use the output here
+          drivetrain.drive(output, 0, output, 0, 0, 0);
         });
     // Use addRequirements() here to declare subsystem dependencies.
 
     this.drivetrain = drivetrain;
-    double[] encoderValue = drivetrain.getEncoder();
-addRequirements(drivetrain);
+    this.driveTrainSwitch = driveTrainSwitch;
+    driveTrainSwitch.ActivateDrivetank();
+    drivetrain.setDriveMode(true);
+
+  addRequirements(drivetrain);
     // Configure additional PID options by calling `getController` here.
   }
 

src/main/java/frc/robot/commands/ShooterActivateCommand.java

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.Shooter;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class 
+ShooterActivateCommand extends InstantCommand {
+  private Shooter shooter;
+  public ShooterActivateCommand(Shooter shooter) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.shooter = shooter;
+    addRequirements(shooter);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    shooter.shoot(1);
+  }
+}

src/main/java/frc/robot/commands/ShooterDisactivateCommand.java

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.Shooter;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class ShooterDisactivateCommand extends InstantCommand {
+  private Shooter shooter;
+  public ShooterDisactivateCommand(Shooter shooter) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.shooter = shooter;
+    addRequirements(shooter);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    shooter.shoot(0);
+  }
+}

src/main/java/frc/robot/subsystems/Shooter.java

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import com.ctre.phoenix.motorcontrol.TalonSRXControlMode;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.OperatorConstants;
+public class Shooter extends SubsystemBase {
+  /** Creates a new Shooter. */
+  private TalonSRX shooterGauche;
+  private TalonSRX shooterDroit;
+  public Shooter() {
+    OperatorConstants deviceNumber = new OperatorConstants();
+    shooterGauche = new TalonSRX(deviceNumber.DeviceNumberShooterGauche);
+    shooterDroit = new TalonSRX(deviceNumber.DeviceNumberShooterDroit);
+
+    shooterGauche.setInverted(false);
+    shooterDroit.setInverted(false);
+
+  }
+  public void shoot(double activated){
+    shooterGauche.set(TalonSRXControlMode.PercentOutput, activated);
+    shooterDroit.set(TalonSRXControlMode.PercentOutput, activated);
+
+
+  }
+
+  @Override
+  public void periodic() {
+
+    // This method will be called once per scheduler run
+  }
+}