This commit is contained in:
samuel desharnais 2024-11-04 17:47:30 -05:00
commit 810ea210b1
8 changed files with 19 additions and 36 deletions

View File

@ -3,7 +3,6 @@
// the WPILib BSD license file in the root directory of this project.
package frc.robot.Commands;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Subsystems.Accumulateur;
@ -15,25 +14,21 @@ public class Desaccumuler extends Command {
addRequirements(accumulateur);
// 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() {
accumulateur.desaccumule(0.1);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
accumulateur.desaccumule(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {

View File

@ -3,9 +3,7 @@
// the WPILib BSD license file in the root directory of this project.
package frc.robot.Commands;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Subsystems.Drive;
import frc.robot.Subsystems.Lanceur;
import frc.robot.Subsystems.Limelight3G;
@ -18,7 +16,6 @@ public class FollowAprilTag extends Command {
// Use addRequirements() here to declare subsystem dependencies.
this.lanceur = lanceur;
this.enlignement = enlignement;
addRequirements(lanceur, enlignement);
}
@ -32,11 +29,11 @@ public class FollowAprilTag extends Command {
if (enlignement.getv()==1)
{
drive.drive(0, 0, enlignement.getx()/30);
lanceur.tourelRotation(0,0, enlignement.getx()/30);
}
else
{
drive.drive(0, 0, 0);
lanceur.tourelRotation(0, 0, 0);
}
}
@ -44,9 +41,8 @@ public class FollowAprilTag extends Command {
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
drive.drive(0, 0, 0);
lanceur.tourelRotation(0, 0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {

View File

@ -3,7 +3,6 @@
// the WPILib BSD license file in the root directory of this project.
package frc.robot.Commands;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Subsystems.Lanceur;
@ -15,25 +14,21 @@ public class Lancer extends Command {
addRequirements(lanceur);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
lanceur.PIDlanceur(0, 0, 0);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
lanceur.lance(0.5);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
lanceur.lance(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {

View File

@ -16,14 +16,12 @@ import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation2d;
import java.io.IOException;
import java.net.HttpURLConnection;
import java.net.MalformedURLException;
import java.net.URL;
import java.util.Map;
import java.util.concurrent.CompletableFuture;
import com.fasterxml.jackson.annotation.JsonFormat;
import com.fasterxml.jackson.annotation.JsonFormat.Shape;
import com.fasterxml.jackson.annotation.JsonProperty;

View File

@ -3,15 +3,11 @@
// the WPILib BSD license file in the root directory of this project.
package frc.robot.Subsystems;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
public class Accumulateur extends SubsystemBase {
@ -19,7 +15,6 @@ public class Accumulateur extends SubsystemBase {
public Accumulateur() {dashboard.addBoolean("photocellacc", this::limitswitch)
.withSize(1, 1)
.withPosition(0, 1);
}
ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
@ -44,7 +39,6 @@ public class Accumulateur extends SubsystemBase {
public void desaccumule(){
desaccumule(vitesse.getDouble(0.9));
}
@Override
public void periodic() {
// This method will be called once per scheduler run

View File

@ -3,12 +3,9 @@
// the WPILib BSD license file in the root directory of this project.
package frc.robot.Subsystems;
import java.io.File;
import java.io.IOException;
import com.ctre.phoenix6.hardware.Pigeon2;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.wpilibj.Filesystem;
@ -16,7 +13,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
import swervelib.SwerveDrive;
import swervelib.parser.SwerveParser;
public class Drive extends SubsystemBase {
SwerveDrive swerveDrive;

View File

@ -12,22 +12,28 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkLowLevel.MotorType;
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.SubsystemBase;
public class Lanceur extends SubsystemBase {
/** Creates a new Lanceur. */
public Lanceur() {}
ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
final WPI_TalonSRX lanceur1 = new WPI_TalonSRX(0);
final WPI_TalonSRX lanceur2 = new WPI_TalonSRX(1);
final CANSparkMax tourelle = new CANSparkMax(2, MotorType.kBrushed);
private GenericEntry vitesse =
dashboard.add("vitesselanceur", 0.2)
.withSize(0,0)
.withPosition(1, 4)
.getEntry();
public void encodeur(double distance){
lanceur1.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
lanceur2.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
lanceur1.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1);
}
public void masterslave(){
lanceur2.follow(lanceur1);
@ -36,8 +42,11 @@ public class Lanceur extends SubsystemBase {
public void lance(double vitesse){
lanceur1.set(vitesse);
}
public void tourelRotation(double vitesse){
tourelle.set(vitesse);
public void lance(){
lance(vitesse.getDouble(0.2));
}
public void tourelRotation(double x, double y, double rotation){
tourelle.set(rotation);
}
public double distancetourel(){
return(tourelle.getEncoder().getPosition());

View File

@ -9,9 +9,9 @@ import edu.wpi.first.net.PortForwarder;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
public class Limelight3G extends SubsystemBase {
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
NetworkTableEntry tx = table.getEntry("tx");
NetworkTableEntry ty = table.getEntry("ty");
NetworkTableEntry pipeline = table.getEntry("pipeline");