Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 08:48: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) { switch (mode) {
case ExtendingIdle: case ExtendingIdle:
io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); // io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
io.setRollerOutput(state, 0); io.setRollerOutput(state, 0);
break; break;
case ExtendingRolling: 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)); io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); //getTargetRollerSpeed(ChassisOverallSpeed));
break; break;
case EncoderFix: case EncoderFix:
io.armFix(IntakeConstants.FIX_ARM_PERCENT_OUTPUT.get()); // io.armFix(IntakeConstants.FIX_ARM_PERCENT_OUTPUT.get());
io.setRollerOutput(state, 0); io.setRollerOutput(state, 0);
break; break;
case Retracting: 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()) { // if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) {
io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
@@ -145,41 +145,41 @@ public class Intake extends SubsystemBase {
// } // }
break; break;
case ArmIdleRollingNot: case ArmIdleRollingNot:
io.armOutput(0); // io.armOutput(0);
io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
break; break;
case Bouncing: case Bouncing:
// io.setRollerOutput(state, 0); // io.setRollerOutput(state, 0);
if( // if(
state.armMotorCurrent.in(Amps) > IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get() // state.armMotorCurrent.in(Amps) > IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get()
// Math.abs(state.armMotorVelocity.in(RotationsPerSecond)) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_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(); // this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get();
} // }
// Get the time delta from the last bounce time update // // Get the time delta from the last bounce time update
double currentTime = Utils.getSystemTimeSeconds() - state.currentBounceTime; // double currentTime = Utils.getSystemTimeSeconds() - state.currentBounceTime;
// Get the percentage through the bounce period (0 output means one half period has passed) // // 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(); // double percentOutput = (currentTime / IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get()) * IntakeConstants.INTAKE_BOUNCE_OUTPUT.get();
// Clamp the output of the motor to some value // // 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()); // 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) { // if(percentOutput < 0) {
io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); // io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
} else { // } else {
io.setRollerOutput(state, 0); // io.setRollerOutput(state, 0);
} // }
break; // break;
case RectractTorque: // case RectractTorque:
io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); // io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
if (!overCompressed){ // if (!overCompressed){
io.armOutput(IntakeConstants.ARM_SQUEEZE_PERCENT_OUTPUT.get()); // io.armOutput(IntakeConstants.ARM_SQUEEZE_PERCENT_OUTPUT.get());
} else { // } else {
io.armOutput(IntakeConstants.ARM_REDUCED_SQUEEZE_PERCENT_OUTPUT.get()); // io.armOutput(IntakeConstants.ARM_REDUCED_SQUEEZE_PERCENT_OUTPUT.get());
} // }
// if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) { // if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) {
// io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); // io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
@@ -189,19 +189,19 @@ public class Intake extends SubsystemBase {
break; break;
case RectractAuto: case RectractAuto:
io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
io.armOutput(IntakeConstants.ARM_AUTO_OUTPUT.get()); // io.armOutput(IntakeConstants.ARM_AUTO_OUTPUT.get());
break; break;
case Idle: case Idle:
io.armOutput(0); // io.armOutput(0);
io.setRollerOutput(state, 0); io.setRollerOutput(state, 0);
break; break;
case ExpelBalls: case ExpelBalls:
io.armOutput(0); // io.armOutput(0);
io.setRollerOutput(state, IntakeConstants.ROLLER_EJECT_PERCENT_OUTPUT.get()); io.setRollerOutput(state, IntakeConstants.ROLLER_EJECT_PERCENT_OUTPUT.get());
break; break;
case LabubuGrowl: 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()); io.setRollerOutput(state, IntakeConstants.ROLLER_LABUBU_GROWL_PERCENT_OUTPUT.get());
break; 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 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_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.);
// public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10); // 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) { public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) {
double speed = 0; double speed = DEMO_PERCENT_OUTPUT.get();
if (Math.abs(chassisXSpeed) < 0.1){ // if (Math.abs(chassisXSpeed) < 0.1){
speed = 0.0593402*hubDistMeters*hubDistMeters + // speed = 0.0593402*hubDistMeters*hubDistMeters +
4.90561*hubDistMeters + // 4.90561*hubDistMeters +
30.35696 + MODEL_TRIM.get(); // 30.35696 + MODEL_TRIM.get();
} else if (chassisXSpeed > 0){ // } else if (chassisXSpeed > 0){
speed = 0.0593402*hubDistMeters*hubDistMeters + // speed = 0.0593402*hubDistMeters*hubDistMeters +
4.90561*hubDistMeters + // 4.90561*hubDistMeters +
30.35696 + chassisXSpeed * POS_OFFSET.get() + MODEL_TRIM.get(); // 30.35696 + chassisXSpeed * POS_OFFSET.get() + MODEL_TRIM.get();
} else { // Negative is closer to hub // } else { // Negative is closer to hub
speed = 0.0593402*hubDistMeters*hubDistMeters + // speed = 0.0593402*hubDistMeters*hubDistMeters +
4.90561*hubDistMeters + // 4.90561*hubDistMeters +
30.35696 + chassisXSpeed * NEG_OFFSET.get() + MODEL_TRIM.get(); // 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 // // Clamp speed to max
if(speed > max) { // if(speed > max) {
speed = max; // speed = max;
} else if(speed < -max) { // } else if(speed < -max) {
speed = -max; // speed = -max;
} // }
// double speed = SHOOTER_MAX_VELOCITY.get(); // double speed = SHOOTER_MAX_VELOCITY.get();