diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a7ac5fe..f77f02a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.ActivateDrivetrainCommand; import frc.robot.commands.ActivateMecanumCommand; +import frc.robot.commands.AvancerXmCommand; import frc.robot.commands.ClimberInABoxCommand; import frc.robot.commands.OctocanumDrivetrainCommand; import frc.robot.commands.ResetGryoCommand; @@ -64,6 +65,7 @@ private void configureBindings() { xboxController.x().onTrue(new ResetGryoCommand(drivetrain)); xboxController1.rightBumper().onTrue(new ShooterActivateCommand(shooter)); xboxController1.rightBumper().onFalse(new ShooterDisactivateCommand(shooter)); + xboxController1.a().onTrue(new AvancerXmCommand(drivetrain, driveTrainSwitch, 1)); } /** diff --git a/src/main/java/frc/robot/commands/GetShooterToSpeedCommand.java b/src/main/java/frc/robot/commands/GetShooterToSpeedCommand.java new file mode 100644 index 0000000..2ca4c5c --- /dev/null +++ b/src/main/java/frc/robot/commands/GetShooterToSpeedCommand.java @@ -0,0 +1,38 @@ +// 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.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.PIDCommand; +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 GetShooterToSpeedCommand extends PIDCommand { + /** Creates a new GetShooterToSpeedCommand. */ + public Shooter shooter; + public GetShooterToSpeedCommand(Shooter shooter, double targetRPM) { + super( + // The controller that the command will use + new PIDController(0, 0, 0), + // This should return the measurement + () -> shooter.getRPM(), + // This should return the setpoint (can also be a constant) + () -> targetRPM, + // This uses the output + output -> { + shooter.shoot(output);// Use the output here + }); + this.shooter = shooter; + addRequirements(shooter); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index eec9867..21ba0a1 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -4,30 +4,63 @@ package frc.robot.subsystems; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.OperatorConstants; public class Shooter extends SubsystemBase { /** Creates a new Shooter. */ - private Talon shooterGauche; - - private Talon shooterDroit; - + private CANSparkMax shooterGauche; + private CANSparkMax shooterDroit; + private RelativeEncoder shooterDroitEncoder; + private RelativeEncoder shooterGaucheEncoder; public Shooter() { OperatorConstants deviceNumber = new OperatorConstants(); - shooterGauche = new Talon(deviceNumber.DeviceNumberShooterGauche); - shooterDroit = new Talon(deviceNumber.DeviceNumberShooterDroit); + shooterGauche = new CANSparkMax(deviceNumber.DeviceNumberShooterGauche, MotorType.kBrushless); + shooterDroit = new CANSparkMax(deviceNumber.DeviceNumberShooterDroit, MotorType.kBrushless); shooterGauche.setInverted(false); shooterDroit.setInverted(false); + + } - public void shoot(double activated) { - shooterGauche.set(activated); - shooterDroit.set(activated); + public void shoot(double power) { + shooterGauche.set(power); + shooterDroit.set(power); } + public double[] getEncoder() { + double[] encoderValue = { + shooterGaucheEncoder.getPosition(), + shooterDroitEncoder.getPosition() + + }; + return encoderValue; + } + public double getRPM(){ + double startposition = shooterGaucheEncoder.getPosition(); + while(shooterGaucheEncoder.getPosition()< startposition+0.058 || shooterGaucheEncoder.getPosition() > startposition+0.058){ + + /* TODO: Regle cela (manque de temps) + + i = i + 1 + wait(10) + i * 10 * 100 * 60 + + */ + + + } + + //TODO find a way to get speed with rpm + return 1.0; + } @Override public void periodic() {