diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 18b8937..7b6231f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,36 +3,80 @@ // 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; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.commands.Avancer; +import frc.robot.commands.Force1; +import frc.robot.commands.Force2; +import frc.robot.commands.Force3; +import frc.robot.commands.Force4; +import frc.robot.commands.Force5; +import frc.robot.commands.Force6; +import frc.robot.commands.Force7; +import frc.robot.commands.Reculer; +import frc.robot.subsystems.Accumulateur; import frc.robot.subsystems.Drive; +import frc.robot.subsystems.Lanceur; public class RobotContainer { + Lanceur lanceur = new Lanceur(); + Accumulateur accumulateur = new Accumulateur(); + + + + 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); + CommandJoystick joystick1 = new CommandJoystick(0); Drive drive = new Drive(); public RobotContainer() { + configureBindings(); - // drive.setDefaultCommand(new RunCommand(()->{ - // drive.drive(manette.getLeftX(), manette.getLeftY(), manette.getRightX()); - //},drive)); drive.setDefaultCommand(new RunCommand(()->{ drive.drive(joystick1.getX(), -joystick1.getY(), -joystick1.getZ()); },drive)); } private void configureBindings() { - JoystickButton button =new JoystickButton(joystick1, 1); + Force1 Force1 = new Force1(lanceur, accumulateur, null); + Force2 Force2 = new Force2(lanceur, accumulateur, null); + Force3 Force3 = new Force3(lanceur, null, accumulateur); + Force4 Force4 = new Force4(lanceur, null, accumulateur); + Force5 Force5 = new Force5(lanceur, null, accumulateur); + Force6 Force6 = new Force6(lanceur, null, accumulateur); + Force7 Force7 = new Force7(lanceur, null, accumulateur); + joystick1.button(7).onTrue(Force1); + joystick1.button(8).onTrue(Force2); + joystick1.button(9).onTrue(Force3); + joystick1.button(10).onTrue(Force4); + joystick1.button(11).onTrue(Force5); + joystick1.button(12).onTrue(Force6); + joystick1.button(3).onTrue(Force7); + } - + public Command getAutonomousCommand() { - return new SequentialCommandGroup(null); + return new SequentialCommandGroup(new Avancer(drive), new Force7(lanceur, force7, accumulateur) + , new Force1(lanceur, accumulateur, force1), new Reculer(drive)); } } diff --git a/src/main/java/frc/robot/commands/Avancer.java b/src/main/java/frc/robot/commands/Avancer.java new file mode 100644 index 0000000..476601e --- /dev/null +++ b/src/main/java/frc/robot/commands/Avancer.java @@ -0,0 +1,40 @@ +// 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.CommandBase; +import frc.robot.subsystems.Drive; + +public class Avancer extends CommandBase { + private Drive drive; + /** Creates a new Avancer. */ + public Avancer(Drive drive) { + this.drive = drive; + addRequirements(drive); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + drive.drive(0.5, 0.2, 0); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return drive.distance()[0].distanceMeters>1; + } +} diff --git a/src/main/java/frc/robot/commands/Force1.java b/src/main/java/frc/robot/commands/Force1.java index 650d76b..8d4e567 100644 --- a/src/main/java/frc/robot/commands/Force1.java +++ b/src/main/java/frc/robot/commands/Force1.java @@ -3,57 +3,56 @@ // 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.Accumulateur; 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,Accumulateur accumulateur, GenericEntry force1) { this.lanceur = lanceur; this.accumulateur = accumulateur; - addRequirements(lanceur , accumulateur); + addRequirements(lanceur, accumulateur); + 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); + lanceur.lancer(force1.getDouble(100)); + double vitesse = (100); if (lanceur.vitesse() > vitesse ){ - if(accumulateur.tourneavant() < 1024){ - accumulateur.Deaccumuler(); - } - if(accumulateur.tournearriere()<-256){ - accumulateur.reaccumuler(); - } + accumulateur.tourneavant(); accumulateur.tournearriere(); } else { accumulateur.stop(); } - } + // 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..ea8564c 100644 --- a/src/main/java/frc/robot/commands/Force2.java +++ b/src/main/java/frc/robot/commands/Force2.java @@ -2,57 +2,54 @@ // 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.networktables.GenericEntry; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Accumulateur; import frc.robot.subsystems.Lanceur; public class Force2 extends CommandBase { - private Lanceur lanceur; private Accumulateur accumulateur; - - public void Lancer(Lanceur lanceur) { + GenericEntry force2; + /** Creates a new Force1. */ + public Force2(Lanceur lanceur,Accumulateur accumulateur, GenericEntry force2) { this.lanceur = lanceur; this.accumulateur = accumulateur; - addRequirements(lanceur , accumulateur); + addRequirements(lanceur, accumulateur); + this.force2 = force2; + // 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); + double vitesse = (200); + lanceur.lancer(force2.getDouble(200)); if (lanceur.vitesse() > vitesse ){ accumulateur.tourneavant(); accumulateur.tournearriere(); } else { accumulateur.stop(); } - } // 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..4c1e363 100644 --- a/src/main/java/frc/robot/commands/Force3.java +++ b/src/main/java/frc/robot/commands/Force3.java @@ -8,7 +8,9 @@ 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,11 +18,15 @@ public class Force3 extends CommandBase { private Lanceur lanceur; private Accumulateur accumulateur; - - public void Lancer(Lanceur lanceur) { + GenericEntry force3; + + /** Creates a new Force1. */ + public Force3(Lanceur lanceur,GenericEntry force3, Accumulateur accumulateur) { this.lanceur = lanceur; + addRequirements(lanceur, accumulateur); + this.force3 = force3; this.accumulateur = accumulateur; - addRequirements(lanceur , accumulateur); + // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -32,27 +38,18 @@ 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 - public void end(boolean interrupted) { - lanceur.stop(); - accumulateur.stop(); + double vitesse = (300); + lanceur.lancer(force3.getDouble(300)); + if (lanceur.vitesse() > vitesse ){ + accumulateur.tourneavant(); + accumulateur.tournearriere(); + } else { + accumulateur.stop(); + } + } + @Override + public boolean isFinished() { + return false; } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return lanceur.distance()>1; } -} \ 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..8658cfc 100644 --- a/src/main/java/frc/robot/commands/Force4.java +++ b/src/main/java/frc/robot/commands/Force4.java @@ -4,7 +4,9 @@ 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; @@ -12,11 +14,15 @@ public class Force4 extends CommandBase { private Lanceur lanceur; private Accumulateur accumulateur; - - public void Lancer(Lanceur lanceur) { + GenericEntry force4; + + /** Creates a new Force1. */ + public Force4(Lanceur lanceur,GenericEntry force4, Accumulateur accumulateur) { this.lanceur = lanceur; + addRequirements(lanceur, accumulateur); + this.force4 = force4; this.accumulateur = accumulateur; - addRequirements(lanceur , accumulateur); + // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -28,8 +34,8 @@ public class Force4 extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double vitesse = SmartDashboard.getNumber("Force Lanceur", 400); - lanceur.lancer(200); + double vitesse = (400); + lanceur.lancer(force4.getDouble(400)); if (lanceur.vitesse() > vitesse ){ accumulateur.tourneavant(); accumulateur.tournearriere(); @@ -37,6 +43,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..1354f81 100644 --- a/src/main/java/frc/robot/commands/Force5.java +++ b/src/main/java/frc/robot/commands/Force5.java @@ -4,7 +4,9 @@ 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; @@ -12,11 +14,15 @@ public class Force5 extends CommandBase { private Lanceur lanceur; private Accumulateur accumulateur; - - public void Lancer(Lanceur lanceur) { + GenericEntry force5; + + /** Creates a new Force1. */ + public Force5(Lanceur lanceur,GenericEntry force5, Accumulateur accumulateur) { this.lanceur = lanceur; + addRequirements(lanceur, accumulateur); + this.force5 = force5; this.accumulateur = accumulateur; - addRequirements(lanceur , accumulateur); + // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -28,8 +34,8 @@ public class Force5 extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double vitesse = SmartDashboard.getNumber("Force Lanceur", 500); - lanceur.lancer(200); + double vitesse = (500); + lanceur.lancer(force5.getDouble(500)); if (lanceur.vitesse() > vitesse ){ accumulateur.tourneavant(); accumulateur.tournearriere(); @@ -37,6 +43,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..6655df4 100644 --- a/src/main/java/frc/robot/commands/Force6.java +++ b/src/main/java/frc/robot/commands/Force6.java @@ -4,7 +4,7 @@ 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; @@ -12,11 +12,16 @@ public class Force6 extends CommandBase { private Lanceur lanceur; private Accumulateur accumulateur; - - public void Lancer(Lanceur lanceur) { + GenericEntry force6; + + + /** Creates a new Force1. */ + public Force6(Lanceur lanceur, GenericEntry force6, Accumulateur accumulateur) { this.lanceur = lanceur; + addRequirements(lanceur, accumulateur); + this.force6 = force6; this.accumulateur = accumulateur; - addRequirements(lanceur , accumulateur); + // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -28,8 +33,8 @@ public class Force6 extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double vitesse = SmartDashboard.getNumber("Force Lanceur", 600); - lanceur.lancer(200); + double vitesse = (600); + lanceur.lancer(force6.getDouble(600)); if (lanceur.vitesse() > vitesse ){ accumulateur.tourneavant(); accumulateur.tournearriere(); @@ -37,6 +42,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..b1ba4cc 100644 --- a/src/main/java/frc/robot/commands/Force7.java +++ b/src/main/java/frc/robot/commands/Force7.java @@ -4,7 +4,7 @@ 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; @@ -12,11 +12,15 @@ public class Force7 extends CommandBase { private Lanceur lanceur; private Accumulateur accumulateur; - - public void Lancer(Lanceur lanceur) { + GenericEntry force7; + + /** Creates a new Force1. */ + public Force7(Lanceur lanceur,GenericEntry force7, Accumulateur accumulateur) { this.lanceur = lanceur; + addRequirements(lanceur, accumulateur); + this.force7 = force7; this.accumulateur = accumulateur; - addRequirements(lanceur , accumulateur); + // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -28,8 +32,8 @@ public class Force7 extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double vitesse = SmartDashboard.getNumber("Force Lanceur", 700); - lanceur.lancer(200); + double vitesse = (700); + lanceur.lancer(force7.getDouble(700)); if (lanceur.vitesse() > vitesse ){ accumulateur.tourneavant(); accumulateur.tournearriere(); @@ -37,6 +41,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/commands/Reculer.java b/src/main/java/frc/robot/commands/Reculer.java new file mode 100644 index 0000000..9d67460 --- /dev/null +++ b/src/main/java/frc/robot/commands/Reculer.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.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Drive; + +public class Reculer extends CommandBase { + private Drive drive; + /** Creates a new Reculer. */ + public Reculer(Drive drive) { + this.drive = drive; + addRequirements(drive); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + drive.drive(-0.5, -0.2, 0); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return drive.distance()[0].distanceMeters>1; + } +} diff --git a/src/main/java/frc/robot/subsystems/Accumulateur.java b/src/main/java/frc/robot/subsystems/Accumulateur.java index daa68d0..facaa88 100644 --- a/src/main/java/frc/robot/subsystems/Accumulateur.java +++ b/src/main/java/frc/robot/subsystems/Accumulateur.java @@ -7,7 +7,7 @@ package frc.robot.subsystems; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import edu.wpi.first.wpilibj.DigitalInput; + import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index 637b22c..d2391d1 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -8,6 +8,7 @@ import java.io.File; import java.io.IOException; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; import swervelib.SwerveDrive; @@ -30,7 +31,9 @@ public class Drive extends SubsystemBase { e.printStackTrace(); } } - + public SwerveModulePosition[] distance(){ + return swerveDrive.getModulePositions(); + } @Override public void periodic() { // This method will be called once per scheduler run 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);