diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 73fa1ef..e46be4b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -10,6 +10,7 @@ public class Constants { //moteur public static int leverGratte = 0; + public static int baiserGratte = 1; public static int baisserGratte = 1; // pneumatique diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3734664..bfb40f7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,7 +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.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -16,7 +16,6 @@ import frc.robot.subsystems.Gratte; import frc.robot.subsystems.bras.BrasTelescopique; import frc.robot.subsystems.bras.Pince; import frc.robot.subsystems.bras.Pivot; -import pabeles.concurrency.ConcurrencyOps.Reset; import frc.robot.subsystems.Limelight; import frc.robot.commands.Apriltag; // command @@ -33,6 +32,7 @@ import frc.robot.commands.bras.PivotBrasRentre; import frc.robot.commands.bras.PivoteBrasBas; import frc.robot.commands.bras.PivoteBrasHaut; import frc.robot.commands.bras.PivoteBrasMilieux; +<<<<<<< HEAD import frc.robot.commands.bras.PivotChercheBas; import frc.robot.commands.bras.PivotChercheHaut; import edu.wpi.first.apriltag.AprilTag; @@ -40,8 +40,12 @@ import edu.wpi.first.math.spline.CubicHermiteSpline; import frc.robot.commands.Cube; import frc.robot.commands.Cone; import frc.robot.commands.Tape; +======= + +>>>>>>> a50cbc2cf5643bf54839bb1e5520aa0bf79cd925 public class RobotContainer { +//CameraServer.startAutomaticCapture(); CommandXboxController manette1 = new CommandXboxController(0); CommandXboxController manette2 = new CommandXboxController(1); // subsystems @@ -80,11 +84,15 @@ public RobotContainer() { } private void configureBindings() { +<<<<<<< HEAD // manette 1 +======= +>>>>>>> a50cbc2cf5643bf54839bb1e5520aa0bf79cd925 manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer,pince)); manette1.x().toggleOnTrue(Commands.startEnd(basePilotable::BrakeFerme,basePilotable::BrakeOuvre,basePilotable)); manette1.y().whileTrue(gyro); manette1.start().toggleOnTrue(Commands.startEnd(basePilotable::resetGyro, basePilotable::resetGyro, basePilotable)); +<<<<<<< HEAD manette1.b().onTrue(Commands.either(gratteBaisser, gratteMonte, gratte::getenHaut)); manette1.leftBumper().toggleOnTrue(aprilTag); manette1.rightBumper().toggleOnTrue(tape); @@ -97,6 +105,17 @@ public RobotContainer() { manette2.povLeft().onTrue(pivotChercheHaut); manette2.rightBumper().toggleOnTrue(cube); manette2.leftBumper().toggleOnTrue(cone); +======= + /*manette1.b().toggleOnTrue(Commands.startEnd(gratte::baiser, gratte::Lever,gratte)); + manette2.a().toggleOnTrue(Commands.startEnd(brasTelescopique::pivoteBrasHaut, brasTelescopique::pivoteBrasHaut)); + manette2.b().toggleOnTrue(Commands.startEnd(brasTelescopique::pivoteBrasBas, brasTelescopique::pivoteBrasBas, brasTelescopique)); + manette2.x().toggleOnTrue(Commands.startEnd(brasTelescopique::pivoteBrasMilieux, brasTelescopique::pivoteBrasMilieux, brasTelescopique)); + manette2.y().toggleOnTrue(Commands.startEnd(brasTelescopique::pivotBrasRentre, brasTelescopique::pivotBrasRentre, brasTelescopique)); + manette2.povRight().toggleOnTrue(Commands.startEnd(brasTelescopique::PivotChercheBas, brasTelescopique::PivotChercheBas, brasTelescopique)); + manette2.povLeft().toggleOnTrue(Commands.startEnd(brasTelescopique::PivotChercheHaut, brasTelescopique::PivotChercheHaut, brasTelescopique)); + manette2.rightBumper().toggleOnTrue(Commands.startEnd(null, null, null)); + manette2.leftBumper().toggleOnTrue(Commands.startEnd(null, null, null));**/ +>>>>>>> a50cbc2cf5643bf54839bb1e5520aa0bf79cd925 } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Gyro.java b/src/main/java/frc/robot/commands/Gyro.java index dcb7893..e8742ba 100644 --- a/src/main/java/frc/robot/commands/Gyro.java +++ b/src/main/java/frc/robot/commands/Gyro.java @@ -4,13 +4,18 @@ package frc.robot.commands; +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.BasePilotable; public class Gyro extends CommandBase { + ShuffleboardTab teb = Shuffleboard.getTab("teb"); private BasePilotable basePilotable; /** Creates a new Gyro. */ public Gyro(BasePilotable basePilotable) { + teb.add("angleGyro", 0.1); + teb.add("vitesseGyro", 0.1); this.basePilotable = basePilotable; // Use addRequirements() here to declare subsystem dependencies. addRequirements(basePilotable); @@ -23,13 +28,13 @@ public class Gyro extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(basePilotable.getpitch()<10) + if(basePilotable.getpitch()>4) { - basePilotable.drive(0.4, 0); + basePilotable.drive(0.3*basePilotable.getpitch()/15, 0); } - else if(basePilotable.getpitch()>-10) + else if(basePilotable.getpitch()<-4) { - basePilotable.drive(-0.4, 0); + basePilotable.drive(0.3*basePilotable.getpitch()/15, 0); } else { diff --git a/src/main/java/frc/robot/subsystems/BasePilotable.java b/src/main/java/frc/robot/subsystems/BasePilotable.java index 0aec4ea..fb14cae 100644 --- a/src/main/java/frc/robot/subsystems/BasePilotable.java +++ b/src/main/java/frc/robot/subsystems/BasePilotable.java @@ -8,7 +8,6 @@ import com.kauailabs.navx.frc.AHRS; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.drive.DifferentialDrive; @@ -35,7 +34,7 @@ public class BasePilotable extends SubsystemBase { //gyro ShuffleboardTab teb = Shuffleboard.getTab("teb"); ShuffleboardLayout layout = Shuffleboard.getTab("teb") - .getLayout ("encodeurs base pilotable", BuiltInLayouts.kList) + .getLayout ("distance", BuiltInLayouts.kList) .withSize(2, 2); private AHRS gyroscope = new AHRS();public double getangle() {return gyroscope.getAngle();} public double getpitch() { @@ -71,12 +70,11 @@ public void resetGyro(){ /** Creates a new BasePilotable. */ public BasePilotable() { droit.setInverted(true); + layout .addDouble("distance", this::distance); } @Override public void periodic() { - teb .add("distance",0.1); - teb .add("brakedroit",0.1); - teb .add("brakegauche", 0.1); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Gratte.java b/src/main/java/frc/robot/subsystems/Gratte.java index 0934180..df518c7 100644 --- a/src/main/java/frc/robot/subsystems/Gratte.java +++ b/src/main/java/frc/robot/subsystems/Gratte.java @@ -14,18 +14,17 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; 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 frc.robot.Constants; public class Gratte extends SubsystemBase { - ShuffleboardTab teb = Shuffleboard.getTab("teb"); +ShuffleboardTab teb = Shuffleboard.getTab("teb"); ShuffleboardLayout limitswitchgratte = Shuffleboard.getTab("teb") .getLayout("limitswitchsgratte", BuiltInLayouts.kList) .withSize(2, 2); private WPI_TalonSRX Gratted = new WPI_TalonSRX(Constants.leverGratte); - private WPI_TalonSRX Gratteg = new WPI_TalonSRX(Constants.baisserGratte); + private WPI_TalonSRX Gratteg = new WPI_TalonSRX(Constants.baiserGratte); private DigitalInput limithd = new DigitalInput(Constants.limithd); private DigitalInput limithg = new DigitalInput(Constants.limithg); private DigitalInput limitbd = new DigitalInput(Constants.limitbd); @@ -55,12 +54,21 @@ ShuffleboardLayout limitswitchgratte = Shuffleboard.getTab("teb") return limitbg.get(); } /** Creates a new Gratte. */ +<<<<<<< HEAD public Gratte() { } +======= + public Gratte() { + limitswitchgratte.add ("limitbd", 0.1); + limitswitchgratte.add ("limithg", 0.1); + limitswitchgratte.add ("limithd", 0.1); + limitswitchgratte.add ("limitbg", 0.1); + } +>>>>>>> a50cbc2cf5643bf54839bb1e5520aa0bf79cd925 public void Lever(double vitesse){ Gratted.set(vitesse); Gratteg.set(vitesse); @@ -70,10 +78,15 @@ ShuffleboardLayout limitswitchgratte = Shuffleboard.getTab("teb") Gratteg.set(-vitesse); } @Override +<<<<<<< HEAD public void periodic(){ limitswitchgratte.add ("limitbd", 0.1); limitswitchgratte.add ("limithg", 0.1); limitswitchgratte.add ("limithd", 0.1); limitswitchgratte.add ("limitbg", 0.1); +======= + public void periodic() { + +>>>>>>> a50cbc2cf5643bf54839bb1e5520aa0bf79cd925 } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 58d30f8..abdddaf 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -21,6 +21,8 @@ public class Limelight extends SubsystemBase { PhotonCamera limelight = new PhotonCamera("limelight"); /** Creates a new Limelight. */ public Limelight() { + CameraServer.startAutomaticCapture(); + teb .add("limelight", 0.1); PortForwarder.add(5800, "photonvision.local", 5800); } @@ -58,7 +60,5 @@ public class Limelight extends SubsystemBase { } @Override public void periodic() { - CameraServer.startAutomaticCapture(); - teb .add("limelight", 0.1); } } diff --git a/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java b/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java index 8ddbd77..e10ed52 100644 --- a/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java +++ b/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java @@ -24,12 +24,15 @@ public class BrasTelescopique extends SubsystemBase { ShuffleboardLayout layout = Shuffleboard.getTab("teb") .getLayout("layout", BuiltInLayouts.kList) .withSize(2, 2); + ShuffleboardLayout bras = Shuffleboard.getTab("teb") + .getLayout("bras", BuiltInLayouts.kList) + .withSize(2, 2); -ShuffleboardLayout bras = Shuffleboard.getTab("teb") -.getLayout("bras", BuiltInLayouts.kList) -.withSize(2, 2); /** Creates a new BrasTelescopique. */ - public BrasTelescopique() {} + public BrasTelescopique() { + teb .add("photocell",0.1); + teb .add("winch",0.1); + bras.add ("encodeur",0.1);} final CANSparkMax Winch = new CANSparkMax(Constants.BrasTelescopique,MotorType.kBrushless); private DigitalInput photocell = new DigitalInput(Constants.photocell); private DoubleSolenoid brakewinch = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,Constants.brakewinchf, Constants.brakewinchb); @@ -53,8 +56,6 @@ ShuffleboardLayout bras = Shuffleboard.getTab("teb") } @Override public void periodic() { - teb .add("photocell",0.1); - teb .add("winch",0.1); - bras.add ("encodeur",0.1); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/bras/Pince.java b/src/main/java/frc/robot/subsystems/bras/Pince.java index 62022de..6044d4c 100644 --- a/src/main/java/frc/robot/subsystems/bras/Pince.java +++ b/src/main/java/frc/robot/subsystems/bras/Pince.java @@ -8,8 +8,6 @@ import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.robot.Constants; diff --git a/src/main/java/frc/robot/subsystems/bras/Pivot.java b/src/main/java/frc/robot/subsystems/bras/Pivot.java index ad36e3a..7fa1c28 100644 --- a/src/main/java/frc/robot/subsystems/bras/Pivot.java +++ b/src/main/java/frc/robot/subsystems/bras/Pivot.java @@ -14,7 +14,8 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; public class Pivot extends SubsystemBase { - ShuffleboardTab teb = Shuffleboard.getTab("teb"); + ShuffleboardTab teb = Shuffleboard.getTab("teb"); + // moteur private CANSparkMax pivot = new CANSparkMax(Constants.pivot,MotorType.kBrushless); private DigitalInput limitpivot = new DigitalInput(Constants.limitpivot); @@ -23,6 +24,10 @@ public class Pivot extends SubsystemBase { public void monteDescendre(double vitesse) { pivot.set (vitesse); } + public Pivot(){ + teb .add ("encodeurpivot",0.1); + teb .add ("limitpivot",0.1); + } // encodeur public double distance(){ return (pivot.getEncoder().getPosition()); @@ -36,9 +41,6 @@ public class Pivot extends SubsystemBase { } @Override public void periodic() { - teb .add("encodeur", 0.1); } - { - teb.add ("encodeur pivot",0.1); - } -} +} +