PLS5618/robot-2023
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'brasTelescopiquev2' into 'main'

Browse Source 
bras

See merge request pls5618/2023/robot!1
This commit is contained in:
Olivier Demers 2023-03-20 22:28:06 +00:00
commit d4a0188c04
8 changed files with 177 additions and 211 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

171
src/main/java/frc/robot/RobotContainer.java
View File

@ -3,6 +3,7 @@
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import java.util.Map;
import edu.wpi.first.cameraserver.CameraServer;
@ -39,75 +40,63 @@ import frc.robot.commands.Gyro;
import frc.robot.commands.Reculer;
import frc.robot.commands.Tape;
import frc.robot.commands.bras.BrasManuel;
import frc.robot.commands.bras.DescendrePivotBras;
import frc.robot.commands.bras.FermePince;
import frc.robot.commands.bras.MonterPivotBras;
import frc.robot.commands.bras.OuvrePince;
import frc.robot.commands.bras.PivotBrasRentre;
import frc.robot.commands.bras.PivotChercheBas;
import frc.robot.commands.bras.PivotChercheHaut;
import frc.robot.commands.bras.PivotManuel;
import frc.robot.commands.bras.PivoteBrasBas;
import frc.robot.commands.bras.PivoteBrasHaut;
import frc.robot.commands.bras.PivoteBrasMilieux;
public class RobotContainer {
CommandXboxController manette1 = new CommandXboxController(0);
CommandXboxController manette2 = new CommandXboxController(1);
// dashboard
SendableChooser<String> chooser = new SendableChooser<>();
String enhaut = "en haut";
String aumilieux = "au milieux";
String enbas = "en bas";
String nulpart = "nul part";
ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto",BuiltInLayouts.kList)
.withSize(2, 2).withProperties(Map.of("Label position","LEFT"));
GenericEntry autobalance = layoutauto.add("choix balance",true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
GenericEntry autosortir = layoutauto.add("choix sortir",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
GenericEntry reculerdistances = layoutauto.addPersistent("reculerdistancesortir",0).getEntry();
GenericEntry reculerdistanceb = layoutauto.addPersistent("reculerdistancebalance", 0).getEntry();
GenericEntry avancerdistance = layoutauto.addPersistent("avancer",0).getEntry();
CommandXboxController manette1 = new CommandXboxController(0);
CommandXboxController manette2 = new CommandXboxController(1);
// subsystems
BasePilotable basePilotable = new BasePilotable();
Gratte gratte = new Gratte();
BrasTelescopique brasTelescopique = new BrasTelescopique();
Pince pince = new Pince();
Pivot pivot = new Pivot();
Limelight limelight = new Limelight();
// dashboard
SendableChooser<String> chooser = new SendableChooser<>();
String enhaut = "en haut";
String aumilieux = "au milieux";
String enbas = "en bas";
String nulpart = "nul part";
ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto", BuiltInLayouts.kList)
.withSize(2, 2).withProperties(Map.of("Label position", "LEFT"));
GenericEntry autobalance = layoutauto.add("choix balance", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
GenericEntry autosortir = layoutauto.add("choix sortir", false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
GenericEntry reculerdistances = layoutauto.addPersistent("reculerdistancesortir", 0).getEntry();
GenericEntry reculerdistanceb = layoutauto.addPersistent("reculerdistancebalance", 0).getEntry();
GenericEntry avancerdistance = layoutauto.addPersistent("avancer", 0).getEntry();
//commands
BrakeFerme brakeFerme = new BrakeFerme(basePilotable);
BrakeOuvre brakeOuvre = new BrakeOuvre(basePilotable);
GratteBaisser gratteBaisser = new GratteBaisser(gratte);
GratteMonte gratteMonte = new GratteMonte(gratte);
Gyro gyro = new Gyro(basePilotable);
FermePince fermePince = new FermePince(pince);
OuvrePince ouvrePince = new OuvrePince(pince);
PivotBrasRentre pivotBrasRentre = new PivotBrasRentre(brasTelescopique, pivot);
PivoteBrasBas pivoteBrasBas = new PivoteBrasBas(brasTelescopique, pivot);
PivoteBrasMilieux pivoteBrasMilieux = new PivoteBrasMilieux(brasTelescopique, pivot);
PivoteBrasHaut pivoteBrasHaut = new PivoteBrasHaut(brasTelescopique, pivot);
Cone cone = new Cone(limelight, basePilotable, ()->-manette1.getLeftY());
Reculer reculers = new Reculer(basePilotable, ()->reculerdistances.getDouble(0));
Reculer reculerb = new Reculer(basePilotable, ()->reculerdistanceb.getDouble(0));
Avancer avancer = new Avancer(basePilotable, ()->avancerdistance.getDouble(0));
PivotChercheBas pivotChercheBas = new PivotChercheBas(brasTelescopique, pivot);
PivotChercheHaut pivotChercheHaut = new PivotChercheHaut(brasTelescopique, pivot);
Cube cube = new Cube(limelight, basePilotable, null);
Apriltag aprilTag = new Apriltag(limelight, basePilotable, null);
Tape tape = new Tape(limelight, basePilotable, null);
PivotManuel pivotManuel = new PivotManuel(pivot,manette2::getLeftX);
BrasManuel brasManuel = new BrasManuel(brasTelescopique,manette2::getLeftY);
// subsystems
BasePilotable basePilotable = new BasePilotable();
Gratte gratte = new Gratte();
BrasTelescopique brasTelescopique = new BrasTelescopique();
Pince pince = new Pince();
Pivot pivot = new Pivot();
Limelight limelight = new Limelight();
// commands
BrakeFerme brakeFerme = new BrakeFerme(basePilotable);
BrakeOuvre brakeOuvre = new BrakeOuvre(basePilotable);
GratteBaisser gratteBaisser = new GratteBaisser(gratte);
GratteMonte gratteMonte = new GratteMonte(gratte);
Gyro gyro = new Gyro(basePilotable);
FermePince fermePince = new FermePince(pince);
OuvrePince ouvrePince = new OuvrePince(pince);
Cone cone = new Cone(limelight, basePilotable, () -> -manette1.getLeftY());
Reculer reculers = new Reculer(basePilotable, () -> reculerdistances.getDouble(0));
Reculer reculerb = new Reculer(basePilotable, () -> reculerdistanceb.getDouble(0));
Avancer avancer = new Avancer(basePilotable, () -> avancerdistance.getDouble(0));
Cube cube = new Cube(limelight, basePilotable, () -> manette1.getLeftY());
Apriltag aprilTag = new Apriltag(limelight, basePilotable, () -> manette1.getLeftY());
Tape tape = new Tape(limelight, basePilotable, () -> manette1.getLeftY());
PivotManuel pivotManuel = new PivotManuel(pivot, manette2::getLeftX);
BrasManuel brasManuel = new BrasManuel(brasTelescopique, manette2::getLeftY);
public RobotContainer() {
public RobotContainer() {
chooser.addOption(enhaut, enhaut);
chooser.addOption(enbas, enbas);
chooser.addOption(aumilieux, aumilieux);
chooser.addOption(nulpart, nulpart);
layoutauto.add("choix hauteur",chooser);
layoutauto.add("choix hauteur", chooser);
configureBindings();
CameraServer.startAutomaticCapture();
basePilotable.setDefaultCommand(new RunCommand(() -> {
@ -116,48 +105,58 @@ public RobotContainer() {
brasTelescopique.setDefaultCommand(brasManuel);
pivot.setDefaultCommand(pivotManuel);
}
private void configureBindings() {
// manette 1
manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer,pince));
manette1.x().toggleOnTrue(Commands.startEnd(basePilotable::BrakeFerme,basePilotable::BrakeOuvre,basePilotable));
manette1.leftBumper().toggleOnTrue(aprilTag);
manette1.rightBumper().toggleOnTrue(tape);
manette1.povUp().whileTrue(pivoteBrasHaut);
manette1.povDown().whileTrue(pivoteBrasBas);
manette1.povRight().whileTrue(pivoteBrasMilieux);
manette1.povLeft().whileTrue(pivotBrasRentre);
//manette 2
manette2.povDown().onTrue(pivotChercheBas);
manette2.povUp().onTrue(pivotChercheHaut);
manette2.rightBumper().toggleOnTrue(cube);
manette2.leftBumper().toggleOnTrue(cone);
manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer, pince));
manette1.x().onTrue(brakeOuvre);
manette1.b().onTrue(brakeFerme);
manette1.leftBumper().whileTrue(aprilTag);
manette1.rightBumper().whileTrue(tape);
manette1.povUp().whileTrue(creerCommandBras(51, -40));
manette1.povDown().whileTrue(creerCommandBras(9, -14));
manette1.povRight().whileTrue(creerCommandBras(44, -17));
manette1.povLeft().whileTrue(creerCommandBras(0, 0));
// manette 2
manette2.povDown().onTrue(creerCommandBras(9, -18));
manette2.povUp().onTrue(creerCommandBras(44, 0));
manette2.rightBumper().whileTrue(cube);
manette2.leftBumper().whileTrue(cone);
manette2.y().whileTrue(gyro);
manette2.start().onTrue(new InstantCommand(basePilotable::resetGyro));
manette2.a().whileTrue(gratteMonte);
manette2.b().whileTrue(gratteBaisser);
}
//mode auto
private Command creerCommandBras(double distancePivot, double distanceBras) {
return Commands.either(
new MonterPivotBras(brasTelescopique, pivot, distanceBras, distancePivot),
new DescendrePivotBras(brasTelescopique, pivot, distanceBras, distancePivot),
() -> pivot.distance() < distancePivot);
}
public Command getAutonomousCommand() {
chooser.getSelected();
autobalance.getBoolean(true);
return new SequentialCommandGroup(
Commands.select(Map.ofEntries(
Map.entry(enhaut,pivoteBrasHaut),
Map.entry(aumilieux,pivoteBrasMilieux),
Map.entry(enbas,pivoteBrasBas),
Map.entry(nulpart,pivotBrasRentre)
), chooser::getSelected),
new OuvrePince(pince).unless(()->chooser.getSelected().equals(nulpart)),
Commands.waitSeconds(1),
new FermePince(pince).unless(()->chooser.getSelected().equals(nulpart)),
new PivotBrasRentre(brasTelescopique , pivot).unless(()->chooser.getSelected().equals(nulpart)),
Commands.waitSeconds(1),
Commands.either(reculers, reculerb,()-> autosortir.getBoolean(true)),
new Avancer(basePilotable,()-> avancerdistance.getDouble(0)).unless(()->!autosortir.getBoolean(true)|| autobalance.getBoolean(false)),
Commands.either(gyro, Commands.none(),()-> autobalance.getBoolean(true))
).deadlineWith(Commands.waitSeconds(14.6)).andThen(brakeOuvre);
return Commands.deadline(
Commands.waitSeconds(14.6),
new SequentialCommandGroup(
Commands.select(Map.ofEntries(
Map.entry(enhaut, creerCommandBras(51, -40)),
Map.entry(aumilieux, creerCommandBras(9, -14)),
Map.entry(enbas, creerCommandBras(44, -17)),
Map.entry(nulpart, creerCommandBras(0, 0))), chooser::getSelected),
new OuvrePince(pince).unless(() -> chooser.getSelected().equals(nulpart)),
Commands.waitSeconds(1),
fermePince.unless(() -> chooser.getSelected().equals(nulpart)),
creerCommandBras(0, 0).unless(() -> chooser.getSelected().equals(nulpart)),
Commands.waitSeconds(1),
Commands.either(reculers, reculerb, () -> autosortir.getBoolean(true)),
new Avancer(basePilotable, () -> avancerdistance.getDouble(0))
.unless(() -> !autosortir.getBoolean(true) || !autobalance.getBoolean(false)),
Commands.either(gyro, Commands.none(), () -> autobalance.getBoolean(true))))
.andThen(brakeOuvre);
}
}

45
src/main/java/frc/robot/commands/bras/PivotChercheHaut.java → src/main/java/frc/robot/commands/bras/Bougerbras.java
View File

@ -6,45 +6,36 @@ package frc.robot.commands.bras;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.bras.BrasTelescopique;
import frc.robot.subsystems.bras.Pivot;
public class PivotChercheHaut extends CommandBase {
private BrasTelescopique brasTelescopique;
private Pivot pivot;
/** Creates a new PivotChercheHaut. */
public PivotChercheHaut(BrasTelescopique brasTelescopique, Pivot pivot) {
public class Bougerbras extends CommandBase {
/** Creates a new bougerbras. */
double distance;
BrasTelescopique brasTelescopique;
public Bougerbras(BrasTelescopique brasTelescopique,double distance) {
this.brasTelescopique = brasTelescopique;
this.pivot = pivot;
// Use addRequirements() here to declare subsystem dependencies.
this.distance = distance;
addRequirements(brasTelescopique);
addRequirements(pivot);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
brasTelescopique.fermer();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(brasTelescopique.photocell()){
brasTelescopique.ouvrir();
brasTelescopique.Reset();
brasTelescopique.AvanceRecule(0);
}
else{
if(brasTelescopique.distance()>distance+0.5 ) {
brasTelescopique.AvanceRecule(-0.3);
}
else if(brasTelescopique.distance()<distance-0.5) {
brasTelescopique.AvanceRecule(0.3);
}
if (pivot.distance()<43.5){
pivot.monteDescendre(0.5);
}
else if(pivot.distance()>44.5) {
pivot.monteDescendre(-0.5);
}
else{
pivot.monteDescendre(0);
else {
brasTelescopique.AvanceRecule(0);
}
}
@ -52,13 +43,13 @@ public class PivotChercheHaut extends CommandBase {
@Override
public void end(boolean interrupted) {
brasTelescopique.AvanceRecule(0);
pivot.monteDescendre(0);
brasTelescopique.ouvrir();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
return brasTelescopique.distance()>distance-0.5 && brasTelescopique.distance()<distance+0.5;
}
}

49
src/main/java/frc/robot/commands/bras/PivotBrasRentre.java → src/main/java/frc/robot/commands/bras/Bougerpivot.java
View File

@ -5,58 +5,47 @@
package frc.robot.commands.bras;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.bras.BrasTelescopique;
import frc.robot.subsystems.bras.Pivot;
public class PivotBrasRentre extends CommandBase {
private BrasTelescopique brasTelescopique;
private Pivot pivot;
/** Creates a new PivotBrasRentre. */
public PivotBrasRentre(BrasTelescopique brasTelescopique, Pivot pivot) {
this.brasTelescopique = brasTelescopique;
public class Bougerpivot extends CommandBase {
/** Creates a new Bougerpivot. */
Pivot pivot;
double distance;
public Bougerpivot(Pivot pivot,double distance) {
this.pivot = pivot;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(brasTelescopique);
this.distance = distance;
addRequirements(pivot);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
brasTelescopique.fermer();
}
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(brasTelescopique.photocell()){
brasTelescopique.ouvrir();
brasTelescopique.Reset();
brasTelescopique.AvanceRecule(0);
}
else{
brasTelescopique.AvanceRecule(0.5);
}
if(pivot.limitpivot()){
pivot.Reset();
pivot.monteDescendre(0);
}
else{
if(pivot.distance()>distance+0.5 ) {
pivot.monteDescendre(-0.5);
}
else if(pivot.distance()<distance-0.5) {
pivot.monteDescendre(0.5);
}
else {
pivot.monteDescendre(0);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
brasTelescopique.AvanceRecule(0);
pivot.monteDescendre(0);
brasTelescopique.ouvrir();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return brasTelescopique.photocell() && pivot.limitpivot();
}
return pivot.distance()>distance-0.5 && pivot.distance()<distance+0.5;
}
}

25
src/main/java/frc/robot/commands/bras/DescendrePivotBras.java Normal file
View File

@ -0,0 +1,25 @@
// 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.bras;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.subsystems.bras.BrasTelescopique;
import frc.robot.subsystems.bras.Pivot;
// 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 DescendrePivotBras extends SequentialCommandGroup {
/** Creates a new DescendrePivotBras. */
public DescendrePivotBras(BrasTelescopique brasTelescopique, Pivot pivot, double distanceBras, double distancePivot) {
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
addCommands(
new Bougerbras(brasTelescopique, distanceBras),
new Bougerpivot(pivot, distancePivot)
);
}
}

28
src/main/java/frc/robot/commands/bras/MonterPivotBras.java Normal file
View File

@ -0,0 +1,28 @@
// 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.bras;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.subsystems.bras.BrasTelescopique;
import frc.robot.subsystems.bras.Pivot;
// 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 MonterPivotBras extends ParallelCommandGroup {
/** Creates a new Sequancepivotbras. */
public MonterPivotBras(BrasTelescopique brasTelescopique, Pivot pivot, double distanceBras, double distancePivot) {
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
addCommands(
new SequentialCommandGroup(Commands.waitUntil(()->pivot.distance()>8),new Bougerbras(brasTelescopique, distanceBras)),
new Bougerpivot(pivot, distancePivot)
);
}
}

67
src/main/java/frc/robot/commands/bras/PivotChercheBas.java
View File

@ -1,67 +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.commands.bras;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.bras.BrasTelescopique;
import frc.robot.subsystems.bras.Pivot;
public class PivotChercheBas extends CommandBase {
private BrasTelescopique brasTelescopique;
private Pivot pivot;
/** Creates a new PivotChercheBas. */
public PivotChercheBas(BrasTelescopique brasTelescopique, Pivot pivot) {
this.brasTelescopique = brasTelescopique;
this.pivot = pivot;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(brasTelescopique);
addRequirements(pivot);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
brasTelescopique.fermer();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(brasTelescopique.distance()>-17.5){
brasTelescopique.AvanceRecule(-0.2);
brasTelescopique.fermer();
}
else if(brasTelescopique.distance()<-19.5) {
brasTelescopique.AvanceRecule(0.2);
}
else {
brasTelescopique.AvanceRecule(0);
brasTelescopique.ouvrir();
}
if (pivot.distance()<8.5){
pivot.monteDescendre(0.3);
}
else if(pivot.distance()>10.5) {
pivot.monteDescendre(-0.3);
}
else{
pivot.monteDescendre(0);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
brasTelescopique.AvanceRecule(0);
pivot.monteDescendre(0);
brasTelescopique.fermer();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

2
src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java
View File

@ -51,6 +51,6 @@ public class BrasTelescopique extends SubsystemBase {
}
@Override
public void periodic() {
if(photocell())Reset();
}
}

1
src/main/java/frc/robot/subsystems/bras/Pivot.java
View File

@ -42,6 +42,7 @@ public class Pivot extends SubsystemBase {
}
@Override
public void periodic() {
if(limitpivot())Reset();
}