Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Remove angle and pitch motors. Clean some stuff up.

Browse Source
This commit is contained in:
Michael Mikovsky
2026-01-13 10:08:44 -07:00
parent 8dbb9d5a90
commit b990b762e8
12 changed files with 75 additions and 328 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
-3
View File
@@ -38,8 +38,6 @@ import frc4388.utility.controller.VirtualController;
import frc4388.robot.commands.MoveForTimeCommand;
import frc4388.robot.commands.MoveUntilSuply;
// import frc4388.robot.commands.alignment.DriveToReef;
import frc4388.robot.commands.alignment.DriveUntilLiDAR;
import frc4388.robot.commands.alignment.LidarAlign;
// import frc4388.robot.commands.wait.waitElevatorRefrence;
// import frc4388.robot.commands.wait.waitEndefectorRefrence;
// import frc4388.robot.commands.wait.waitFeedCoral;
@@ -57,7 +55,6 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
import frc4388.robot.subsystems.LED;
// 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;
// Utilites
src/main/java/frc4388/robot/RobotMap.java
-3
View File
@@ -23,9 +23,6 @@ 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.LidarReal;
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
import frc4388.robot.subsystems.swerve.SwerveIO;
import frc4388.robot.subsystems.swerve.SwerveReal;
src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java
-48
View File
@@ -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;
}
}
src/main/java/frc4388/robot/commands/alignment/LidarAlign.java
-107
View File
@@ -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;
}
}
}
src/main/java/frc4388/robot/constants/BuildConstants.java
+5 -5
View File
@@ -7,12 +7,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters-new";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 2;
public static final String GIT_SHA = "17c3ff1ec9ef6763ee1c736622be3ef0fcc30d10";
public static final String GIT_DATE = "2026-01-08 19:42:41 MST";
public static final int GIT_REVISION = 3;
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_BRANCH = "master";
public static final String BUILD_DATE = "2026-01-10 16:21:06 MST";
public static final long BUILD_UNIX_TIME = 1768087266722L;
public static final String BUILD_DATE = "2026-01-12 16:53:00 MST";
public static final long BUILD_UNIX_TIME = 1768261980081L;
public static final int DIRTY = 1;
private BuildConstants(){}
src/main/java/frc4388/robot/constants/Constants.java
+8 -8
View File
@@ -36,16 +36,16 @@ import frc4388.utility.structs.LEDPatterns;
public final class Constants {
public static final String CANBUS_NAME = "rio";
public static final class LiDARConstants {
public static final int REEF_LIDAR_DIO_CHANNEL = 7;
public static final int REVERSE_LIDAR_DIO_CHANNEL = 4;
// public static final class LiDARConstants {
// public static final int REEF_LIDAR_DIO_CHANNEL = 7;
// 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_MICROS_TO_CM = 10;
public static final int SECONDS_TO_MICROS = 1000000;
}
// 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 SECONDS_TO_MICROS = 1000000;
// }
public static final class AutoConstants {
// public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java
-58
View File
@@ -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;
}
}
src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java
-13
View File
@@ -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) {}
}
src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java
-27
View File
@@ -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;
}
}
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+19 -13
View File
@@ -8,10 +8,11 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.units.measure.Angle;
import frc4388.utility.status.CanDevice;
public class ShooterConstants {
// 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 FLYWHEEL_GEAR_RATIO = 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.
// negative is left, positive is right
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_LEFT = Degrees.of(-180);
// public static final Angle ANGLE_LIMIT_RIGHT = Degrees.of(180);
// 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_LOWER = Degrees.of(0);
// Motor configs
public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune???
.withStatorCurrentLimitEnable(true)
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
);
// public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration()
// .withCurrentLimits(
// new CurrentLimitsConfigs()
// .withStatorCurrentLimit(40) // TODO: tune???
// .withStatorCurrentLimitEnable(true)
// ).withMotorOutput(
// new MotorOutputConfigs()
// .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
// .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()
.withCurrentLimits(
new CurrentLimitsConfigs()
src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java
+8 -8
View File
@@ -16,13 +16,13 @@ import edu.wpi.first.units.measure.LinearVelocity;
public interface ShooterIO {
@AutoLog
public class ShooterState {
Angle shooterAngle = Rotations.of(0);
Angle shooterTargetAngle = Rotations.of(0);
Current angleMotorCurrent = Amps.of(0);
// Angle shooterAngle = Rotations.of(0);
// Angle shooterTargetAngle = Rotations.of(0);
// Current angleMotorCurrent = Amps.of(0);
Angle shooterPitch = Rotations.of(0);
Angle shooterTargetPitch = Rotations.of(0);
Current pitchMotorCurrent = Amps.of(0);
// Angle shooterPitch = Rotations.of(0);
// Angle shooterTargetPitch = Rotations.of(0);
// Current pitchMotorCurrent = Amps.of(0);
AngularVelocity flywheelVelocity = RotationsPerSecond.of(0);
AngularVelocity flywheelTargetVelocity = RotationsPerSecond.of(0);
@@ -33,8 +33,8 @@ public interface ShooterIO {
Current feederMotorCurrent = Amps.of(0);
}
public default void setShooterAngle(ShooterState state, Angle angle) {}
public default void setShooterPitch(ShooterState state, Angle angle) {}
// public default void setShooterAngle(ShooterState state, Angle angle) {}
// public default void setShooterPitch(ShooterState state, Angle angle) {}
public default void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {}
public default void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {}
src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java
+35 -35
View File
@@ -12,25 +12,25 @@ import edu.wpi.first.units.measure.*;
public class ShooterReal implements ShooterIO {
TalonFX m_angleMotor;
TalonFX m_pitchMotor;
// TalonFX m_angleMotor;
// TalonFX m_pitchMotor;
TalonFX m_flywheelMotor;
TalonFX m_feederMotor;
public ShooterReal(
TalonFX angleMotor,
TalonFX pitchMotor,
// TalonFX angleMotor,
// TalonFX pitchMotor,
TalonFX flywheelMotor,
TalonFX feederMotor
) {
m_angleMotor = angleMotor;
m_pitchMotor = pitchMotor;
// m_angleMotor = angleMotor;
// m_pitchMotor = pitchMotor;
m_flywheelMotor = flywheelMotor;
m_feederMotor = feederMotor;
// Apply the configs
m_angleMotor.getConfigurator().apply(ShooterConstants.ANGLE_MOTOR_CONFIG);
m_pitchMotor.getConfigurator().apply(ShooterConstants.PITCH_MOTOR_CONFIG);
// m_angleMotor.getConfigurator().apply(ShooterConstants.ANGLE_MOTOR_CONFIG);
// m_pitchMotor.getConfigurator().apply(ShooterConstants.PITCH_MOTOR_CONFIG);
m_flywheelMotor.getConfigurator().apply(ShooterConstants.FLYWHEEL_MOTOR_CONFIG);
m_feederMotor.getConfigurator().apply(ShooterConstants.FEEDER_MOTOR_CONFIG);
}
@@ -45,35 +45,35 @@ public class ShooterReal implements ShooterIO {
}
}
// TODO: Test
@Override
public void setShooterAngle(ShooterState state, Angle angle) {
state.shooterTargetAngle = angle;
// 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
// // TODO: Test
// @Override
// public void setShooterAngle(ShooterState state, Angle angle) {
// state.shooterTargetAngle = angle;
// // 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
Angle boundedAngle = clampAng(angle, ShooterConstants.ANGLE_LIMIT_LEFT, ShooterConstants.ANGLE_LIMIT_RIGHT);
// (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.ANGLE_MOTOR_GEAR_RATIO;
PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
m_angleMotor.setControl(posRequest);
}
// Angle boundedAngle = clampAng(angle, ShooterConstants.ANGLE_LIMIT_LEFT, ShooterConstants.ANGLE_LIMIT_RIGHT);
// // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
// double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.ANGLE_MOTOR_GEAR_RATIO;
// PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
// m_angleMotor.setControl(posRequest);
// }
// TODO: Test
@Override
public void setShooterPitch(ShooterState state, Angle angle) {
state.shooterTargetPitch = angle;
// TODO: Test
// This assumes that the 0 is paralell to the ground. Might need an offset here
// @Override
// public void setShooterPitch(ShooterState state, Angle angle) {
// state.shooterTargetPitch = angle;
// // TODO: Test
// // 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);
// (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.PITCH_MOTOR_GEAR_RATIO;
PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
m_angleMotor.setControl(posRequest);
}
// Angle boundedAngle = clampAng(angle, ShooterConstants.PITCH_LIMIT_UPPER, ShooterConstants.PITCH_LIMIT_LOWER);
// // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
// double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.PITCH_MOTOR_GEAR_RATIO;
// PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
// m_pitchMotor.setControl(posRequest);
// }
@Override
public void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {
@@ -95,11 +95,11 @@ public class ShooterReal implements ShooterIO {
@Override
public void updateInputs(ShooterState state) {
state.shooterAngle = m_angleMotor.getPosition().getValue().times(ShooterConstants.ANGLE_MOTOR_GEAR_RATIO);
state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue();
// state.shooterAngle = m_angleMotor.getPosition().getValue().times(ShooterConstants.ANGLE_MOTOR_GEAR_RATIO);
// state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue();
state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO);
state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue();
// state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO);
// state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue();
state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue();
state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue();