diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9f25d03..9ec8880 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.MathUtil; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; @@ -10,6 +11,7 @@ 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; @@ -39,6 +41,7 @@ public class RobotContainer { ShuffleboardLayout forces = Shuffleboard.getTab("Dashboard") .getLayout("forces", BuiltInLayouts.kList) .withSize(3, 7); + GenericEntry force1 = forces.add("Force1", 2).getEntry(); GenericEntry force2 = forces.add("Force2", 4).getEntry(); GenericEntry force3 = forces.add("Force3", 6).getEntry(); @@ -76,9 +79,8 @@ public class RobotContainer { public Command getAutonomousCommand() { - return null; - /* new SequentialCommandGroup(new Avancer(drive), new Force7(lanceur, force7, accumulateur) - , new Lancer(lanceur, accumulateur, force1), new Reculer(drive)); - */ } + return new SequentialCommandGroup(new ParallelCommandGroup(new LancerTest(lanceur).withTimeout(5),new accumulateurtest(accumulateur).withTimeout(5)) + ,new Avancer(drive)); + } } diff --git a/src/main/java/frc/robot/commands/LancerTest.java b/src/main/java/frc/robot/commands/LancerTest.java index 95eaeb0..03ed1ab 100644 --- a/src/main/java/frc/robot/commands/LancerTest.java +++ b/src/main/java/frc/robot/commands/LancerTest.java @@ -4,13 +4,19 @@ 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 LancerTest extends CommandBase { + ShuffleboardTab dashboard = Shuffleboard.getTab("Dashboard"); + private Lanceur lanceur; /** Creates a new LancerTest. */ public LancerTest(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/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index c0aa314..1032e73 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -7,17 +7,32 @@ package frc.robot.subsystems; import java.io.File; import java.io.IOException; +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"); 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); } @@ -25,6 +40,7 @@ public class Drive extends SubsystemBase { /** Creates a new Drive. */ public Drive() { + try { this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(); } catch (IOException e) { @@ -34,6 +50,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