mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
Merge branch 'master' of https://github.com/Team4388/2026KPopRobotHunters
This commit is contained in:
@@ -30,7 +30,14 @@ import frc4388.utility.controller.VirtualController;
|
|||||||
import frc4388.utility.controller.XboxController;
|
import frc4388.utility.controller.XboxController;
|
||||||
import frc4388.robot.commands.MoveForTimeCommand;
|
import frc4388.robot.commands.MoveForTimeCommand;
|
||||||
import frc4388.robot.commands.alignment.RotTo45;
|
import frc4388.robot.commands.alignment.RotTo45;
|
||||||
|
import frc4388.robot.commands.MoveUntilSuply;
|
||||||
|
// import frc4388.robot.commands.alignment.DriveToReef;
|
||||||
|
// import frc4388.robot.commands.wait.waitElevatorRefrence;
|
||||||
|
// import frc4388.robot.commands.wait.waitEndefectorRefrence;
|
||||||
|
// import frc4388.robot.commands.wait.waitFeedCoral;
|
||||||
|
import frc4388.robot.commands.wait.waitSupplier;
|
||||||
import frc4388.robot.constants.Constants;
|
import frc4388.robot.constants.Constants;
|
||||||
|
import frc4388.robot.constants.Constants.AutoConstants;
|
||||||
import frc4388.robot.constants.Constants.OIConstants;
|
import frc4388.robot.constants.Constants.OIConstants;
|
||||||
import frc4388.robot.constants.Constants.SimConstants.Mode;
|
import frc4388.robot.constants.Constants.SimConstants.Mode;
|
||||||
|
|
||||||
|
|||||||
@@ -18,14 +18,10 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrain;
|
|||||||
import edu.wpi.first.math.geometry.Rotation2d;
|
import edu.wpi.first.math.geometry.Rotation2d;
|
||||||
import edu.wpi.first.wpilibj.DigitalInput;
|
import edu.wpi.first.wpilibj.DigitalInput;
|
||||||
//import frc4388.robot.constants.Constants.ElevatorConstants;
|
//import frc4388.robot.constants.Constants.ElevatorConstants;
|
||||||
import frc4388.robot.constants.Constants.LiDARConstants;
|
|
||||||
import frc4388.robot.constants.Constants.SimConstants;
|
import frc4388.robot.constants.Constants.SimConstants;
|
||||||
import frc4388.robot.constants.Constants.VisionConstants;
|
import frc4388.robot.constants.Constants.VisionConstants;
|
||||||
// import frc4388.robot.subsystems.elevator.ElevatorIO;
|
// import frc4388.robot.subsystems.elevator.ElevatorIO;
|
||||||
// import frc4388.robot.subsystems.elevator.ElevatorReal;
|
// import frc4388.robot.subsystems.elevator.ElevatorReal;
|
||||||
import frc4388.robot.subsystems.lidar.LiDAR;
|
|
||||||
import frc4388.robot.subsystems.lidar.LidarIO;
|
|
||||||
import frc4388.robot.subsystems.lidar.LidarReal;
|
|
||||||
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
|
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
|
||||||
import frc4388.robot.subsystems.swerve.SwerveIO;
|
import frc4388.robot.subsystems.swerve.SwerveIO;
|
||||||
import frc4388.robot.subsystems.swerve.SwerveReal;
|
import frc4388.robot.subsystems.swerve.SwerveReal;
|
||||||
|
|||||||
@@ -1,48 +0,0 @@
|
|||||||
package frc4388.robot.commands.alignment;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Translation2d;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc4388.robot.subsystems.lidar.LiDAR;
|
|
||||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
|
||||||
|
|
||||||
// Command to repeat a joystick movement for a specific time.
|
|
||||||
public class DriveUntilLiDAR extends Command {
|
|
||||||
private final SwerveDrive swerveDrive;
|
|
||||||
private final Translation2d leftStick;
|
|
||||||
private final Translation2d rightStick;
|
|
||||||
private final LiDAR m_lidar;
|
|
||||||
private final double mindistance;
|
|
||||||
|
|
||||||
public DriveUntilLiDAR(
|
|
||||||
SwerveDrive swerveDrive,
|
|
||||||
Translation2d leftStick,
|
|
||||||
Translation2d rightStick,
|
|
||||||
LiDAR lidar,
|
|
||||||
double mindistance) {
|
|
||||||
addRequirements(swerveDrive);
|
|
||||||
|
|
||||||
this.swerveDrive = swerveDrive;
|
|
||||||
this.leftStick = leftStick;
|
|
||||||
this.rightStick = rightStick;
|
|
||||||
this.m_lidar = lidar;
|
|
||||||
this.mindistance = mindistance;
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void initialize() {
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
swerveDrive.driveFine(leftStick, rightStick, 0.3);
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
if (Math.abs(m_lidar.getDistance()) < mindistance) {
|
|
||||||
swerveDrive.softStop();
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,107 +0,0 @@
|
|||||||
// 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 frc4388.robot.commands.alignment;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Rotation2d;
|
|
||||||
import edu.wpi.first.math.geometry.Translation2d;
|
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc4388.robot.subsystems.lidar.LiDAR;
|
|
||||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
|
||||||
|
|
||||||
/* 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 LidarAlign extends Command {
|
|
||||||
private SwerveDrive swerveDrive;
|
|
||||||
private LiDAR lidar;
|
|
||||||
|
|
||||||
private int currentFinderTick;
|
|
||||||
// private int tickFoundPipe;
|
|
||||||
private boolean foundReef;
|
|
||||||
private boolean headedRight;
|
|
||||||
private double speed;
|
|
||||||
private int bounces;
|
|
||||||
private double additionalDistance = 0;
|
|
||||||
// private final boolean constructedHeadedRight;
|
|
||||||
|
|
||||||
/** Creates a new LidarAlign. */
|
|
||||||
public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) {
|
|
||||||
// Use addRequirements() here to declare subsystem dependencies.
|
|
||||||
|
|
||||||
this.swerveDrive = swerveDrive;
|
|
||||||
this.lidar = lidar;
|
|
||||||
|
|
||||||
addRequirements(swerveDrive, lidar);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called when the command is initially scheduled.
|
|
||||||
@Override
|
|
||||||
public void initialize() {
|
|
||||||
this.currentFinderTick = 0;
|
|
||||||
this.speed = 0.4; // TODO: find good speed for this
|
|
||||||
this.foundReef = false;
|
|
||||||
//this.headedRight = (DriveToReef.tagRelativeXError < 0);
|
|
||||||
this.additionalDistance = 0;
|
|
||||||
this.bounces = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// Called every time the scheduler runs while the command is scheduled.
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
if (lidar.withinDistance()) {
|
|
||||||
swerveDrive.softStop();
|
|
||||||
foundReef = true;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (currentFinderTick > (15 + additionalDistance)) { //arbutrary threshhold for now.
|
|
||||||
headedRight = !headedRight;
|
|
||||||
currentFinderTick *= -1;
|
|
||||||
bounces++;
|
|
||||||
additionalDistance += 5;
|
|
||||||
if (bounces == 5) return;
|
|
||||||
}
|
|
||||||
double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
|
|
||||||
double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
|
|
||||||
SmartDashboard.putNumber("Rel Angle", relAngle);
|
|
||||||
SmartDashboard.putNumber("heading", currentHeading);
|
|
||||||
if (!headedRight) {
|
|
||||||
swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle));
|
|
||||||
} else {
|
|
||||||
swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle));
|
|
||||||
}
|
|
||||||
|
|
||||||
currentFinderTick++;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 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() {
|
|
||||||
if (lidar.getDistance() < 4) {
|
|
||||||
swerveDrive.stopModules();
|
|
||||||
return true;
|
|
||||||
} else if (foundReef && lidar.withinDistance()) { // spot on
|
|
||||||
swerveDrive.stopModules();
|
|
||||||
return true;
|
|
||||||
} else if (foundReef && !lidar.withinDistance()) { // over shot
|
|
||||||
speed = speed / 2;
|
|
||||||
headedRight = !headedRight;
|
|
||||||
currentFinderTick = 0;
|
|
||||||
foundReef = false;
|
|
||||||
return false;
|
|
||||||
} else if (bounces >= 3) {
|
|
||||||
swerveDrive.stopModules();
|
|
||||||
return true;
|
|
||||||
} else {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -11,8 +11,8 @@ public final class BuildConstants {
|
|||||||
public static final String GIT_SHA = "8dbb9d5a9099f417617ec2245e275a42b6788116";
|
public static final String GIT_SHA = "8dbb9d5a9099f417617ec2245e275a42b6788116";
|
||||||
public static final String GIT_DATE = "2026-01-10 16:52:43 MST";
|
public static final String GIT_DATE = "2026-01-10 16:52:43 MST";
|
||||||
public static final String GIT_BRANCH = "master";
|
public static final String GIT_BRANCH = "master";
|
||||||
public static final String BUILD_DATE = "2026-01-13 17:47:04 MST";
|
public static final String BUILD_DATE = "2026-01-12 16:53:00 MST";
|
||||||
public static final long BUILD_UNIX_TIME = 1768351624667L;
|
public static final long BUILD_UNIX_TIME = 1768261980081L;
|
||||||
public static final int DIRTY = 1;
|
public static final int DIRTY = 1;
|
||||||
|
|
||||||
private BuildConstants(){}
|
private BuildConstants(){}
|
||||||
|
|||||||
@@ -36,16 +36,16 @@ import frc4388.utility.structs.LEDPatterns;
|
|||||||
public final class Constants {
|
public final class Constants {
|
||||||
public static final String CANBUS_NAME = "rio";
|
public static final String CANBUS_NAME = "rio";
|
||||||
|
|
||||||
public static final class LiDARConstants {
|
// public static final class LiDARConstants {
|
||||||
public static final int REEF_LIDAR_DIO_CHANNEL = 7;
|
// public static final int REEF_LIDAR_DIO_CHANNEL = 7;
|
||||||
public static final int REVERSE_LIDAR_DIO_CHANNEL = 4;
|
// public static final int REVERSE_LIDAR_DIO_CHANNEL = 4;
|
||||||
|
|
||||||
public static final int HUMAN_PLAYER_STATION_DISTANCE = 40;
|
// public static final int HUMAN_PLAYER_STATION_DISTANCE = 40;
|
||||||
|
|
||||||
public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
|
// public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
|
||||||
public static final int LIDAR_MICROS_TO_CM = 10;
|
// public static final int LIDAR_MICROS_TO_CM = 10;
|
||||||
public static final int SECONDS_TO_MICROS = 1000000;
|
// public static final int SECONDS_TO_MICROS = 1000000;
|
||||||
}
|
// }
|
||||||
|
|
||||||
public static final class AutoConstants {
|
public static final class AutoConstants {
|
||||||
// public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
|
// public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
|
||||||
|
|||||||
@@ -1,58 +0,0 @@
|
|||||||
package frc4388.robot.subsystems.lidar;
|
|
||||||
|
|
||||||
import org.littletonrobotics.junction.Logger;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
||||||
import frc4388.robot.constants.Constants.LiDARConstants;
|
|
||||||
import frc4388.utility.status.Status;
|
|
||||||
import frc4388.utility.status.FaultReporter;
|
|
||||||
import frc4388.utility.status.Queryable;
|
|
||||||
import frc4388.utility.status.Status.ReportLevel;
|
|
||||||
|
|
||||||
public class LiDAR extends SubsystemBase implements Queryable {
|
|
||||||
LidarIO io;
|
|
||||||
LidarStateAutoLogged state = new LidarStateAutoLogged();
|
|
||||||
|
|
||||||
private String name = "Lidar";
|
|
||||||
|
|
||||||
public LiDAR(LidarIO device, String name) {
|
|
||||||
FaultReporter.register(this);
|
|
||||||
|
|
||||||
this.io = device;
|
|
||||||
this.name = name;
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void periodic() {
|
|
||||||
io.updateInputs(state);
|
|
||||||
Logger.processInputs("LiDAR/"+name, state);
|
|
||||||
}
|
|
||||||
|
|
||||||
// @AutoLogOutput(key = "Lidar/{name}")
|
|
||||||
public double getDistance(){
|
|
||||||
return state.distance;
|
|
||||||
}
|
|
||||||
|
|
||||||
public boolean withinDistance(){
|
|
||||||
if(state.distance == -1) return false;
|
|
||||||
return state.distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public String getName() {
|
|
||||||
return "Lidar " + name;
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public Status diagnosticStatus() {
|
|
||||||
Status s = new Status();
|
|
||||||
|
|
||||||
if(state.distance == -1)
|
|
||||||
s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED");
|
|
||||||
|
|
||||||
s.addReport(ReportLevel.INFO, "LiDAR Distance: " + state.distance);
|
|
||||||
|
|
||||||
return s;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
@@ -1,13 +0,0 @@
|
|||||||
package frc4388.robot.subsystems.lidar;
|
|
||||||
|
|
||||||
import org.littletonrobotics.junction.AutoLog;
|
|
||||||
|
|
||||||
public interface LidarIO {
|
|
||||||
@AutoLog
|
|
||||||
public class LidarState {
|
|
||||||
public boolean connected;
|
|
||||||
public double distance;
|
|
||||||
}
|
|
||||||
|
|
||||||
public default void updateInputs(LidarState state) {}
|
|
||||||
}
|
|
||||||
@@ -1,27 +0,0 @@
|
|||||||
package frc4388.robot.subsystems.lidar;
|
|
||||||
|
|
||||||
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 LidarReal implements LidarIO {
|
|
||||||
|
|
||||||
|
|
||||||
private Counter LidarPWM;
|
|
||||||
|
|
||||||
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
|
|
||||||
LidarPWM.reset();
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void updateInputs(LidarState state) {
|
|
||||||
|
|
||||||
if(LidarPWM.get() < 1)
|
|
||||||
state.distance = -1;
|
|
||||||
else
|
|
||||||
state.distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -8,10 +8,11 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
|||||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||||
|
|
||||||
import edu.wpi.first.units.measure.Angle;
|
import edu.wpi.first.units.measure.Angle;
|
||||||
|
import frc4388.utility.status.CanDevice;
|
||||||
|
|
||||||
public class ShooterConstants {
|
public class ShooterConstants {
|
||||||
// Motor conversions
|
// Motor conversions
|
||||||
public static final double ANGLE_MOTOR_GEAR_RATIO = 1.;
|
// public static final double ANGLE_MOTOR_GEAR_RATIO = 1.;
|
||||||
public static final double PITCH_MOTOR_GEAR_RATIO = 1.;
|
public static final double PITCH_MOTOR_GEAR_RATIO = 1.;
|
||||||
public static final double FLYWHEEL_GEAR_RATIO = 1.;
|
public static final double FLYWHEEL_GEAR_RATIO = 1.;
|
||||||
public static final double FEEDER_INCHES_PER_ROT = 1.;
|
public static final double FEEDER_INCHES_PER_ROT = 1.;
|
||||||
@@ -20,24 +21,29 @@ public class ShooterConstants {
|
|||||||
|
|
||||||
// 0 is the forward angle on the robot.
|
// 0 is the forward angle on the robot.
|
||||||
// negative is left, positive is right
|
// negative is left, positive is right
|
||||||
public static final Angle ANGLE_LIMIT_LEFT = Degrees.of(-180);
|
// public static final Angle ANGLE_LIMIT_LEFT = Degrees.of(-180);
|
||||||
public static final Angle ANGLE_LIMIT_RIGHT = Degrees.of(180);
|
// public static final Angle ANGLE_LIMIT_RIGHT = Degrees.of(180);
|
||||||
|
|
||||||
// 0 is paralell to the ground, 90 is directly up
|
// 0 is paralell to the ground, 90 is directly up
|
||||||
public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
|
public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
|
||||||
public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
|
public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
|
||||||
|
|
||||||
// Motor configs
|
// Motor configs
|
||||||
public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration()
|
// public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||||
.withCurrentLimits(
|
// .withCurrentLimits(
|
||||||
new CurrentLimitsConfigs()
|
// new CurrentLimitsConfigs()
|
||||||
.withStatorCurrentLimit(40) // TODO: tune???
|
// .withStatorCurrentLimit(40) // TODO: tune???
|
||||||
.withStatorCurrentLimitEnable(true)
|
// .withStatorCurrentLimitEnable(true)
|
||||||
).withMotorOutput(
|
// ).withMotorOutput(
|
||||||
new MotorOutputConfigs()
|
// new MotorOutputConfigs()
|
||||||
.withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
|
// .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
|
||||||
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||||
);
|
// );
|
||||||
|
|
||||||
|
public static final class IDs {
|
||||||
|
public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22);
|
||||||
|
}
|
||||||
|
|
||||||
public static final TalonFXConfiguration PITCH_MOTOR_CONFIG = new TalonFXConfiguration()
|
public static final TalonFXConfiguration PITCH_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||||
.withCurrentLimits(
|
.withCurrentLimits(
|
||||||
new CurrentLimitsConfigs()
|
new CurrentLimitsConfigs()
|
||||||
|
|||||||
@@ -16,13 +16,13 @@ import edu.wpi.first.units.measure.LinearVelocity;
|
|||||||
public interface ShooterIO {
|
public interface ShooterIO {
|
||||||
@AutoLog
|
@AutoLog
|
||||||
public class ShooterState {
|
public class ShooterState {
|
||||||
Angle shooterAngle = Rotations.of(0);
|
// Angle shooterAngle = Rotations.of(0);
|
||||||
Angle shooterTargetAngle = Rotations.of(0);
|
// Angle shooterTargetAngle = Rotations.of(0);
|
||||||
Current angleMotorCurrent = Amps.of(0);
|
// Current angleMotorCurrent = Amps.of(0);
|
||||||
|
|
||||||
Angle shooterPitch = Rotations.of(0);
|
// Angle shooterPitch = Rotations.of(0);
|
||||||
Angle shooterTargetPitch = Rotations.of(0);
|
// Angle shooterTargetPitch = Rotations.of(0);
|
||||||
Current pitchMotorCurrent = Amps.of(0);
|
// Current pitchMotorCurrent = Amps.of(0);
|
||||||
|
|
||||||
AngularVelocity flywheelVelocity = RotationsPerSecond.of(0);
|
AngularVelocity flywheelVelocity = RotationsPerSecond.of(0);
|
||||||
AngularVelocity flywheelTargetVelocity = RotationsPerSecond.of(0);
|
AngularVelocity flywheelTargetVelocity = RotationsPerSecond.of(0);
|
||||||
@@ -33,8 +33,8 @@ public interface ShooterIO {
|
|||||||
Current feederMotorCurrent = Amps.of(0);
|
Current feederMotorCurrent = Amps.of(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
public default void setShooterAngle(ShooterState state, Angle angle) {}
|
// public default void setShooterAngle(ShooterState state, Angle angle) {}
|
||||||
public default void setShooterPitch(ShooterState state, Angle angle) {}
|
// public default void setShooterPitch(ShooterState state, Angle angle) {}
|
||||||
public default void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {}
|
public default void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {}
|
||||||
public default void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {}
|
public default void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {}
|
||||||
|
|
||||||
|
|||||||
@@ -12,25 +12,25 @@ import edu.wpi.first.units.measure.*;
|
|||||||
|
|
||||||
public class ShooterReal implements ShooterIO {
|
public class ShooterReal implements ShooterIO {
|
||||||
|
|
||||||
TalonFX m_angleMotor;
|
// TalonFX m_angleMotor;
|
||||||
TalonFX m_pitchMotor;
|
// TalonFX m_pitchMotor;
|
||||||
TalonFX m_flywheelMotor;
|
TalonFX m_flywheelMotor;
|
||||||
TalonFX m_feederMotor;
|
TalonFX m_feederMotor;
|
||||||
|
|
||||||
public ShooterReal(
|
public ShooterReal(
|
||||||
TalonFX angleMotor,
|
// TalonFX angleMotor,
|
||||||
TalonFX pitchMotor,
|
// TalonFX pitchMotor,
|
||||||
TalonFX flywheelMotor,
|
TalonFX flywheelMotor,
|
||||||
TalonFX feederMotor
|
TalonFX feederMotor
|
||||||
) {
|
) {
|
||||||
m_angleMotor = angleMotor;
|
// m_angleMotor = angleMotor;
|
||||||
m_pitchMotor = pitchMotor;
|
// m_pitchMotor = pitchMotor;
|
||||||
m_flywheelMotor = flywheelMotor;
|
m_flywheelMotor = flywheelMotor;
|
||||||
m_feederMotor = feederMotor;
|
m_feederMotor = feederMotor;
|
||||||
|
|
||||||
// Apply the configs
|
// Apply the configs
|
||||||
m_angleMotor.getConfigurator().apply(ShooterConstants.ANGLE_MOTOR_CONFIG);
|
// m_angleMotor.getConfigurator().apply(ShooterConstants.ANGLE_MOTOR_CONFIG);
|
||||||
m_pitchMotor.getConfigurator().apply(ShooterConstants.PITCH_MOTOR_CONFIG);
|
// m_pitchMotor.getConfigurator().apply(ShooterConstants.PITCH_MOTOR_CONFIG);
|
||||||
m_flywheelMotor.getConfigurator().apply(ShooterConstants.FLYWHEEL_MOTOR_CONFIG);
|
m_flywheelMotor.getConfigurator().apply(ShooterConstants.FLYWHEEL_MOTOR_CONFIG);
|
||||||
m_feederMotor.getConfigurator().apply(ShooterConstants.FEEDER_MOTOR_CONFIG);
|
m_feederMotor.getConfigurator().apply(ShooterConstants.FEEDER_MOTOR_CONFIG);
|
||||||
}
|
}
|
||||||
@@ -45,35 +45,35 @@ public class ShooterReal implements ShooterIO {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// TODO: Test
|
// // TODO: Test
|
||||||
@Override
|
// @Override
|
||||||
public void setShooterAngle(ShooterState state, Angle angle) {
|
// public void setShooterAngle(ShooterState state, Angle angle) {
|
||||||
state.shooterTargetAngle = angle;
|
// state.shooterTargetAngle = angle;
|
||||||
// Assume that the angle is always accurate, because I think we will use a shaft encoder
|
// // Assume that the angle is always accurate, because I think we will use a shaft encoder
|
||||||
// Assume that 0 degrees = forwards. Might need an offset here
|
// // Assume that 0 degrees = forwards. Might need an offset here
|
||||||
|
|
||||||
Angle boundedAngle = clampAng(angle, ShooterConstants.ANGLE_LIMIT_LEFT, ShooterConstants.ANGLE_LIMIT_RIGHT);
|
// Angle boundedAngle = clampAng(angle, ShooterConstants.ANGLE_LIMIT_LEFT, ShooterConstants.ANGLE_LIMIT_RIGHT);
|
||||||
// (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
|
// // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
|
||||||
double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.ANGLE_MOTOR_GEAR_RATIO;
|
// double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.ANGLE_MOTOR_GEAR_RATIO;
|
||||||
PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
|
// PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
|
||||||
m_angleMotor.setControl(posRequest);
|
// m_angleMotor.setControl(posRequest);
|
||||||
}
|
// }
|
||||||
|
|
||||||
|
|
||||||
// TODO: Test
|
// TODO: Test
|
||||||
@Override
|
// @Override
|
||||||
public void setShooterPitch(ShooterState state, Angle angle) {
|
// public void setShooterPitch(ShooterState state, Angle angle) {
|
||||||
state.shooterTargetPitch = angle;
|
// state.shooterTargetPitch = angle;
|
||||||
// TODO: Test
|
// // TODO: Test
|
||||||
// This assumes that the 0 is paralell to the ground. Might need an offset here
|
// // This assumes that the 0 is paralell to the ground. Might need an offset here
|
||||||
|
|
||||||
|
|
||||||
Angle boundedAngle = clampAng(angle, ShooterConstants.PITCH_LIMIT_UPPER, ShooterConstants.PITCH_LIMIT_LOWER);
|
// Angle boundedAngle = clampAng(angle, ShooterConstants.PITCH_LIMIT_UPPER, ShooterConstants.PITCH_LIMIT_LOWER);
|
||||||
// (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
|
// // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
|
||||||
double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.PITCH_MOTOR_GEAR_RATIO;
|
// double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.PITCH_MOTOR_GEAR_RATIO;
|
||||||
PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
|
// PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
|
||||||
m_angleMotor.setControl(posRequest);
|
// m_pitchMotor.setControl(posRequest);
|
||||||
}
|
// }
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {
|
public void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {
|
||||||
@@ -95,11 +95,11 @@ public class ShooterReal implements ShooterIO {
|
|||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void updateInputs(ShooterState state) {
|
public void updateInputs(ShooterState state) {
|
||||||
state.shooterAngle = m_angleMotor.getPosition().getValue().times(ShooterConstants.ANGLE_MOTOR_GEAR_RATIO);
|
// state.shooterAngle = m_angleMotor.getPosition().getValue().times(ShooterConstants.ANGLE_MOTOR_GEAR_RATIO);
|
||||||
state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue();
|
// state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue();
|
||||||
|
|
||||||
state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO);
|
// state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO);
|
||||||
state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue();
|
// state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue();
|
||||||
|
|
||||||
state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue();
|
state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue();
|
||||||
state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue();
|
state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue();
|
||||||
|
|||||||
@@ -0,0 +1,38 @@
|
|||||||
|
{
|
||||||
|
"fileName": "PathplannerLib-2026.1.2.json",
|
||||||
|
"name": "PathplannerLib",
|
||||||
|
"version": "2026.1.2",
|
||||||
|
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
|
||||||
|
"frcYear": "2026",
|
||||||
|
"mavenUrls": [
|
||||||
|
"https://3015rangerrobotics.github.io/pathplannerlib/repo"
|
||||||
|
],
|
||||||
|
"jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
|
||||||
|
"javaDependencies": [
|
||||||
|
{
|
||||||
|
"groupId": "com.pathplanner.lib",
|
||||||
|
"artifactId": "PathplannerLib-java",
|
||||||
|
"version": "2026.1.2"
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"jniDependencies": [],
|
||||||
|
"cppDependencies": [
|
||||||
|
{
|
||||||
|
"groupId": "com.pathplanner.lib",
|
||||||
|
"artifactId": "PathplannerLib-cpp",
|
||||||
|
"version": "2026.1.2",
|
||||||
|
"libName": "PathplannerLib",
|
||||||
|
"headerClassifier": "headers",
|
||||||
|
"sharedLibrary": false,
|
||||||
|
"skipInvalidPlatforms": true,
|
||||||
|
"binaryPlatforms": [
|
||||||
|
"windowsx86-64",
|
||||||
|
"linuxx86-64",
|
||||||
|
"osxuniversal",
|
||||||
|
"linuxathena",
|
||||||
|
"linuxarm32",
|
||||||
|
"linuxarm64"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user