Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-08 16:28:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

No armmovement and constant shooter speed

Browse Source
This commit is contained in:
Mira
2026-04-17 17:27:23 -06:00
parent 1bb098eb04
commit 6316daedf7
2 changed files with 57 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/intake/Intake.java
+35 -35
View File
@@ -120,23 +120,23 @@ public class Intake extends SubsystemBase {
switch (mode) {
case ExtendingIdle:
io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
// io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
io.setRollerOutput(state, 0);
break;
case ExtendingRolling:
io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
// io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); //getTargetRollerSpeed(ChassisOverallSpeed));
break;
case EncoderFix:
io.armFix(IntakeConstants.FIX_ARM_PERCENT_OUTPUT.get());
// io.armFix(IntakeConstants.FIX_ARM_PERCENT_OUTPUT.get());
io.setRollerOutput(state, 0);
break;
case Retracting:
io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get());
// io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get());
// if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) {
io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
@@ -145,41 +145,41 @@ public class Intake extends SubsystemBase {
// }
break;
case ArmIdleRollingNot:
io.armOutput(0);
// io.armOutput(0);
io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
break;
case Bouncing:
// io.setRollerOutput(state, 0);
if(
state.armMotorCurrent.in(Amps) > IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get()
// Math.abs(state.armMotorVelocity.in(RotationsPerSecond)) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get()
) {
this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get();
}
// if(
// state.armMotorCurrent.in(Amps) > IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get()
// // Math.abs(state.armMotorVelocity.in(RotationsPerSecond)) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get()
// ) {
// this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get();
// }
// Get the time delta from the last bounce time update
double currentTime = Utils.getSystemTimeSeconds() - state.currentBounceTime;
// Get the percentage through the bounce period (0 output means one half period has passed)
double percentOutput = (currentTime / IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get()) * IntakeConstants.INTAKE_BOUNCE_OUTPUT.get();
// Clamp the output of the motor to some value
percentOutput = -Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get());
// // Get the time delta from the last bounce time update
// double currentTime = Utils.getSystemTimeSeconds() - state.currentBounceTime;
// // Get the percentage through the bounce period (0 output means one half period has passed)
// double percentOutput = (currentTime / IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get()) * IntakeConstants.INTAKE_BOUNCE_OUTPUT.get();
// // Clamp the output of the motor to some value
// percentOutput = -Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get());
io.armOutput(percentOutput);
// io.armOutput(percentOutput);
if(percentOutput < 0) {
io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
} else {
io.setRollerOutput(state, 0);
}
break;
case RectractTorque:
io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
if (!overCompressed){
io.armOutput(IntakeConstants.ARM_SQUEEZE_PERCENT_OUTPUT.get());
} else {
io.armOutput(IntakeConstants.ARM_REDUCED_SQUEEZE_PERCENT_OUTPUT.get());
}
// if(percentOutput < 0) {
// io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
// } else {
// io.setRollerOutput(state, 0);
// }
// break;
// case RectractTorque:
// io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
// if (!overCompressed){
// io.armOutput(IntakeConstants.ARM_SQUEEZE_PERCENT_OUTPUT.get());
// } else {
// io.armOutput(IntakeConstants.ARM_REDUCED_SQUEEZE_PERCENT_OUTPUT.get());
// }
// if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) {
// io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
@@ -189,19 +189,19 @@ public class Intake extends SubsystemBase {
break;
case RectractAuto:
io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
io.armOutput(IntakeConstants.ARM_AUTO_OUTPUT.get());
// io.armOutput(IntakeConstants.ARM_AUTO_OUTPUT.get());
break;
case Idle:
io.armOutput(0);
// io.armOutput(0);
io.setRollerOutput(state, 0);
break;
case ExpelBalls:
io.armOutput(0);
// io.armOutput(0);
io.setRollerOutput(state, IntakeConstants.ROLLER_EJECT_PERCENT_OUTPUT.get());
break;
case LabubuGrowl:
io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
// io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
io.setRollerOutput(state, IntakeConstants.ROLLER_LABUBU_GROWL_PERCENT_OUTPUT.get());
break;
}
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+22 -21
View File
@@ -27,6 +27,7 @@ public class ShooterConstants {
public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", -0.15);
public static final ConfigurableDouble DEMO_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", -0.15);
// public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.);
// public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10);
@@ -55,31 +56,31 @@ public class ShooterConstants {
//
public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) {
double speed = 0;
double speed = DEMO_PERCENT_OUTPUT.get();
if (Math.abs(chassisXSpeed) < 0.1){
speed = 0.0593402*hubDistMeters*hubDistMeters +
4.90561*hubDistMeters +
30.35696 + MODEL_TRIM.get();
} else if (chassisXSpeed > 0){
speed = 0.0593402*hubDistMeters*hubDistMeters +
4.90561*hubDistMeters +
30.35696 + chassisXSpeed * POS_OFFSET.get() + MODEL_TRIM.get();
// if (Math.abs(chassisXSpeed) < 0.1){
// speed = 0.0593402*hubDistMeters*hubDistMeters +
// 4.90561*hubDistMeters +
// 30.35696 + MODEL_TRIM.get();
// } else if (chassisXSpeed > 0){
// speed = 0.0593402*hubDistMeters*hubDistMeters +
// 4.90561*hubDistMeters +
// 30.35696 + chassisXSpeed * POS_OFFSET.get() + MODEL_TRIM.get();
} else { // Negative is closer to hub
speed = 0.0593402*hubDistMeters*hubDistMeters +
4.90561*hubDistMeters +
30.35696 + chassisXSpeed * NEG_OFFSET.get() + MODEL_TRIM.get();
}
// } else { // Negative is closer to hub
// speed = 0.0593402*hubDistMeters*hubDistMeters +
// 4.90561*hubDistMeters +
// 30.35696 + chassisXSpeed * NEG_OFFSET.get() + MODEL_TRIM.get();
// }
double max = SHOOTER_MAX_VELOCITY.get();
// double max = SHOOTER_MAX_VELOCITY.get();
// Clamp speed to max
if(speed > max) {
speed = max;
} else if(speed < -max) {
speed = -max;
}
// // Clamp speed to max
// if(speed > max) {
// speed = max;
// } else if(speed < -max) {
// speed = -max;
// }
// double speed = SHOOTER_MAX_VELOCITY.get();