Skip to content

Week 1 throwaway #15

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
jckitkat merged 3 commits into from
Mar 9, 2026
Merged

Week 1 throwaway #15

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
509ef2d
the robot now has fully functional teleop code; indexer not very mech…
jckitkat Mar 6, 2026
dfc0620
added auton and better shooter tuning
jckitkat Mar 6, 2026
0f38a4b
week 1 complete code
jckitkat Mar 9, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 13 additions & 11 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -158,13 +158,14 @@ public static class Flywheel{
public static final double simD = 0.0;
public static final double simS = 8;

public static double motionMagicCruise = 100;
public static double motionMagicAccel = 50;
public static double motionMagicCruise = 400;
public static double motionMagicAccel = 600;

public static final double compP = 13;
public static final double compP = 4.5;
public static final double compI = 0;
public static final double compD = 0;
public static final double compS = 0;
public static final double compS = 6;
public static final double compV = 0.1;

public static final double gearRatio = 0.75;
public static double currentLimit = 60;
Expand Down Expand Up @@ -202,10 +203,10 @@ public static class Indexer {
public static final double simS = 0.0;

// Competition bot
public static final double compP = 20;
public static final double compP = 15;
public static final double compI = 0.0;
public static final double compD = 0.0;
public static final double compS = 10;
public static final double compS = 4;

public static final int indexerCanID = 25;

Expand All @@ -226,19 +227,20 @@ public static class Intake{
public static final double RollerkP = 0.6;
public static final double RollerkI = 0.0;
public static final double RollerkD = 0.0;
public static final double RollerkS = 2.1;
public static final double RollerkS = .6;
public static final double RollerkG = 0.0;

public static final int pivotMotorCANID = 40;
public static final double PivotCurrentLimit = 40;
public static final double PivotCurrentLimit = 60;
public static final double PivotGearRatio = 11.555;
public static final double PivotMotionMagicCruise = 100;
public static final double PivotMotionMagicAccel = 200;
public static final double PivotkP = 10.0;
public static final double PivotkP = 3.8;
public static final double PivotkI = 0.0;
public static final double PivotkD = 0.0;
public static final double PivotkS = 0.0;
public static final double PivotkG = 0.0;
public static final double PivotkS = 0.9;
public static final double PivotkG = 0.5;
public static final double PivotkV = 0.6;

public static final double FrequencyUpdateRate = 20;
public static double rollerRadius = Units.inchesToMeters(1);
Expand Down
34 changes: 31 additions & 3 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -188,7 +188,7 @@ public Robot() {
));
indexer = new Indexer(new IndexerIOCompetition());
intake = new Intake(new IntakeIOCompetition());
shooterLimelight = new LimelightCamera("limelight-turret", new Pose3d(Units.inchesToMeters(-12.140), Units.inchesToMeters(-12.745), Units.inchesToMeters(19.296), new Rotation3d(0, 0, Units.degreesToRadians(145))), LimelightCamera.limelightPipeline.APRIL_TAG);
// shooterLimelight = new LimelightCamera("limelight-turret", new Pose3d(Units.inchesToMeters(-12.140), Units.inchesToMeters(-12.745), Units.inchesToMeters(19.296), new Rotation3d(0, 0, Units.degreesToRadians(145))), LimelightCamera.limelightPipeline.APRIL_TAG);
// swerveLimelight = new LimelightCamera("limelight-swerve", new Pose3d(Units.inchesToMeters(6.833), Units.inchesToMeters(11.255), Units.inchesToMeters(8.691), new Rotation3d(0, Units.degreesToRadians(15), Units.degreesToRadians(-35))), LimelightCamera.limelightPipeline.APRIL_TAG);
break;

Expand Down Expand Up @@ -269,7 +269,7 @@ public Robot() {

// initialize default state and drive commands
RobotControl.setDriveModeCommand(DriveModes.teleopDrive);
RobotControl.setCurrentMode(RobotTransitions.shooterStacksInit);
// RobotControl.setCurrentMode(RobotStates.onSideState);
// drivetrain.setPose(new Pose2d(0, 2, Rotation2d.fromDegrees(32)));
}

Expand Down Expand Up @@ -332,7 +332,35 @@ public void teleopInit() {

/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}
public void teleopPeriodic() {
if (Robot.driverController.getLeftTriggerAxis() > 0.5) {
// Intake mode
Robot.shooterArray.enableShooting(false);
Robot.intake.setRollerVelocity(60);
// Robot.indexer.setIndexVelocity(25);
Robot.intake.setPivotPosition(-0.5);
Robot.shooterArray.setFeederVelocity(-10);
} else if (Robot.driverController.getRightTriggerAxis() > 0.5) {
// Shoot mode
Robot.shooterArray.enableShooting(true);
Robot.shooterArray.setHoodAngle(1100);
Robot.intake.setRollerVelocity(45);
Robot.indexer.setIndexVelocity(25);
Robot.intake.setPivotPosition(0.25);
} else if (Robot.driverController.getLeftBumperButton()) {
Robot.intake.setRollerVelocity(-30);
Robot.indexer.setIndexVelocity(-25);
Robot.shooterArray.setFeederVelocity(-20);
} else {
// Neutral mode
Robot.shooterArray.enableShooting(false);
Robot.intake.setRollerSpeed(0);
Robot.indexer.setIndexSpeed(0);
Robot.shooterArray.setHoodAngle(1000);
Robot.shooterArray.setFeederVelocity(0);
Robot.intake.setPivotSpeed(0);
}
}

/** This function is called once when test mode is enabled. */
@Override
Expand Down
3 changes: 3 additions & 0 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.commands.BasicAuton;
import frc.robot.commands.PanicCommand;
import frc.robot.commands.SetMode;
import frc.robot.commands.basicCommands.exampleCommands.ExampleCommand;
Expand Down Expand Up @@ -52,6 +53,8 @@ public RobotContainer() {
// Set up auto routines
autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());

autoChooser.addDefaultOption("Basic Auto", new BasicAuton());

// Configure the button bindings
configureButtonBindings();
}
Expand Down
24 changes: 24 additions & 0 deletions src/main/java/frc/robot/commands/BasicAuton.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
package frc.robot.commands;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.Robot;

public class BasicAuton extends SequentialCommandGroup {

public BasicAuton() {
addCommands(
new InstantCommand(() -> Robot.drivetrain.setPose(new Pose2d(new Translation2d(), new Rotation2d(Units.degreesToRadians(180))))),
new InstantCommand(() -> {
Robot.intake.setPivotPosition(0);
Robot.intake.setRollerSpeed(0.2);
Robot.indexer.setIndexSpeed(0.20);
Robot.shooterArray.enableShooting(true);
})
);
}
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/driveStates/TeleopDrive.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@ public class TeleopDrive extends Command {

public TeleopDrive() {
DrivetrainPublisher.setSuppliers(
() -> -Robot.driverController.getLeftX(),
Robot.driverController::getLeftY,
Robot.driverController::getLeftX,
Robot.driverController::getRightX,
() -> true
);
Expand Down
90 changes: 90 additions & 0 deletions src/main/java/frc/robot/commands/states/OnSideState.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,90 @@
package frc.robot.commands.states;

import ControlAnnotations.State;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.Constants;
import frc.robot.Robot;
import frc.robot.TriggerBoard;
import frc.robot.commands.basicCommands.intake.IntakeRollerDriveVelocityMatch;
import frc.robot.commands.transitions.OnSideEnterTransition;
import frc.robot.subsystems.robotControl.RobotControl;
import frc.robot.util.HubState;
import frc.robot.utils.RobotTransitions;

import static edu.wpi.first.units.Units.Seconds;

@State
public class OnSideState extends Command {

public OnSideState() {

}

@Override
public void initialize() {
// Robot.indexer.setIndexVelocity(15);
// Robot.intake.setPivotPosition(0);
// Robot.intake.setRollerVelocitySupplier(() -> MathUtil.clamp(Math.sqrt(((Robot.drivetrain.getChassisSpeeds().vxMetersPerSecond * Robot.drivetrain.getChassisSpeeds().vxMetersPerSecond) + (Robot.drivetrain.getChassisSpeeds().vyMetersPerSecond * Robot.drivetrain.getChassisSpeeds().vyMetersPerSecond)) / Constants.Intake.rollerRadius) / (2 * Math.PI), 40.0, 100));
}

@Override
public void execute() {
// if (TriggerBoard.isRobotInNoShootingZone()) {
// inactive();
// } else {
// if (HubState.isActive()) {
// active();
// } else {
// inactive();
// }
// }
//
// if (HubState.timeRemainingInCurrentShift().get().in(Seconds) <= Constants.shiftOffset && !HubState.isActiveNext()) {
// RobotControl.setCurrentMode(RobotTransitions.hubInactiveTransition);
// return;
// } else if (HubState.timeRemainingInCurrentShift().get().in(Seconds) <= Constants.shiftOffset && HubState.isActiveNext()) {
// RobotControl.setCurrentMode(RobotTransitions.hubActiveTransition);
// return;
// }

// if (TriggerBoard.isInNeutralZone()) {
// RobotControl.setCurrentMode(RobotTransitions.neutralZoneEnterTransition);
// return;
// }
if (Robot.driverController.getLeftTriggerAxis() > 0.5) {
// Intake mode
Robot.shooterArray.enableShooting(false);
Robot.intake.setRollerVelocity(50);
Robot.indexer.setIndexVelocity(15);
Robot.intake.setPivotPosition(0);
} else if (Robot.driverController.getRightTriggerAxis() > 0.5) {
// Shoot mode
Robot.shooterArray.enableShooting(true);
Robot.intake.setRollerVelocity(50);
Robot.indexer.setIndexVelocity(15);
// Robot.intake.setPivotPosition(0.15);
} else {
// Neutral mode
Robot.shooterArray.enableShooting(false);
Robot.intake.setRollerVelocity(0);
Robot.indexer.setIndexVelocity(0);
}
// System.out.println("Ran Active function from OnSideState");
// active();
// System.out.println("Ran execute from OnSideState");
}

public void active() {
}

public void inactive() {
Robot.shooterArray.enableShooting(false);
}

@Override
public boolean isFinished() {
return false;
}
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/generated/TunerConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ public class TunerConstants {

// The remote sensor feedback type to use for the steer motors;
// When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.RemoteCANcoder;

// The stator current at which the wheels start to slip;
// This needs to be tuned to your individual robot
Expand Down
20 changes: 15 additions & 5 deletions src/main/java/frc/robot/subsystems/drive/Drivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -288,12 +288,22 @@ public void joystickDrive(
// Square rotation value for more precise control
omega = Math.copySign(omega * omega, omega);

ChassisSpeeds speeds;

// Convert to field relative speeds & send command
ChassisSpeeds speeds =
new ChassisSpeeds(
linearVelocity.getX() * getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * getMaxLinearSpeedMetersPerSec(),
omega * getMaxAngularSpeedRadPerSec());
if (Robot.getAlliance() == Alliance.Red) {
speeds =
new ChassisSpeeds(
linearVelocity.getX() * getMaxLinearSpeedMetersPerSec() * 0.5,
linearVelocity.getY() * getMaxLinearSpeedMetersPerSec() * 0.5,
omega * getMaxAngularSpeedRadPerSec() * 0.5);
} else {
speeds =
new ChassisSpeeds(
-linearVelocity.getX() * getMaxLinearSpeedMetersPerSec() * 0.5,
-linearVelocity.getY() * getMaxLinearSpeedMetersPerSec() * 0.5,
omega * getMaxAngularSpeedRadPerSec() * 0.5);
}
boolean isFlipped =
DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == Alliance.Red;
Expand Down
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -183,7 +183,7 @@ public void updateInputs(ModuleIOInputs inputs) {
public void setDriveOpenLoop(double output) {
driveTalon.setControl(
switch (constants.DriveMotorClosedLoopOutput) {
case Voltage -> voltageRequest.withOutput(output);
case Voltage -> voltageRequest.withOutput(output).withEnableFOC(false);
case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output);
});
}
Expand All @@ -192,7 +192,7 @@ public void setDriveOpenLoop(double output) {
public void setTurnOpenLoop(double output) {
turnTalon.setControl(
switch (constants.SteerMotorClosedLoopOutput) {
case Voltage -> voltageRequest.withOutput(output);
case Voltage -> voltageRequest.withOutput(output).withEnableFOC(false);
case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output);
});
}
Expand All @@ -203,7 +203,7 @@ public void setDriveVelocity(double wheelVelocityRadPerSec) {
Units.radiansToRotations(wheelVelocityRadPerSec) * constants.DriveMotorGearRatio;
driveTalon.setControl(
switch (constants.DriveMotorClosedLoopOutput) {
case Voltage -> velocityVoltageRequest.withVelocity(motorVelocityRotPerSec);
case Voltage -> velocityVoltageRequest.withVelocity(motorVelocityRotPerSec).withEnableFOC(false);
case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(motorVelocityRotPerSec);
});
}
Expand All @@ -212,7 +212,7 @@ public void setDriveVelocity(double wheelVelocityRadPerSec) {
public void setTurnPosition(Rotation2d rotation) {
turnTalon.setControl(
switch (constants.SteerMotorClosedLoopOutput) {
case Voltage -> positionVoltageRequest.withPosition(rotation.getRotations());
case Voltage -> positionVoltageRequest.withPosition(rotation.getRotations()).withEnableFOC(false);
case TorqueCurrentFOC -> positionTorqueCurrentRequest.withPosition(rotation.getRotations());
});
}
Expand Down
9 changes: 8 additions & 1 deletion src/main/java/frc/robot/subsystems/intake/IntakeIOCompetition.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
import com.ctre.phoenix6.configs.MotionMagicConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.MotionMagicExpoVoltage;
import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
Expand Down Expand Up @@ -92,6 +93,7 @@ public IntakeIOCompetition() {
pivotMotorSlot0Config.kD = Constants.Intake.PivotkD;
pivotMotorSlot0Config.kS = Constants.Intake.PivotkS;
pivotMotorSlot0Config.kG = Constants.Intake.PivotkG;
pivotMotorSlot0Config.kV = Constants.Intake.PivotkV;

pivotMotor.getConfigurator().apply(pivotMotorConfiguration);
pivotMotor.getConfigurator().apply(pivotMotorMotionMagicConfig);
Expand Down Expand Up @@ -139,6 +141,11 @@ public void updateInputs(IntakeIOInputs inputs) {
}
case POSITION -> {
pivotMotor.setControl(new MotionMagicVoltage(pivotTargetPosition));
// if (pivotMotorPosition.getValueAsDouble() < 0.18) {
// pivotMotor.setControl(new MotionMagicExpoVoltage(pivotTargetPosition).withFeedForward(50));
// } else {
// pivotMotor.setControl(new MotionMagicVoltage(pivotTargetPosition));
// }
}
case VELOCITY -> {
break;
Expand Down Expand Up @@ -170,7 +177,7 @@ public void setPivotPosition(double position) {

@Override
public void setRollerSpeed(double velocity) {
this.rollerControlMode = ControlMode.VELOCITY;
this.rollerControlMode = ControlMode.POWER;
rollerTargetPower = velocity;
}

Expand Down
5 changes: 5 additions & 0 deletions src/main/java/frc/robot/subsystems/shooter/ShooterArray.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,4 +104,9 @@ public void setFeederVelocity(double velocity) {
});
}

public void setHoodAngle(int angle) {
shooterStacks.forEach((shooterStackName, shooterStack) -> {
shooterStack.setHoodAngle(angle);
});
}
}
Loading