diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 18b8937..613b4df 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,8 +3,13 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -14,10 +19,22 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.subsystems.Drive; public class RobotContainer { + ShuffleboardTab dashboard = Shuffleboard.getTab("Dashboard"); + ShuffleboardLayout forces = Shuffleboard.getTab("Dashboard") + .getLayout("limitswitchsgratte", BuiltInLayouts.kList) + .withSize(3, 7); + GenericEntry force1 = forces.add("Force1", 0).getEntry(); + GenericEntry force2 = forces.add("Force2", 0).getEntry(); + GenericEntry force3 = forces.add("Force3", 0).getEntry(); + GenericEntry force4 = forces.add("Force4", 0).getEntry(); + GenericEntry force5 = forces.add("Force5", 0).getEntry(); + GenericEntry force6 = forces.add("Force6", 0).getEntry(); + GenericEntry force7 = forces.add("Force7", 0).getEntry(); CommandXboxController manette = new CommandXboxController(0); Joystick joystick1 = new Joystick(0); Drive drive = new Drive(); public RobotContainer() { + configureBindings(); // drive.setDefaultCommand(new RunCommand(()->{ // drive.drive(manette.getLeftX(), manette.getLeftY(), manette.getRightX()); diff --git a/src/main/java/frc/robot/commands/Force1.java b/src/main/java/frc/robot/commands/Force1.java index 650d76b..dc3e8d5 100644 --- a/src/main/java/frc/robot/commands/Force1.java +++ b/src/main/java/frc/robot/commands/Force1.java @@ -3,57 +3,46 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.commands; -import frc.robot.subsystems.Accumulateur; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + + +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Lanceur; public class Force1 extends CommandBase { - private Lanceur lanceur; - private Accumulateur accumulateur; - - public void Lancer(Lanceur lanceur) { + GenericEntry force1; + + /** Creates a new Force1. */ + public Force1(Lanceur lanceur, GenericEntry force1) { this.lanceur = lanceur; - this.accumulateur = accumulateur; - addRequirements(lanceur , accumulateur); + addRequirements(lanceur); + this.force1 = force1; + // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override public void initialize() { - lanceur.setPID(0, 0, 0); + lanceur.setPID(0,0,0); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double vitesse = SmartDashboard.getNumber("Force Lanceur", 100); - lanceur.lancer(200); - if (lanceur.vitesse() > vitesse ){ - if(accumulateur.tourneavant() < 1024){ - accumulateur.Deaccumuler(); - } - if(accumulateur.tournearriere()<-256){ - accumulateur.reaccumuler(); - } - accumulateur.tournearriere(); - } else { - accumulateur.stop(); - } - + lanceur.lancer(force1.getDouble(0)); + } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - lanceur.stop(); - accumulateur.stop(); - } + lanceur.lancer(0); + } // Returns true when the command should end. @Override public boolean isFinished() { - return lanceur.distance()>1; + return false; } } diff --git a/src/main/java/frc/robot/commands/Force2.java b/src/main/java/frc/robot/commands/Force2.java index 23ff4fa..d359ed5 100644 --- a/src/main/java/frc/robot/commands/Force2.java +++ b/src/main/java/frc/robot/commands/Force2.java @@ -2,57 +2,42 @@ // 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. -// 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 frc.robot.subsystems.Accumulateur; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Lanceur; public class Force2 extends CommandBase { - private Lanceur lanceur; - private Accumulateur accumulateur; - public void Lancer(Lanceur lanceur) { + /** Creates a new Force1. */ + public Force2(Lanceur lancer) { this.lanceur = lanceur; - this.accumulateur = accumulateur; - addRequirements(lanceur , accumulateur); + addRequirements(lanceur); + // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override public void initialize() { - lanceur.setPID(0, 0, 0); + lanceur.setPID(0,0,0); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double vitesse = SmartDashboard.getNumber("Force Lanceur", 200); - lanceur.lancer(200); - if (lanceur.vitesse() > vitesse ){ - accumulateur.tourneavant(); - accumulateur.tournearriere(); - } else { - accumulateur.stop(); - } - + lanceur.lancer(500); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - lanceur.stop(); - accumulateur.stop(); - } + lanceur.lancer(0); + } // Returns true when the command should end. @Override public boolean isFinished() { - return lanceur.distance()>1; + return false; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/Force3.java b/src/main/java/frc/robot/commands/Force3.java index d187be3..71c9e95 100644 --- a/src/main/java/frc/robot/commands/Force3.java +++ b/src/main/java/frc/robot/commands/Force3.java @@ -9,6 +9,8 @@ package frc.robot.commands; import frc.robot.subsystems.Accumulateur; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Lanceur; @@ -20,7 +22,15 @@ public class Force3 extends CommandBase { public void Lancer(Lanceur lanceur) { this.lanceur = lanceur; this.accumulateur = accumulateur; - addRequirements(lanceur , accumulateur); + addRequirements(lanceur , accumulateur);} + + GenericEntry force3; + /** Creates a new Force1. */ + public Force3(Lanceur lanceur,GenericEntry force3) { + this.lanceur = lanceur; + addRequirements(lanceur); + this.force3 = force3; + // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -32,27 +42,14 @@ public class Force3 extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + double vitesse = SmartDashboard.getNumber("Force Lanceur", 300); lanceur.lancer(200); - if (lanceur.vitesse() > vitesse ){ - accumulateur.tourneavant(); - accumulateur.tournearriere(); - } else { - accumulateur.stop(); - } - - } - - // Called once the command ends or is interrupted. - @Override + lanceur.lancer(force3.getDouble(0));} public void end(boolean interrupted) { - lanceur.stop(); - accumulateur.stop(); } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return lanceur.distance()>1; + @Override + public boolean isFinished() { + return false; + } } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Force4.java b/src/main/java/frc/robot/commands/Force4.java index 056b8dc..498cc4b 100644 --- a/src/main/java/frc/robot/commands/Force4.java +++ b/src/main/java/frc/robot/commands/Force4.java @@ -5,6 +5,8 @@ package frc.robot.commands; import frc.robot.subsystems.Accumulateur; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Lanceur; @@ -16,7 +18,14 @@ public class Force4 extends CommandBase { public void Lancer(Lanceur lanceur) { this.lanceur = lanceur; this.accumulateur = accumulateur; - addRequirements(lanceur , accumulateur); + addRequirements(lanceur , accumulateur);} + GenericEntry force4; + /** Creates a new Force1. */ + public Force4(Lanceur lanceur,GenericEntry force4) { + this.lanceur = lanceur; + addRequirements(lanceur); + this.force4 = force4; + // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -37,6 +46,7 @@ public class Force4 extends CommandBase { accumulateur.stop(); } + lanceur.lancer(force4.getDouble(0)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/Force5.java b/src/main/java/frc/robot/commands/Force5.java index bc8ec1b..669dae0 100644 --- a/src/main/java/frc/robot/commands/Force5.java +++ b/src/main/java/frc/robot/commands/Force5.java @@ -5,6 +5,8 @@ package frc.robot.commands; import frc.robot.subsystems.Accumulateur; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Lanceur; @@ -16,7 +18,14 @@ public class Force5 extends CommandBase { public void Lancer(Lanceur lanceur) { this.lanceur = lanceur; this.accumulateur = accumulateur; - addRequirements(lanceur , accumulateur); + addRequirements(lanceur , accumulateur);} + GenericEntry force5; + /** Creates a new Force1. */ + public Force5(Lanceur lanceur,GenericEntry force5) { + this.lanceur = lanceur; + addRequirements(lanceur); + this.force5 = force5; + // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -37,6 +46,7 @@ public class Force5 extends CommandBase { accumulateur.stop(); } + lanceur.lancer(force5.getDouble(0)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/Force6.java b/src/main/java/frc/robot/commands/Force6.java index 9cc2f04..2cbf863 100644 --- a/src/main/java/frc/robot/commands/Force6.java +++ b/src/main/java/frc/robot/commands/Force6.java @@ -5,6 +5,8 @@ package frc.robot.commands; import frc.robot.subsystems.Accumulateur; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Lanceur; @@ -18,6 +20,14 @@ public class Force6 extends CommandBase { this.accumulateur = accumulateur; addRequirements(lanceur , accumulateur); } + GenericEntry force6; + /** Creates a new Force1. */ + public Force6(Lanceur lanceur, GenericEntry force6) { + this.lanceur = lanceur; + addRequirements(lanceur); + this.force6 = force6; + // Use addRequirements() here to declare subsystem dependencies. + } // Called when the command is initially scheduled. @Override @@ -37,6 +47,7 @@ public class Force6 extends CommandBase { accumulateur.stop(); } + lanceur.lancer(force6.getDouble(0)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/Force7.java b/src/main/java/frc/robot/commands/Force7.java index 3ed4970..6558569 100644 --- a/src/main/java/frc/robot/commands/Force7.java +++ b/src/main/java/frc/robot/commands/Force7.java @@ -5,6 +5,8 @@ package frc.robot.commands; import frc.robot.subsystems.Accumulateur; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Lanceur; @@ -16,7 +18,14 @@ public class Force7 extends CommandBase { public void Lancer(Lanceur lanceur) { this.lanceur = lanceur; this.accumulateur = accumulateur; - addRequirements(lanceur , accumulateur); + addRequirements(lanceur , accumulateur);} + GenericEntry force7; + /** Creates a new Force1. */ + public Force7(Lanceur lanceur,GenericEntry force7) { + this.lanceur = lanceur; + addRequirements(lanceur); + this.force7 = force7; + // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -37,6 +46,7 @@ public class Force7 extends CommandBase { accumulateur.stop(); } + lanceur.lancer(force7.getDouble(0)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/Lanceur.java b/src/main/java/frc/robot/subsystems/Lanceur.java index ccdb0f8..1a26f5f 100644 --- a/src/main/java/frc/robot/subsystems/Lanceur.java +++ b/src/main/java/frc/robot/subsystems/Lanceur.java @@ -6,14 +6,14 @@ package frc.robot.subsystems; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; + import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; public class Lanceur extends SubsystemBase { - - final CANSparkMax lanceur = new CANSparkMax(Constants.lanceur, MotorType.kBrushless); public Lanceur(){} + final CANSparkMax lanceur = new CANSparkMax(Constants.lanceur, MotorType.kBrushless); public void lancer(double vitesse){ lanceur.set(vitesse);