Skip to content

TeleOp: Fix issues #2

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

Open
qwertychouskie wants to merge 5 commits into
base: main
Choose a base branch
from
Open

TeleOp: Fix issues #2

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
fc96d9d
TeleOp: Fix issues
qwertychouskie Oct 22, 2025
4b46e2b
TeleOp: Adjust power based on hardware tests
qwertychouskie Nov 2, 2025
96c2ca2
fixup! TeleOp: Fix issues
qwertychouskie Nov 2, 2025
63620ba
TeleOp: Automatically reset catapult down after launching
qwertychouskie Nov 3, 2025
ffd1357
Merge branch 'main' into patch-1
qwertychouskie Nov 14, 2025
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
59 changes: 43 additions & 16 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpControlLinearOpMode.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -96,12 +96,16 @@ public class TeleOpControlLinearOpMode extends LinearOpMode {
private double footPower = FOOT_OFF_POWER;

private double CATAPULT_UP_POWER = -1.0;
private double CATAPULT_DOWN_POWER = 1.0;
private double CATAPULT_HOLD_POWER = 0.2;
private double CATAPULT_DOWN_POWER = 1.0; // Need full power with 12 rubber bands. Half that amount can be adjusted to use 0.5 power.
private double CATAPULT_HOLD_POWER = 0.15; // Only use a small amount of power to hold it down once it is down, othewise the motor will get very hot from stalling and can damage itself

private enum CatapultModes {UP, DOWN, HOLD}

private CatapultModes pivotMode;

private static ElapsedTime pivotUpTime = new ElapsedTime();
private static ElapsedTime pivotDownTime = new ElapsedTime();

private enum FootMode {UP, DOWN, BRAKE}
private FootMode footmode;

Expand Down Expand Up @@ -138,19 +142,45 @@ public void runOpMode() {
// Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward
// Keep testing until ALL the wheels move the robot forward when you push the left joystick forward.

// reset encoders and set runmode of drive motors
leftFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftFrontDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
leftBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftBackDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
rightFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightFrontDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
rightBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightBackDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

// set direction of wheel motors
leftFrontDrive.setDirection(DcMotor.Direction.FORWARD);
leftBackDrive.setDirection(DcMotor.Direction.REVERSE);
rightFrontDrive.setDirection(DcMotor.Direction.REVERSE);
rightBackDrive.setDirection(DcMotor.Direction.FORWARD);

// set wheel motors to BRAKE mode for easier control
leftFrontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftBackDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFrontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightBackDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

// reset encoders and set runmode of drive motors
intake.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
catapult1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
catapult1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
catapult2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
catapult2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
foot.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
foot.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

// set direction of subsystem motors
intake.setDirection(DcMotor.Direction.FORWARD); // Forward should INTAKE.
catapult1.setDirection(DcMotor.Direction.REVERSE); // Backwards should pivot DOWN, or in the stowed position.
catapult2.setDirection(DcMotor.Direction.FORWARD);
foot.setDirection(DcMotor.Direction.REVERSE); // Backwards should should stay UP, or in the stowed position

// set initial subsystem behavior
// set subsystem motors to BRAKE mode
intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
catapult1.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
catapult2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
Expand Down Expand Up @@ -192,11 +222,7 @@ public void runOpMode() {
footOutButton = false;
}

boolean catapultUpButton = gamepad1.right_bumper;
boolean catapultDownButton = gamepad1.right_trigger > 0.2;
if (catapultUpButton && catapultDownButton) {
catapultUpButton = false;
}
boolean catapultFireButton = gamepad1.right_bumper;

// DRIVE CODE
// Combine the joystick requests for each axis-motion to determine each wheel's power.
Expand Down Expand Up @@ -255,19 +281,20 @@ public void runOpMode() {
}

// Determine pivot mode
if (catapultUpButton) {
if (catapultFireButton) {
pivotMode = CatapultModes.UP;
catapult1.setPower(CATAPULT_UP_POWER);
catapult2.setPower(CATAPULT_UP_POWER);
} else if (catapultDownButton) {
pivotUpTime.reset();
} else if (pivotMode == CatapultModes.UP && pivotUpTime.time() > 0.5) {
pivotMode = CatapultModes.DOWN;
catapult1.setPower(CATAPULT_DOWN_POWER);
catapult2.setPower(CATAPULT_DOWN_POWER);
} else {
pivotDownTime.reset();
} else if (pivotMode == CatapultModes.DOWN && pivotDownTime.time() > 0.5) {
pivotMode = CatapultModes.HOLD;
catapult1.setPower(CATAPULT_HOLD_POWER);
catapult2.setPower(CATAPULT_HOLD_POWER);
//Slight feed forward to keep catapult down while driving
}

// WRITE EFFECTORS - Send calculated power to wheels
Expand Down Expand Up @@ -296,10 +323,10 @@ public void runOpMode() {
telemetry.addData("Intake", "%%4.2f", intake.getPower());
telemetry.addData("Foot Power", "%4.2f", foot.getPower());
telemetry.addData("Foot MODE", "%s", footmode);
telemetry.addData("Catapult1 Current/Target/power", "%d, %d, %4.2f",
catapult1.getCurrentPosition(), catapult1.getTargetPosition(), catapult1.getPower());
telemetry.addData("Catapult2 Current/Target/power", "%d, %d, %4.2f",
catapult2.getCurrentPosition(), catapult2.getTargetPosition(), catapult2.getPower());
telemetry.addData("Catapult1 Position/power", "%d, %4.2f",
catapult1.getCurrentPosition(), catapult1.getPower());
telemetry.addData("Catapult2 Position/power", "%d, %4.2f",
catapult2.getCurrentPosition(), catapult2.getPower());
telemetry.addData("Catapult MODE", "%s", catapult_mode_str);
telemetry.update();
}
Expand Down