fuck les pistons
This commit is contained in:
parent
7e8c930103
commit
34472de0fd
@ -30,8 +30,6 @@ import frc.robot.command.LancerNote;
|
|||||||
import frc.robot.command.LancerTrape;
|
import frc.robot.command.LancerTrape;
|
||||||
import frc.robot.command.Lancerampli;
|
import frc.robot.command.Lancerampli;
|
||||||
import frc.robot.command.Limelight_tracker;
|
import frc.robot.command.Limelight_tracker;
|
||||||
import frc.robot.command.PistonFerme;
|
|
||||||
import frc.robot.command.PistonOuvre;
|
|
||||||
import frc.robot.command.RestGyro;
|
import frc.robot.command.RestGyro;
|
||||||
// Subsystems
|
// Subsystems
|
||||||
import frc.robot.subsystem.Accumulateur;
|
import frc.robot.subsystem.Accumulateur;
|
||||||
@ -59,7 +57,6 @@ public class RobotContainer {
|
|||||||
CommandXboxController manette = new CommandXboxController(1);
|
CommandXboxController manette = new CommandXboxController(1);
|
||||||
|
|
||||||
//command
|
//command
|
||||||
PistonFerme pistonFerme = new PistonFerme(grimpeur);
|
|
||||||
Balayer balayer = new Balayer(balayeuse, accumulateur);
|
Balayer balayer = new Balayer(balayeuse, accumulateur);
|
||||||
GuiderHaut guiderHaut = new GuiderHaut(guideur);
|
GuiderHaut guiderHaut = new GuiderHaut(guideur);
|
||||||
GuiderBas guiderBas = new GuiderBas(guideur);
|
GuiderBas guiderBas = new GuiderBas(guideur);
|
||||||
@ -82,15 +79,12 @@ public class RobotContainer {
|
|||||||
|
|
||||||
NamedCommands.registerCommand("balayer",new Balayer(balayeuse, accumulateur));
|
NamedCommands.registerCommand("balayer",new Balayer(balayeuse, accumulateur));
|
||||||
NamedCommands.registerCommand("lancer", new LancerNote(lanceur, accumulateur));
|
NamedCommands.registerCommand("lancer", new LancerNote(lanceur, accumulateur));
|
||||||
NamedCommands.registerCommand("piston", new PistonFerme(grimpeur));
|
|
||||||
autoChooser = AutoBuilder.buildAutoChooser();
|
autoChooser = AutoBuilder.buildAutoChooser();
|
||||||
|
|
||||||
//manette
|
//manette
|
||||||
manette.start().whileTrue(new RestGyro(drive));
|
manette.start().whileTrue(new RestGyro(drive));
|
||||||
manette.a().whileTrue(new GuiderBas(guideur));
|
manette.a().whileTrue(new GuiderBas(guideur));
|
||||||
manette.b().whileTrue(new GuiderHaut(guideur));
|
manette.b().whileTrue(new GuiderHaut(guideur));
|
||||||
manette.x().whileTrue(new PistonFerme(grimpeur));
|
|
||||||
manette.y().whileTrue(new PistonOuvre(grimpeur));
|
|
||||||
|
|
||||||
//joystick
|
//joystick
|
||||||
joystick.button(3).whileTrue(new Balayer(balayeuse, accumulateur));
|
joystick.button(3).whileTrue(new Balayer(balayeuse, accumulateur));
|
||||||
|
@ -36,7 +36,6 @@ public class GrimpeurDroit extends Command {
|
|||||||
@Override
|
@Override
|
||||||
public void end(boolean interrupted) {
|
public void end(boolean interrupted) {
|
||||||
grimpeur.droit(0);
|
grimpeur.droit(0);
|
||||||
grimpeur.pistonouvre();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Returns true when the command should end.
|
// Returns true when the command should end.
|
||||||
|
@ -1,40 +0,0 @@
|
|||||||
// 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.command;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystem.Grimpeur;
|
|
||||||
|
|
||||||
public class PistonFerme extends Command {
|
|
||||||
private Grimpeur grimpeur;
|
|
||||||
/** Creates a new PistonFerme. */
|
|
||||||
public PistonFerme(Grimpeur grimpeur) {
|
|
||||||
this.grimpeur = grimpeur;
|
|
||||||
addRequirements(grimpeur);
|
|
||||||
// Use addRequirements() here to declare subsystem dependencies.
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called when the command is initially scheduled.
|
|
||||||
@Override
|
|
||||||
public void initialize() {
|
|
||||||
grimpeur.pistonouvre();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called every time the scheduler runs while the command is scheduled.
|
|
||||||
@Override
|
|
||||||
public void execute() {}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
grimpeur.pistonferme();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
}
|
|
@ -1,40 +0,0 @@
|
|||||||
// 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.command;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystem.Grimpeur;
|
|
||||||
|
|
||||||
public class PistonOuvre extends Command {
|
|
||||||
private Grimpeur grimpeur;
|
|
||||||
/** Creates a new PistonFerme. */
|
|
||||||
public PistonOuvre(Grimpeur grimpeur) {
|
|
||||||
this.grimpeur = grimpeur;
|
|
||||||
addRequirements(grimpeur);
|
|
||||||
// Use addRequirements() here to declare subsystem dependencies.
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called when the command is initially scheduled.
|
|
||||||
@Override
|
|
||||||
public void initialize() {
|
|
||||||
grimpeur.pistonferme();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called every time the scheduler runs while the command is scheduled.
|
|
||||||
@Override
|
|
||||||
public void execute() {}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
grimpeur.pistonouvre();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
}
|
|
@ -4,7 +4,6 @@
|
|||||||
|
|
||||||
package frc.robot.subsystem;
|
package frc.robot.subsystem;
|
||||||
|
|
||||||
import com.kauailabs.navx.frc.AHRS;
|
|
||||||
import com.revrobotics.CANSparkMax;
|
import com.revrobotics.CANSparkMax;
|
||||||
import com.revrobotics.CANSparkLowLevel.MotorType;
|
import com.revrobotics.CANSparkLowLevel.MotorType;
|
||||||
import edu.wpi.first.wpilibj.PneumaticsModuleType;
|
import edu.wpi.first.wpilibj.PneumaticsModuleType;
|
||||||
@ -31,10 +30,8 @@ public class Grimpeur extends SubsystemBase {
|
|||||||
final Solenoid pistondroite= new Solenoid(PneumaticsModuleType.REVPH, Constants.pistondroite);
|
final Solenoid pistondroite= new Solenoid(PneumaticsModuleType.REVPH, Constants.pistondroite);
|
||||||
//fonction
|
//fonction
|
||||||
public Grimpeur() {
|
public Grimpeur() {
|
||||||
pistonouvre();
|
|
||||||
layout.addDouble("grimpeurencodeurd", this::encoderd);
|
layout.addDouble("grimpeurencodeurd", this::encoderd);
|
||||||
layout.addDouble("grimpeurencodeurg", this::encoderg);
|
layout.addDouble("grimpeurencodeurg", this::encoderg);
|
||||||
layout.addDouble("pitchgyrogrimpeur", this::getpitch);
|
|
||||||
}
|
}
|
||||||
public void droit(double vitesse){
|
public void droit(double vitesse){
|
||||||
grimpeurd.set(vitesse);
|
grimpeurd.set(vitesse);
|
||||||
@ -54,19 +51,6 @@ public double encoderd(){
|
|||||||
public double encoderg(){
|
public double encoderg(){
|
||||||
return grimpeurg.getEncoder().getPosition();
|
return grimpeurg.getEncoder().getPosition();
|
||||||
}
|
}
|
||||||
public AHRS gyroscope = new AHRS();
|
|
||||||
public double getpitch(){
|
|
||||||
return gyroscope.getPitch();
|
|
||||||
}
|
|
||||||
public void pistonferme(){
|
|
||||||
pistondroite.set(true);
|
|
||||||
}
|
|
||||||
public void pistonouvre(){
|
|
||||||
pistondroite.set(false);
|
|
||||||
}
|
|
||||||
public boolean piston(){
|
|
||||||
return pistondroite.get();
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void periodic() {
|
public void periodic() {
|
||||||
|
Loading…
x
Reference in New Issue
Block a user