mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-08 16:28:04 -06:00
Add elevator to advantatekit
This commit is contained in:
Michael Mikovsky
@@ -53,10 +53,10 @@ import frc4388.robot.constants.Constants.SimConstants.Mode;
|
||||
import com.pathplanner.lib.auto.NamedCommands;
|
||||
import com.pathplanner.lib.commands.PathPlannerAuto;
|
||||
|
||||
import frc4388.robot.subsystems.Elevator;
|
||||
// Subsystems
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.Elevator.CoordinationState;
|
||||
import frc4388.robot.subsystems.elevator.Elevator;
|
||||
import frc4388.robot.subsystems.elevator.Elevator.CoordinationState;
|
||||
import frc4388.robot.subsystems.lidar.LiDAR;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
@@ -80,7 +80,7 @@ public class RobotContainer {
|
||||
/* Subsystems */
|
||||
public final LED m_robotLED = new LED();
|
||||
public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
|
||||
public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED);
|
||||
public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED);
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
|
||||
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
|
||||
|
||||
|
||||
@@ -21,14 +21,16 @@ import frc4388.robot.constants.Constants.ElevatorConstants;
|
||||
import frc4388.robot.constants.Constants.LiDARConstants;
|
||||
import frc4388.robot.constants.Constants.SimConstants;
|
||||
import frc4388.robot.constants.Constants.VisionConstants;
|
||||
import frc4388.robot.subsystems.elevator.ElevatorIO;
|
||||
import frc4388.robot.subsystems.elevator.ElevatorReal;
|
||||
import frc4388.robot.subsystems.lidar.LiDAR;
|
||||
import frc4388.robot.subsystems.lidar.LidarIO;
|
||||
import frc4388.robot.subsystems.lidar.LidarLiteV2;
|
||||
import frc4388.robot.subsystems.lidar.LidarReal;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
|
||||
import frc4388.robot.subsystems.swerve.SwerveIO;
|
||||
import frc4388.robot.subsystems.swerve.SwervePhoenix;
|
||||
import frc4388.robot.subsystems.swerve.SwerveReal;
|
||||
import frc4388.robot.subsystems.vision.VisionIO;
|
||||
import frc4388.robot.subsystems.vision.VisionPhotonvision;
|
||||
import frc4388.robot.subsystems.vision.VisionReal;
|
||||
import frc4388.utility.status.FaultCANCoder;
|
||||
import frc4388.utility.status.FaultPhotonCamera;
|
||||
import frc4388.utility.status.FaultPidgeon2;
|
||||
@@ -58,13 +60,7 @@ public class RobotMap {
|
||||
public final SwerveIO swerveDrivetrain;
|
||||
|
||||
/* Elevator Subsystem */
|
||||
public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
|
||||
public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
|
||||
|
||||
|
||||
public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
|
||||
public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
|
||||
public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
|
||||
public final ElevatorIO elevatorIO;
|
||||
|
||||
public RobotMap(SimConstants.Mode mode) {
|
||||
switch (mode) {
|
||||
@@ -73,28 +69,40 @@ public class RobotMap {
|
||||
PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
|
||||
PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
|
||||
|
||||
leftCamera = new VisionPhotonvision(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ;
|
||||
rightCamera = new VisionPhotonvision(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS);
|
||||
leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ;
|
||||
rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS);
|
||||
|
||||
FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
|
||||
FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
|
||||
|
||||
// Configure LiDAR
|
||||
reefLidar = new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL);
|
||||
reverseLidar = new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL);
|
||||
reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL);
|
||||
reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL);
|
||||
|
||||
// Configure swerve drive train
|
||||
|
||||
SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrainReal = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new,
|
||||
SwerveDriveConstants.DrivetrainConstants,
|
||||
SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT,
|
||||
SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT
|
||||
);
|
||||
|
||||
swerveDrivetrain = new SwervePhoenix(swerveDrivetrainReal);
|
||||
swerveDrivetrain = new SwerveReal(swerveDrivetrainReal);
|
||||
|
||||
// Configure elevator
|
||||
|
||||
TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
|
||||
TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
|
||||
|
||||
|
||||
DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
|
||||
DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
|
||||
DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
|
||||
|
||||
elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam);
|
||||
|
||||
|
||||
|
||||
// Fault
|
||||
FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
|
||||
|
||||
FaultTalonFX.addDevice(elevator, "Elevator");
|
||||
@@ -122,6 +130,7 @@ public class RobotMap {
|
||||
reefLidar = new LidarIO() {};
|
||||
reverseLidar = new LidarIO() {};
|
||||
swerveDrivetrain = new SwerveIO() {};
|
||||
elevatorIO = new ElevatorIO() {};
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -5,7 +5,7 @@
|
||||
package frc4388.robot.commands.wait;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.Elevator;
|
||||
import frc4388.robot.subsystems.elevator.Elevator;
|
||||
|
||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
||||
public class waitElevatorRefrence extends Command {
|
||||
|
||||
@@ -5,7 +5,7 @@
|
||||
package frc4388.robot.commands.wait;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.Elevator;
|
||||
import frc4388.robot.subsystems.elevator.Elevator;
|
||||
|
||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
||||
public class waitEndefectorRefrence extends Command {
|
||||
|
||||
@@ -5,7 +5,7 @@
|
||||
package frc4388.robot.commands.wait;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.Elevator;
|
||||
import frc4388.robot.subsystems.elevator.Elevator;
|
||||
|
||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
||||
public class waitFeedCoral extends Command {
|
||||
|
||||
@@ -7,12 +7,12 @@ public final class BuildConstants {
|
||||
public static final String MAVEN_GROUP = "";
|
||||
public static final String MAVEN_NAME = "2025RidgeScape";
|
||||
public static final String VERSION = "unspecified";
|
||||
public static final int GIT_REVISION = 172;
|
||||
public static final String GIT_SHA = "8e34bfe35427e1fb86ab2af21dd706d387bf4874";
|
||||
public static final String GIT_DATE = "2025-07-15 13:42:25 MDT";
|
||||
public static final int GIT_REVISION = 173;
|
||||
public static final String GIT_SHA = "2b7bb1224195b9e001e60b895ee04d63abdfc513";
|
||||
public static final String GIT_DATE = "2025-07-17 09:15:19 MDT";
|
||||
public static final String GIT_BRANCH = "advantagekit";
|
||||
public static final String BUILD_DATE = "2025-07-16 16:20:33 MDT";
|
||||
public static final long BUILD_UNIX_TIME = 1752704433011L;
|
||||
public static final String BUILD_DATE = "2025-07-17 11:55:31 MDT";
|
||||
public static final long BUILD_UNIX_TIME = 1752774931371L;
|
||||
public static final int DIRTY = 1;
|
||||
|
||||
private BuildConstants(){}
|
||||
|
||||
@@ -1,107 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix6.controls.Follower;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
|
||||
// import frc4388.utility.RobotGyro;
|
||||
import frc4388.utility.compute.RobotTime;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class DiffDrive extends SubsystemBase implements Queryable {
|
||||
// Put methods for controlling this subsystem
|
||||
// here. Call these from Commands.
|
||||
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
|
||||
private TalonFX m_leftFrontMotor;
|
||||
private TalonFX m_rightFrontMotor;
|
||||
private TalonFX m_leftBackMotor;
|
||||
private TalonFX m_rightBackMotor;
|
||||
private DifferentialDrive m_driveTrain;
|
||||
// private Pigeon2 m_gyro;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor,
|
||||
TalonFX rightBackMotor, DifferentialDrive driveTrain, Pigeon2 gyro) {
|
||||
|
||||
FaultReporter.register(this);
|
||||
|
||||
m_leftFrontMotor = leftFrontMotor;
|
||||
m_rightFrontMotor = rightFrontMotor;
|
||||
m_leftBackMotor = leftBackMotor;
|
||||
m_rightBackMotor = rightBackMotor;
|
||||
m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false));
|
||||
m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false));
|
||||
m_driveTrain = driveTrain;
|
||||
// m_gyro = gyro;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
if (m_robotTime.m_frameNumber % SwerveDriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
|
||||
updateSmartDashboard();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public void driveWithInput(double move, double steer) {
|
||||
m_driveTrain.arcadeDrive(move, steer);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public void tankDriveWithInput(double leftMove, double rightMove) {
|
||||
m_leftFrontMotor.set(leftMove);
|
||||
m_rightFrontMotor.set(rightMove);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
private void updateSmartDashboard() {
|
||||
|
||||
// Examples of the functionality of RobotGyro
|
||||
// SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
|
||||
// SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
|
||||
// SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
|
||||
//SmartDashboard.putData(m_gyro);
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return "Diff Drive";
|
||||
}
|
||||
|
||||
// @Override
|
||||
// public void queryStatus() {
|
||||
// // TODO: Add Stuff
|
||||
// }
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
// Log("Diagnostic info for this has not been inplemented!"); //TODO
|
||||
return new Status();
|
||||
}
|
||||
}
|
||||
+94
-112
@@ -2,7 +2,10 @@
|
||||
// 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 frc4388.robot.subsystems;
|
||||
package frc4388.robot.subsystems.elevator;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLogOutput;
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.ctre.phoenix6.controls.PositionDutyCycle;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
@@ -14,38 +17,31 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.AutoConstants;
|
||||
import frc4388.robot.constants.Constants.ElevatorConstants;
|
||||
import frc4388.robot.constants.Constants.LEDConstants;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.elevator.ElevatorIO.ElevatorState;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
|
||||
public class Elevator extends SubsystemBase implements Queryable {
|
||||
ElevatorIO io;
|
||||
ElevatorStateAutoLogged state = new ElevatorStateAutoLogged();
|
||||
|
||||
/** Creates a new Elevator. */
|
||||
private TalonFX elevatorMotor;
|
||||
private TalonFX endeffectorMotor;
|
||||
private LED led;
|
||||
|
||||
// @AutoLog
|
||||
// private class ElevatorState {
|
||||
@SuppressWarnings("unused")
|
||||
public long wait = 0;
|
||||
public long maxWait = 1000;
|
||||
@SuppressWarnings("unused")
|
||||
public long wait = 0;
|
||||
public long maxWait = 1000;
|
||||
|
||||
public double elevatorRefrence = 0;
|
||||
public double endeffectorRefrence = 0;
|
||||
public boolean elevatorManualStop = true;
|
||||
public boolean endefectorManualStop = true;
|
||||
|
||||
public boolean elevatorManualStop = true;
|
||||
public boolean endefectorManualStop = true;
|
||||
public boolean disableAutoIntake = false;
|
||||
|
||||
public boolean disableAutoIntake = false;
|
||||
|
||||
public boolean seededZeroEndefector = false;
|
||||
public boolean seededZeroElevator = false;
|
||||
|
||||
public DigitalInput basinBeamBreak;
|
||||
public DigitalInput endeffectorLimitSwitch;
|
||||
public DigitalInput intakeIR;
|
||||
// }
|
||||
public boolean seededZeroEndefector = false;
|
||||
public boolean seededZeroElevator = false;
|
||||
|
||||
// private ElevatorState state = new ElevatorState();
|
||||
|
||||
@@ -69,43 +65,15 @@ public class Elevator extends SubsystemBase implements Queryable {
|
||||
private CoordinationState currentState;
|
||||
|
||||
// public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
|
||||
public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) {
|
||||
elevatorMotor = elevatorTalonFX;
|
||||
endeffectorMotor = endeffectorTalonFX;
|
||||
public Elevator(ElevatorIO io, LED led) {
|
||||
this.io = io;
|
||||
this.led = led;
|
||||
|
||||
this.basinBeamBreak = basinLimitSwitch;
|
||||
this.endeffectorLimitSwitch = endeffectorLimitSwitch;
|
||||
this.intakeIR = intakeDigitalInput;
|
||||
|
||||
elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
|
||||
endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
|
||||
|
||||
elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
|
||||
endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID);
|
||||
currentState = CoordinationState.Ready;
|
||||
|
||||
FaultReporter.register(this);
|
||||
}
|
||||
|
||||
//PID methods
|
||||
|
||||
private void PIDPosition(TalonFX motor, double position) {
|
||||
if (motor == elevatorMotor) elevatorRefrence = position;
|
||||
else endeffectorRefrence = position;
|
||||
|
||||
var request = new PositionDutyCycle(position);
|
||||
motor.setControl(request);
|
||||
}
|
||||
|
||||
public void elevatorStop() {
|
||||
elevatorMotor.set(0);
|
||||
}
|
||||
|
||||
public void endeffectorStop() {
|
||||
endeffectorMotor.set(0);
|
||||
}
|
||||
|
||||
|
||||
public void transitionState(CoordinationState state) {
|
||||
// elevatorMotor.enable();
|
||||
@@ -115,98 +83,98 @@ public class Elevator extends SubsystemBase implements Queryable {
|
||||
switch (currentState) {
|
||||
case Waiting: {
|
||||
wait = System.currentTimeMillis() + maxWait;
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0));
|
||||
io.elevatorToPosition(ElevatorConstants.WAITING_POSITION_ELEVATOR);
|
||||
io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0));
|
||||
led.setMode(LEDConstants.WAITING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case WatingBeamTripped: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
|
||||
io.elevatorToPosition(ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
|
||||
io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
|
||||
led.setMode(LEDConstants.DOWN_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case Ready: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0));
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
|
||||
io.elevatorToPosition(ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0));
|
||||
io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
|
||||
led.setMode(LEDConstants.DOWN_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case Hovering: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR);
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
|
||||
io.elevatorToPosition(ElevatorConstants.HOVERING_POSITION_ELEVATOR);
|
||||
io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
|
||||
led.setMode(LEDConstants.READY_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case L2Score: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
io.elevatorToPosition(ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
io.endeffectorToPosition(ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case L2ScoreLeave: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
io.elevatorToPosition(ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
io.endeffectorToPosition(ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case PrimedFour: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR);
|
||||
io.elevatorToPosition(ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
io.endeffectorToPosition(ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR);
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case ScoringFour: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
io.elevatorToPosition(ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
io.endeffectorToPosition(ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case PrimedThree: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
|
||||
io.elevatorToPosition(ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
io.endeffectorToPosition(ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case ScoringThree: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
io.elevatorToPosition(ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case BallRemoverL2Primed: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
|
||||
io.elevatorToPosition(ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case BallRemoverL2Go: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
io.elevatorToPosition(ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case BallRemoverL3Primed: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
|
||||
io.elevatorToPosition(ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
|
||||
break;
|
||||
}
|
||||
|
||||
case BallRemoverL3Go: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
io.elevatorToPosition(ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
@@ -223,27 +191,25 @@ public class Elevator extends SubsystemBase implements Queryable {
|
||||
}
|
||||
|
||||
public boolean elevatorAtReference() {
|
||||
// double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
|
||||
double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
|
||||
double diffrence = elevatorRefrence - elevatorPosition;
|
||||
double diffrence = state.elevatorRefrence - state.elevatorPosition;
|
||||
|
||||
boolean headedUp = diffrence < 0;
|
||||
boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0;
|
||||
boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0;
|
||||
|
||||
return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
|
||||
return (Math.abs(diffrence) <= 0.5
|
||||
|| (state.elevatorReverseLimit && headedUp)
|
||||
|| (state.elevatorForwardLimit && !headedUp)
|
||||
);
|
||||
}
|
||||
|
||||
public boolean endeffectorAtReference() {
|
||||
// double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
|
||||
double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble();
|
||||
double diffrence = endeffectorRefrence - endeffectorPosition;
|
||||
double diffrence = state.endeffectorRefrence - state.endeffectorPosition;
|
||||
|
||||
boolean headedUp = diffrence < 0;
|
||||
boolean forwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0;
|
||||
boolean reverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0;
|
||||
|
||||
return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
|
||||
return (Math.abs(diffrence) <= 0.5
|
||||
|| (state.elevatorReverseLimit && headedUp)
|
||||
|| (state.endeffectorForwardLimit && !headedUp)
|
||||
);
|
||||
}
|
||||
// public void driveElevatorStick(Translation2d stick) {
|
||||
// if (stick.getNorm() > 0.05) {
|
||||
@@ -252,11 +218,11 @@ public class Elevator extends SubsystemBase implements Queryable {
|
||||
// }
|
||||
|
||||
public boolean getEndeffectorLimit() {
|
||||
return endeffectorLimitSwitch.get();
|
||||
return state.endeffectorLimitSwitch;
|
||||
}
|
||||
|
||||
private void periodicWaiting() {
|
||||
if (!basinBeamBreak.get())
|
||||
if (!state.basinBeamBreak)
|
||||
transitionState(CoordinationState.Ready);
|
||||
// if(!endeffectorLimitSwitch.get())
|
||||
// transitionState(CoordinationState.Hovering);
|
||||
@@ -268,39 +234,39 @@ public class Elevator extends SubsystemBase implements Queryable {
|
||||
// }
|
||||
|
||||
private void periodicReady() {
|
||||
if (elevatorAtReference() && !endeffectorLimitSwitch.get())
|
||||
if (elevatorAtReference() && !state.endeffectorLimitSwitch)
|
||||
transitionState(CoordinationState.Hovering);
|
||||
if(elevatorAtReference() && endeffectorLimitSwitch.get())
|
||||
if(elevatorAtReference() && state.endeffectorLimitSwitch)
|
||||
transitionState(CoordinationState.Hovering);
|
||||
}
|
||||
|
||||
@SuppressWarnings("unused")
|
||||
private void periodicScoring() {
|
||||
if (!endeffectorLimitSwitch.get())
|
||||
if (!state.endeffectorLimitSwitch)
|
||||
transitionState(CoordinationState.Waiting);
|
||||
}
|
||||
|
||||
public void manualElevatorVel(double velocity) {
|
||||
if (Math.abs(velocity) > 0.1) {
|
||||
elevatorMotor.set(velocity);
|
||||
io.elevatorToVelocity(velocity);
|
||||
elevatorManualStop = false;
|
||||
return;
|
||||
}
|
||||
if (!elevatorManualStop) {
|
||||
elevatorManualStop = true;
|
||||
elevatorMotor.set(0);
|
||||
io.elevatorToVelocity(0);
|
||||
}
|
||||
}
|
||||
|
||||
public void manualEndeffectorVel(double velocity) {
|
||||
if (Math.abs(velocity) > 0.1) {
|
||||
endeffectorMotor.set(velocity);
|
||||
io.endeffectorToVelocity(velocity);
|
||||
endefectorManualStop = false;
|
||||
return;
|
||||
}
|
||||
if (!endefectorManualStop) {
|
||||
endefectorManualStop = true;
|
||||
endeffectorMotor.set(0);
|
||||
io.endeffectorToVelocity(0);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -320,18 +286,21 @@ public class Elevator extends SubsystemBase implements Queryable {
|
||||
// This method will be called once per scheduler run
|
||||
// SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity);
|
||||
// SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque);
|
||||
SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
|
||||
SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
|
||||
SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
|
||||
SmartDashboard.putString("State", currentState.toString());
|
||||
// SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
|
||||
// SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
|
||||
// SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
|
||||
// SmartDashboard.putString("State", currentState.toString());
|
||||
|
||||
if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) {
|
||||
endeffectorMotor.setPosition(0);
|
||||
io.updateInputs(state);
|
||||
Logger.processInputs("Elevator", state);
|
||||
|
||||
if (!seededZeroEndefector && state.endeffectorForwardLimit) {
|
||||
io.endeffectorToPosition(0);
|
||||
seededZeroEndefector = !seededZeroEndefector;
|
||||
}
|
||||
|
||||
if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) {
|
||||
elevatorMotor.setPosition(0);
|
||||
if (!seededZeroElevator && state.elevatorReverseLimit) {
|
||||
io.endeffectorToPosition(0);
|
||||
seededZeroElevator = !seededZeroElevator;
|
||||
}
|
||||
|
||||
@@ -345,7 +314,7 @@ public class Elevator extends SubsystemBase implements Queryable {
|
||||
periodicReady();
|
||||
}
|
||||
|
||||
if(!intakeIR.get()){
|
||||
if(!state.intakeIR){
|
||||
led.setMode(LEDConstants.DOWN_PATTERN);
|
||||
}
|
||||
|
||||
@@ -355,6 +324,11 @@ public class Elevator extends SubsystemBase implements Queryable {
|
||||
// }
|
||||
}
|
||||
|
||||
@AutoLogOutput(key="Elevator/state")
|
||||
public String getState() {
|
||||
return currentState.toString();
|
||||
}
|
||||
|
||||
public boolean isL4Primed() {
|
||||
return currentState == CoordinationState.PrimedFour;
|
||||
}
|
||||
@@ -364,16 +338,24 @@ public class Elevator extends SubsystemBase implements Queryable {
|
||||
}
|
||||
|
||||
public boolean hasCoral() {
|
||||
return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get();
|
||||
return elevatorAtReference() && currentState == CoordinationState.Hovering || !state.endeffectorLimitSwitch;
|
||||
}
|
||||
|
||||
public void elevatorStop() {
|
||||
io.elevatorToVelocity(0);
|
||||
}
|
||||
|
||||
public void endeffectorStop() {
|
||||
io.endeffectorToVelocity(0);
|
||||
}
|
||||
|
||||
public boolean readyToMove() {
|
||||
return !intakeIR.get() || hasCoral() || !endeffectorLimitSwitch.get();
|
||||
return !state.intakeIR || hasCoral() || !state.endeffectorLimitSwitch;
|
||||
// return hasCoral();
|
||||
}
|
||||
|
||||
public void armShuffle(){
|
||||
if(!basinBeamBreak.get()){
|
||||
if(!state.basinBeamBreak){
|
||||
//shuffle the coral with the arm until coral hits beam break
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
package frc4388.robot.subsystems.elevator;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import frc4388.robot.subsystems.lidar.LidarIO.LidarState;
|
||||
|
||||
public interface ElevatorIO {
|
||||
@AutoLog
|
||||
public class ElevatorState {
|
||||
public double elevatorRefrence;
|
||||
public double elevatorPosition;
|
||||
public boolean elevatorForwardLimit;
|
||||
public boolean elevatorReverseLimit;
|
||||
|
||||
public double endeffectorRefrence;
|
||||
public double endeffectorPosition;
|
||||
public boolean endeffectorForwardLimit;
|
||||
public boolean endeffectorReverseLimit;
|
||||
|
||||
|
||||
public boolean basinBeamBreak;
|
||||
public boolean endeffectorLimitSwitch;
|
||||
public boolean intakeIR;
|
||||
|
||||
}
|
||||
|
||||
public default void elevatorToPosition(double position) {}
|
||||
public default void endeffectorToPosition(double position) {}
|
||||
public default void elevatorToVelocity(double velocity) {}
|
||||
public default void endeffectorToVelocity(double velocity) {}
|
||||
|
||||
public default void updateInputs(ElevatorState state) {}
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
package frc4388.robot.subsystems.elevator;
|
||||
|
||||
import com.ctre.phoenix6.controls.PositionDutyCycle;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import frc4388.robot.constants.Constants.ElevatorConstants;
|
||||
|
||||
public class ElevatorReal implements ElevatorIO {
|
||||
TalonFX elevatorMotor;
|
||||
TalonFX endeffectorMotor;
|
||||
|
||||
DigitalInput basinLimitSwitch;
|
||||
DigitalInput endeffectorLimitSwitch;
|
||||
DigitalInput intakeIR;
|
||||
|
||||
|
||||
double elevatorRefrence = 0;
|
||||
double endeffectorRefrence = 0;
|
||||
|
||||
public ElevatorReal(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeIR) {
|
||||
this.elevatorMotor = elevatorTalonFX;
|
||||
this.endeffectorMotor = endeffectorTalonFX;
|
||||
|
||||
this.basinLimitSwitch = basinLimitSwitch;
|
||||
this.endeffectorLimitSwitch = endeffectorLimitSwitch;
|
||||
this.intakeIR = intakeIR;
|
||||
|
||||
|
||||
elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
|
||||
endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
|
||||
|
||||
elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
|
||||
endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void updateInputs(ElevatorState state) {
|
||||
state.elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
|
||||
state.elevatorRefrence = elevatorRefrence;
|
||||
state.elevatorForwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0;
|
||||
state.elevatorReverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0;
|
||||
|
||||
state.endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble();
|
||||
state.endeffectorRefrence = endeffectorRefrence;
|
||||
state.endeffectorForwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0;
|
||||
state.endeffectorReverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0;
|
||||
|
||||
|
||||
state.basinBeamBreak = basinLimitSwitch.get();
|
||||
state.endeffectorLimitSwitch = endeffectorLimitSwitch.get();
|
||||
state.intakeIR = intakeIR.get();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void elevatorToPosition(double position) {
|
||||
elevatorRefrence = position;
|
||||
var request = new PositionDutyCycle(position);
|
||||
elevatorMotor.setControl(request);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void endeffectorToPosition(double position) {
|
||||
endeffectorRefrence = position;
|
||||
var request = new PositionDutyCycle(position);
|
||||
endeffectorMotor.setControl(request);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void elevatorToVelocity(double velocity) {
|
||||
elevatorMotor.set(velocity);
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void endeffectorToVelocity(double velocity) {
|
||||
endeffectorMotor.set(velocity);
|
||||
}
|
||||
}
|
||||
+2
-2
@@ -4,12 +4,12 @@ import edu.wpi.first.wpilibj.Counter;
|
||||
import frc4388.robot.constants.Constants.LiDARConstants;
|
||||
|
||||
// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
|
||||
public class LidarLiteV2 implements LidarIO {
|
||||
public class LidarReal implements LidarIO {
|
||||
|
||||
|
||||
private Counter LidarPWM;
|
||||
|
||||
public LidarLiteV2(int port) {
|
||||
public LidarReal(int port) {
|
||||
LidarPWM = new Counter(port);
|
||||
LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
|
||||
LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement
|
||||
@@ -4,16 +4,9 @@
|
||||
|
||||
package frc4388.robot.subsystems.swerve;
|
||||
|
||||
import java.util.Optional;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLogOutput;
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
|
||||
import com.ctre.phoenix6.swerve.SwerveModule;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
@@ -22,7 +15,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.AutoConstants;
|
||||
import frc4388.robot.subsystems.swerve.SwerveIO.SwerveState;
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
import frc4388.utility.compute.TimesNegativeOne;
|
||||
import frc4388.utility.status.Status;
|
||||
@@ -56,7 +48,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
|
||||
public double lastOdomSpeed;
|
||||
|
||||
public Pose2d initalPose2d = null;
|
||||
public Pose2d initalPose2d = new Pose2d();
|
||||
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
@@ -353,9 +345,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
SmartDashboard.putNumber("RotTartget", rotTarget);
|
||||
|
||||
io.updateInputs(state);
|
||||
Logger.processInputs("SwerveDrive", state);
|
||||
|
||||
vision.setLastOdomPose(state.currentPose);
|
||||
setLastOdomSpeed(state.currentPose, state.lastPose, state.frequency);
|
||||
setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
|
||||
|
||||
if (vision.isTag()) {
|
||||
Pose2d pose = vision.getPose2d();
|
||||
@@ -448,6 +441,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq;
|
||||
}
|
||||
}
|
||||
|
||||
@AutoLogOutput(key="SwerveDrive/speed ")
|
||||
public double getOdometrySpeed() {
|
||||
return lastOdomSpeed;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -16,7 +16,7 @@ public interface SwerveIO {
|
||||
public Pose2d currentPose = null;
|
||||
public Pose2d lastPose = null;
|
||||
public ChassisSpeeds speeds = null;
|
||||
public double frequency = 1;
|
||||
public double odometryRate = 1;
|
||||
}
|
||||
|
||||
public default void setControl(SwerveRequest ctrl) {}
|
||||
|
||||
+4
-6
@@ -10,15 +10,13 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrain;
|
||||
import com.ctre.phoenix6.swerve.SwerveModule;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import frc4388.robot.subsystems.lidar.LidarIO.LidarState;
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
||||
|
||||
public class SwervePhoenix implements SwerveIO {
|
||||
public class SwerveReal implements SwerveIO {
|
||||
SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
|
||||
|
||||
public SwervePhoenix(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
|
||||
public SwerveReal(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
|
||||
this.swerveDriveTrain = swerveDriveTrain;
|
||||
swerveDriveTrain.getOdometryFrequency();
|
||||
}
|
||||
@@ -26,9 +24,9 @@ public class SwervePhoenix implements SwerveIO {
|
||||
@Override
|
||||
public void updateInputs(SwerveState state) {
|
||||
double time = Vision.getTime();
|
||||
state.frequency = swerveDriveTrain.getOdometryFrequency();
|
||||
state.odometryRate = 1 / swerveDriveTrain.getOdometryFrequency();
|
||||
state.currentPose = swerveDriveTrain.samplePoseAt(time).orElse(null);
|
||||
state.lastPose = swerveDriveTrain.samplePoseAt(time - state.frequency).orElse(null);
|
||||
state.lastPose = swerveDriveTrain.samplePoseAt(time - state.odometryRate).orElse(null);
|
||||
state.speeds = swerveDriveTrain.getState().Speeds;
|
||||
}
|
||||
|
||||
+2
-2
@@ -11,7 +11,7 @@ import org.photonvision.targeting.PhotonPipelineResult;
|
||||
import frc4388.robot.constants.Constants.FieldConstants;
|
||||
import frc4388.robot.constants.Constants.VisionConstants;
|
||||
|
||||
public class VisionPhotonvision implements VisionIO {
|
||||
public class VisionReal implements VisionIO {
|
||||
// private PhotonCamera[] cameras;
|
||||
// private PhotonPoseEstimator[] estimators;
|
||||
|
||||
@@ -21,7 +21,7 @@ public class VisionPhotonvision implements VisionIO {
|
||||
// public List<EstimatedRobotPose> poses = new ArrayList<>();
|
||||
|
||||
|
||||
public VisionPhotonvision(PhotonCamera camera, Transform3d position){
|
||||
public VisionReal(PhotonCamera camera, Transform3d position){
|
||||
this.camera = camera;
|
||||
estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position);
|
||||
estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
|
||||
Reference in New Issue
Block a user