diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b55a36e..7e86879 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,7 +11,9 @@ 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.ParallelCommandGroup; 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 frc.robot.commands.Avancer; @@ -23,6 +25,7 @@ import frc.robot.commands.Force4; import frc.robot.commands.Force5; import frc.robot.commands.Force6; import frc.robot.commands.Force7; +import frc.robot.commands.Force1; import frc.robot.commands.Reculer; import frc.robot.commands.accumulateurtest; import frc.robot.subsystems.Accumulateur; @@ -36,7 +39,10 @@ public class RobotContainer { ShuffleboardTab dashboard = Shuffleboard.getTab("Dashboard"); ShuffleboardLayout forces = Shuffleboard.getTab("Dashboard") .getLayout("forces", BuiltInLayouts.kList) - .withSize(3, 7); + .withSize(1, 7) + .withPosition(1, 0) + .withSize(1, 7); + GenericEntry force1 = forces.add("Force1", 2).getEntry(); GenericEntry force2 = forces.add("Force2", 4).getEntry(); GenericEntry force3 = forces.add("Force3", 6).getEntry(); @@ -78,11 +84,11 @@ public class RobotContainer { joystick1.button(11).whileTrue(Force6); joystick1.button(12).whileTrue(Force7); } - - public Command getAutonomousCommand() { - return null; - /* new SequentialCommandGroup(new Avancer(drive), new Force7(lanceur, force7, accumulateur) - , new Lancer(lanceur, accumulateur, force1), new Reculer(drive)); - */ } + + + public Command getAutonomousCommand() { + return new SequentialCommandGroup(new ParallelCommandGroup(new Force1(lanceur).withTimeout(5),new accumulateurtest(accumulateur).withTimeout(5)) + ,new Avancer(drive)); + } } diff --git a/src/main/java/frc/robot/commands/Avancer.java b/src/main/java/frc/robot/commands/Avancer.java index c0ee5c7..0e7c1f1 100644 --- a/src/main/java/frc/robot/commands/Avancer.java +++ b/src/main/java/frc/robot/commands/Avancer.java @@ -16,7 +16,8 @@ public class Avancer extends CommandBase { ShuffleboardTab dashboard = Shuffleboard.getTab("Dashboard"); ShuffleboardLayout avancer = Shuffleboard.getTab("Dashboard") .getLayout("Avancer", BuiltInLayouts.kList) - .withSize(3, 3); + .withSize(1, 4) + .withPosition(4, 0); /** Creates a new Avancer. */ public Avancer(Drive drive) { avancer.add("vitesse x", 1); diff --git a/src/main/java/frc/robot/commands/Force1.java b/src/main/java/frc/robot/commands/Force1.java index 0241b5a..6496c62 100644 --- a/src/main/java/frc/robot/commands/Force1.java +++ b/src/main/java/frc/robot/commands/Force1.java @@ -5,13 +5,18 @@ package frc.robot.commands; import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Lanceur; public class Force1 extends CommandBase { + ShuffleboardTab dashboard = Shuffleboard.getTab("Dashboard"); + private Lanceur lanceur; /** Creates a new LancerTest. */ - public Force1(Lanceur lanceur, GenericEntry force1) { + public Force1(Lanceur lanceur) { + dashboard.add("vitesse lanceur", 0.5); this.lanceur = lanceur; addRequirements(lanceur); // Use addRequirements() here to declare subsystem dependencies. diff --git a/src/main/java/frc/robot/commands/LanceurAuto.java b/src/main/java/frc/robot/commands/LanceurAuto.java new file mode 100644 index 0000000..72b12f9 --- /dev/null +++ b/src/main/java/frc/robot/commands/LanceurAuto.java @@ -0,0 +1,37 @@ +// 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.Lanceur; + +public class LanceurAuto extends CommandBase { + private Lanceur lanceur; + /** Creates a new LanceurAuto. */ + public LanceurAuto(Lanceur lanceur) { + this.lanceur = lanceur; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + lanceur.setPID(0, 0, 0); + } + + // 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) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Reculer.java b/src/main/java/frc/robot/commands/Reculer.java index b0303ab..9571c64 100644 --- a/src/main/java/frc/robot/commands/Reculer.java +++ b/src/main/java/frc/robot/commands/Reculer.java @@ -16,7 +16,8 @@ public class Reculer extends CommandBase { ShuffleboardTab dashboard = Shuffleboard.getTab("Dashboard"); ShuffleboardLayout reculer = Shuffleboard.getTab("Dashboard") .getLayout("Reculer", BuiltInLayouts.kList) - .withSize(3, 3); + .withSize(1, 4) + .withPosition(6,0); /** Creates a new Reculer. */ public Reculer(Drive drive) { reculer.add("vitesse x", 1); diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index 530646d..5b8b66a 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -7,17 +7,47 @@ package frc.robot.subsystems; import java.io.File; import java.io.IOException; +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import swervelib.SwerveDrive; +import swervelib.encoders.CanAndCoderSwerve; import swervelib.parser.SwerveParser; public class Drive extends SubsystemBase { + ShuffleboardTab dashboard = Shuffleboard.getTab("Dashboard"); + ShuffleboardLayout avancer = Shuffleboard.getTab("Dashboard") + .getLayout("Avancer", BuiltInLayouts.kList) + .withSize(1, 4) + .withPosition(4, 0); + ShuffleboardLayout reculer = Shuffleboard.getTab("Dashboard") + .getLayout("Reculer", BuiltInLayouts.kList) + .withSize(1, 4) + .withPosition(3,0); SwerveDrive swerveDrive; File swerveJsonDirectory = new File(Filesystem.getDeployDirectory(),"swerve"); - + /* final CanAndCoderSwerve avantdroitdrive = new CanAndCoderSwerve(Constants.avantdroitDrive); + final CanAndCoderSwerve avantgauchedrive = new CanAndCoderSwerve(Constants.avantgaucheDrive); + final CanAndCoderSwerve arrieregauchedrive = new CanAndCoderSwerve(Constants.arrieregaucheDrive); + final CanAndCoderSwerve arrieredroitdrive = new CanAndCoderSwerve(Constants.arrieredroitDrive); + final CanAndCoderSwerve avantdroitangle = new CanAndCoderSwerve(Constants.avantdroitAngle); + final CanAndCoderSwerve avantgaucheangle = new CanAndCoderSwerve(Constants.avantgaucheAngle); + final CanAndCoderSwerve arrieregaucheangle = new CanAndCoderSwerve(Constants.arrieregaucheAngle); + final CanAndCoderSwerve arrieredroitangle = new CanAndCoderSwerve(Constants.arrieredroitAngle); */ public void drive(double x, double y, double zRotation){ swerveDrive.drive(new Translation2d(x, y), zRotation, false, false); } @@ -26,6 +56,14 @@ public class Drive extends SubsystemBase { /** Creates a new Drive. */ public Drive() { + reculer.add("vitesse x", 1); + reculer.add("vitesse y", 2); + reculer.add("vitesse z", 3); + reculer.add("distance", 4); + avancer.add("vitesse x", 1); + avancer.add("vitesse y", 2); + avancer.add("vitesse z", 3); + avancer.add("distance", 4); try { this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(); } catch (IOException e) { @@ -36,6 +74,9 @@ public class Drive extends SubsystemBase { public SwerveModulePosition[] distance(){ return swerveDrive.getModulePositions(); } +public void reset(){ + +} @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 dddd1b3..7dd7e59 100644 --- a/src/main/java/frc/robot/subsystems/Lanceur.java +++ b/src/main/java/frc/robot/subsystems/Lanceur.java @@ -7,6 +7,7 @@ package frc.robot.subsystems; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; @@ -18,14 +19,16 @@ public class Lanceur extends SubsystemBase { ShuffleboardTab dash2 = Shuffleboard.getTab("Dashboard2.0"); ShuffleboardTab dashboard = Shuffleboard.getTab("Dashboard"); - //ShuffleboardLayout pid = Shuffleboard.getTab("Dashboard") - //.getLayout("Pid", BuiltInLayouts.kList) - //.withSize(3, 7); + ShuffleboardLayout pid = Shuffleboard.getTab("Dashboard") + .getLayout("Pid", BuiltInLayouts.kList) + .withSize(1, 3) + .withPosition(0, 0); + private GenericEntry p = pid.add("p", 1).getEntry(); + public Lanceur(){ - dash2.add("test", 1); - dashboard.add("p", 1); - dashboard.add("i", 2); - dashboard.add("d", 3); + double P = p.getDouble(1.0); + pid.add("i", 2).getEntry(); + pid.add("d", 3).getEntry(); } final CANSparkMax lanceur = new CANSparkMax(Constants.lanceur, MotorType.kBrushless);