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

left camera

Browse Source
This commit is contained in:
Michael Mikovsky
2026-02-20 20:59:06 -08:00
parent 32ede81ffa
commit 9c7159ba3b
9 changed files with 62 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/intake/Intake.java
+22 -21
View File
@@ -63,30 +63,31 @@ public class Intake extends SubsystemBase {
Logger.processInputs("Intake", state);
Logger.recordOutput("Intake/IntakeState", this.mode);
io.updateInputs(state);
switch (mode) {
case Extended:
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get()));
io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
break;
case Retracted:
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get()));
io.setRollerOutput(state, 0);
break;
case Extending:
io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
break;
case Retracting:
io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get());
io.setRollerOutput(state, 0);
break;
case Idle:
io.stopArm();
break;
}
// switch (mode) {
// case Extended:
// io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get()));
// io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
// break;
// case Retracted:
// io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get()));
// io.setRollerOutput(state, 0);
// break;
// case Extending:
// io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
// io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
// break;
// case Retracting:
// io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get());
// io.setRollerOutput(state, 0);
// break;
// case Idle:
// io.stopArm();
// break;
// }
// if (state.retractedLimit){
// this.mode = IntakeMode.Retracted;
// }
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+19 -5
View File
@@ -19,9 +19,10 @@ public class ShooterConstants {
public static final double INDEXER_GEAR_RATIO = 1.;
public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -40);
public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60);
public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0);
public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.1);
public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4);
public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0);
public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0);
@@ -39,17 +40,30 @@ public class ShooterConstants {
//
public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) {
double speed = SHOOTER_ACTIVE_VELOCITY.get();
// Model derived from points
double speed =
1.11576*hubDistMeters*hubDistMeters +
0.318464*hubDistMeters +
30.6293;
double max = SHOOTER_MAX_VELOCITY.get();
return RotationsPerSecond.of(speed);
// Clamp speed to max
if(speed > max) {
speed = max;
} else if(speed < -max) {
speed = -max;
}
return RotationsPerSecond.of(-speed);
}
// Motor Configuration
public static Slot0Configs SHOOTER_PID = new Slot0Configs()
.withKV(0.0)
.withKP(0.0)
.withKP(0.08)
.withKI(0.1)
.withKD(0.08);
.withKD(0.0);
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+1 -1
View File
@@ -351,7 +351,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos);
// Calculate the angle between the current position and the lead position
Rotation2d ang = fieldPosLead.minus(getPose2d().getTranslation()).getAngle();
Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
driveFieldAngle(leftStick, ang);
src/main/java/frc4388/robot/subsystems/vision/Vision.java
+3 -5
View File
@@ -23,13 +23,11 @@ public class Vision extends SubsystemBase implements Queryable{
public Pose2d lastVisionPose = new Pose2d();
public Pose2d lastPhysOdomPose = new Pose2d();
public LED m_robotLED;
public Vision(VisionIO... devices) {
FaultReporter.register(this);
io = devices;
state = new VisionStateAutoLogged[io.length];
m_robotLED = new LED();
for(int i = 0; i < io.length; i++) {
state[i] = new VisionStateAutoLogged();
}
@@ -43,9 +41,9 @@ public class Vision extends SubsystemBase implements Queryable{
}
Logger.recordOutput("Vision/isTagDectected", isTag());
if (isTag()){
m_robotLED.setMode(LEDPatterns.SOLID_GREEN_DARK);
}
// if (isTag()){
// m_robotLED.setMode(LEDPatterns.SOLID_GREEN_DARK);
// }
}
public List<PoseObservation> getPosesToAdd(){
src/main/java/frc4388/robot/subsystems/vision/VisionReal.java
+1 -1
View File
@@ -49,7 +49,7 @@ public class VisionReal implements VisionIO {
if(!result.hasTargets())
return;
state.isTagDetected = state.isTagDetected | result.hasTargets();
state.isTagDetected = result.hasTargets();
Optional<EstimatedRobotPose> estimatedRobotPose = getEstimatedGlobalPose(result, estimator);