From c2fbb7f55394dedce667b47ba79d7469c560d4ea Mon Sep 17 00:00:00 2001 From: OlivierDubois Date: Wed, 8 Feb 2023 19:16:36 -0500 Subject: [PATCH 1/2] Messa --- src/main/java/frc/robot/Constants.java | 1 + .../frc/robot/subsystems/BasePilotable.java | 16 ++++++++++++++++ src/main/java/frc/robot/subsystems/Gratte.java | 17 +++++++++++++++++ .../java/frc/robot/subsystems/bras/Pince.java | 3 --- .../java/frc/robot/subsystems/bras/Pivot.java | 18 ++++++++++++++++-- 5 files changed, 50 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Gratte.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dea7d5a..426f892 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,4 +14,5 @@ public class Constants { + public static int actuateur = 4; } diff --git a/src/main/java/frc/robot/subsystems/BasePilotable.java b/src/main/java/frc/robot/subsystems/BasePilotable.java index 53ef9ba..1379188 100644 --- a/src/main/java/frc/robot/subsystems/BasePilotable.java +++ b/src/main/java/frc/robot/subsystems/BasePilotable.java @@ -22,6 +22,22 @@ public class BasePilotable extends SubsystemBase { final DifferentialDrive drive = new DifferentialDrive(gauche, droit); + public void drive(double xSpeed, double zRotation){ + drive.arcadeDrive(xSpeed, zRotation); + } + public double distance(){ + return (-avantdroit.getEncoder().getPosition() + +avantgauche.getEncoder().getPosition() + -arrieredroit.getEncoder().getPosition() + +arrieregauche.getEncoder().getPosition()) / 4; + } + public void Reset() { + avantdroit.getEncoder().setPosition(0); + avantgauche.getEncoder().setPosition(0); + arrieredroit.getEncoder().setPosition(0); + arrieregauche.getEncoder().setPosition(0); + } + /** Creates a new BasePilotable. */ public BasePilotable() { droit.setInverted(true); diff --git a/src/main/java/frc/robot/subsystems/Gratte.java b/src/main/java/frc/robot/subsystems/Gratte.java new file mode 100644 index 0000000..5baa6b8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Gratte.java @@ -0,0 +1,17 @@ +// 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.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Gratte extends SubsystemBase { + /** Creates a new Gratte. */ + public Gratte() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/bras/Pince.java b/src/main/java/frc/robot/subsystems/bras/Pince.java index 03e643c..a933219 100644 --- a/src/main/java/frc/robot/subsystems/bras/Pince.java +++ b/src/main/java/frc/robot/subsystems/bras/Pince.java @@ -14,15 +14,12 @@ public class Pince extends SubsystemBase { private DoubleSolenoid pistonPince = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,Constants.pistonpinceouvre, Constants.pistonpinceferme); /** Creates a new Pince. */ public Pince() { - } public void fermer() { pistonPince.set(Value.kReverse); - } public void ouvrir() { pistonPince.set(Value.kForward); - } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/bras/Pivot.java b/src/main/java/frc/robot/subsystems/bras/Pivot.java index 2742df8..e8d5dbb 100644 --- a/src/main/java/frc/robot/subsystems/bras/Pivot.java +++ b/src/main/java/frc/robot/subsystems/bras/Pivot.java @@ -5,11 +5,25 @@ package frc.robot.subsystems.bras; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import static frc.robot.Constants.*; public class Pivot extends SubsystemBase { - /** Creates a new Pivot. */ - public Pivot() {} + // moteur + private CANSparkMax pivot = new CANSparkMax(actuateur, MotorType.kBrushless); + // function + public void monteDescendre(double vitesse) { + pivot.set (vitesse); + } + // encoder + public double distance(){ + return (pivot.getEncoder().getPosition()); + } + public void Reset(){ + pivot.getEncoder().setPosition(0); + } @Override public void periodic() { // This method will be called once per scheduler run From 077494b9fd8ae6c50c7b526c0e45689870d7938c Mon Sep 17 00:00:00 2001 From: OlivierDubois Date: Wed, 8 Feb 2023 19:55:20 -0500 Subject: [PATCH 2/2] Message constuctif --- src/main/java/frc/robot/Constants.java | 8 +++++ .../java/frc/robot/subsystems/Gratte.java | 30 ++++++++++++++++++- 2 files changed, 37 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 426f892..4cf8973 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -5,9 +5,17 @@ public class Constants { public static int avantgauche = 1; public static int arrieredroit = 2; public static int arrieregauche = 3; + public static int leverGratte = 0; + public static int baisserGratte = 1; // pneumatique public static int pistonpinceouvre = 0; public static int pistonpinceferme = 1; + // DIO + public static int limitbd = 0; + public static int limitbg = 2; + public static int limithd = 3; + public static int limithg = 1; + diff --git a/src/main/java/frc/robot/subsystems/Gratte.java b/src/main/java/frc/robot/subsystems/Gratte.java index 5baa6b8..50adfbb 100644 --- a/src/main/java/frc/robot/subsystems/Gratte.java +++ b/src/main/java/frc/robot/subsystems/Gratte.java @@ -4,12 +4,40 @@ package frc.robot.subsystems; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.fasterxml.jackson.annotation.JacksonInject.Value; + +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; public class Gratte extends SubsystemBase { + private WPI_TalonSRX baisserGratte = new WPI_TalonSRX(Constants.leverGratte); + private WPI_TalonSRX leverGratte = new WPI_TalonSRX(Constants.baisserGratte); + private DigitalInput limithd = new DigitalInput(Constants.limithd); + private DigitalInput limithg = new DigitalInput(Constants.limithg); + private DigitalInput limitbd = new DigitalInput(Constants.limitbd); + private DigitalInput limitbg = new DigitalInput(Constants.limitbg); + public boolean hautd(){ + return limithd.get(); + } + + public boolean hautg(){ + return limithg.get(); + } + + public boolean basd(){ + return limitbd.get(); + } + public boolean basg(){ + return limitbg.get(); + } /** Creates a new Gratte. */ public Gratte() {} - + public void BaisserLever(double vitesse){ + baisserGratte.set(vitesse); + leverGratte.set(vitesse); + } @Override public void periodic() { // This method will be called once per scheduler run