this is the first auto commit

TeamCode/TeamCode/BotTwoTeleopPreset.java

@@ -0,0 +1,294 @@
+/*
+ * Copyright (c) 2025 FIRST
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to
+ * endorse or promote products derived from this software without specific prior
+ * written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+ * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode.teamcode;
+
+import static com.qualcomm.robotcore.hardware.DcMotor.ZeroPowerBehavior.BRAKE;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+/*
+ * This file includes a teleop (driver-controlled) file for the goBILDA® StarterBot for the
+ * 2025-2026 FIRST® Tech Challenge season DECODE™. It leverages a differential/Skid-Steer
+ * system for robot mobility, one high-speed motor driving two "launcher wheels", and two servos
+ * which feed that launcher.
+ *
+ * Likely the most niche concept we'll use in this example is closed-loop motor velocity control.
+ * This control method reads the current speed as reported by the motor's encoder and applies a varying
+ * amount of power to reach, and then hold a target velocity. The FTC SDK calls this control method
+ * "RUN_USING_ENCODER". This contrasts to the default "RUN_WITHOUT_ENCODER" where you control the power
+ * applied to the motor directly.
+ * Since the dynamics of a launcher wheel system varies greatly from those of most other FTC mechanisms,
+ * we will also need to adjust the "PIDF" coefficients with some that are a better fit for our application.
+ */
+
+@TeleOp(name = "BotTwoTeleopPreset", group = "StarterBot")
+//@Disabled
+public class BotTwoTeleopPreset extends OpMode {
+
+    /*
+     * When we control our launcher motor, we are using encoders. These allow the control system
+     * to read the current speed of the motor and apply more or less power to keep it at a constant
+     * velocity. Here we are setting the target, and minimum velocity that the launcher should run
+     * at. The minimum velocity is a threshold for determining when to fire.
+     */
+    final double LAUNCHER_TARGET_VELOCITY = 1125;
+    final double LAUNCHER_MIN_VELOCITY = 1075;
+
+    // Declare OpMode members.
+    private DcMotor leftFrontDrive = null;
+    private DcMotor rightFrontDrive = null;
+    private DcMotor leftBackDrive = null;
+    private DcMotor rightBackDrive = null;
+    private DcMotor intake = null;
+
+
+    private DcMotor shooter = null;
+
+
+    private CRServo leftPusher = null;
+
+
+    private CRServo rightPusher = null;
+
+
+    private boolean shotTog = false;
+
+
+
+
+    /*
+     * TECH TIP: State Machines
+     * We use a "state machine" to control our launcher motor and feeder servos in this program.
+     * The first step of a state machine is creating an enum that captures the different "states"
+     * that our code can be in.
+     * The core advantage of a state machine is that it allows us to continue to loop through all
+     * of our code while only running specific code when it's necessary. We can continuously check
+     * what "State" our machine is in, run the associated code, and when we are done with that step
+     * move on to the next state.
+     * This enum is called the "LaunchState". It reflects the current condition of the shooter
+     * motor and we move through the enum when the user asks our code to fire a shot.
+     * It starts at idle, when the user requests a launch, we enter SPIN_UP where we get the
+     * motor up to speed, once it meets a minimum speed then it starts and then ends the launch process.
+     * We can use higher level code to cycle through these states. But this allows us to write
+     * functions and autonomous routines in a way that avoids loops within loops, and "waits".
+     */
+
+
+
+    // Setup a variable for each drive wheel to save power level for telemetry
+    double leftFrontPower;
+    double rightFrontPower;
+    double leftBackPower;
+    double rightBackPower;
+
+    /*
+     * Code to run ONCE when the driver hits INIT
+     */
+    @Override
+    public void init() {
+
+
+        /*
+         * Initialize the hardware variables. Note that the strings used here as parameters
+         * to 'get' must correspond to the names assigned during the robot configuration
+         * step.
+         */
+        leftFrontDrive = hardwareMap.get(DcMotor.class, "leftFront");
+        rightFrontDrive = hardwareMap.get(DcMotor.class, "rightFront");
+        leftBackDrive = hardwareMap.get(DcMotor.class, "leftBack");
+        rightBackDrive = hardwareMap.get(DcMotor.class, "rightBack");
+        intake = hardwareMap.get(DcMotor.class, "intake");
+        shooter = hardwareMap.get(DcMotor.class, "shooter");
+        leftPusher = hardwareMap.get(CRServo.class, "leftPusher");
+        rightPusher = hardwareMap.get(CRServo.class, "rightPusher");
+
+
+        /*
+         * To drive forward, most robots need the motor on one side to be reversed,
+         * because the axles point in opposite directions. Pushing the left stick forward
+         * MUST make robot go forward. So adjust these two lines based on your first test drive.
+         * Note: The settings here assume direct drive on left and right wheels. Gear
+         * Reduction or 90 Deg drives may require direction flips
+         */
+        leftFrontDrive.setDirection(DcMotor.Direction.REVERSE);
+        rightFrontDrive.setDirection(DcMotor.Direction.FORWARD);
+        leftBackDrive.setDirection(DcMotor.Direction.REVERSE);
+        rightBackDrive.setDirection(DcMotor.Direction.FORWARD);
+
+        /*
+         * Here we set our launcher to the RUN_USING_ENCODER runmode.
+         * If you notice that you have no control over the velocity of the motor, it just jumps
+         * right to a number much higher than your set point, make sure that your encoders are plugged
+         * into the port right beside the motor itself. And that the motors polarity is consistent
+         * through any wiring.
+         */
+
+        /*
+         * Setting zeroPowerBehavior to BRAKE enables a "brake mode". This causes the motor to
+         * slow down much faster when it is coasting. This creates a much more controllable
+         * drivetrain. As the robot stops much quicker.
+         */
+        leftFrontDrive.setZeroPowerBehavior(BRAKE);
+        rightFrontDrive.setZeroPowerBehavior(BRAKE);
+        leftBackDrive.setZeroPowerBehavior(BRAKE);
+        rightBackDrive.setZeroPowerBehavior(BRAKE);
+
+        /*
+         * set Feeders to an initial value to initialize the servo controller
+         */
+
+
+
+
+        /*
+         * Much like our drivetrain motors, we set the left feeder servo to reverse so that they
+         * both work to feed the ball into the robot.
+         */
+
+
+        /*
+         * Tell the driver that initialization is complete.
+         */
+        telemetry.addData("Status", "Initialized");
+    }
+
+    /*
+     * Code to run REPEATEDLY after the driver hits INIT, but before they hit START
+     */
+    @Override
+    public void init_loop() {
+    }
+
+    /*
+     * Code to run ONCE when the driver hits START
+     */
+    @Override
+    public void start() {
+    }
+
+    /*
+     * Code to run REPEATEDLY after the driver hits START but before they hit STOP
+     */
+    @Override
+    public void loop() {
+
+
+        if(gamepad2.a) {
+            shotTog = true;
+        }
+        else if (gamepad2.b) {
+            shotTog = false;
+        }
+
+        if (shotTog) {
+            shooter.setPower(-0.55);
+        }
+        else {
+            shooter.setPower(0);
+        }
+        //else if (gamepad2.y) {
+        // shooter.setPower(-.65);
+        // }
+        //else if(gamepad2.b) {
+        // shooter.setPower(-0.8);
+        // }
+        // else if(gamepad2.a) {
+        // shooter.setPower(-1);
+        //}
+
+
+
+
+        if (gamepad1.right_bumper){
+            intake.setPower(-1);
+        }
+        else{
+            intake.setPower(0);
+        }
+
+        if (gamepad2.left_bumper){
+            leftPusher.setPower(1);
+        }
+        else{
+            leftPusher.setPower(0);
+        }
+
+        if(gamepad2.right_bumper){
+            rightPusher.setPower(-1);
+        }
+        else{
+            rightPusher.setPower(0);
+        }
+
+
+        mecanumDrive(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x);
+    }
+
+    /*
+     * Here we give the user control of the speed of the launcher motor without automatically
+     * queuing a shot.
+     */
+
+
+    /*
+     * Code to run ONCE after the driver hits STOP
+     */
+    @Override
+    public void stop() {
+    }
+
+    void mecanumDrive(double forward, double strafe, double rotate){
+
+        /* the denominator is the largest motor power (absolute value) or 1
+         * This ensures all the powers maintain the same ratio,
+         * but only if at least one is out of the range [-1, 1]
+         */
+        double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(rotate), 1);
+
+        leftFrontPower = (forward + strafe + rotate) / denominator;
+        rightFrontPower = (forward - strafe - rotate) / denominator;
+        leftBackPower = (forward - strafe + rotate) / denominator;
+        rightBackPower = (forward + strafe - rotate) / denominator;
+
+        leftFrontDrive.setPower(leftFrontPower);
+        rightFrontDrive.setPower(rightFrontPower);
+        leftBackDrive.setPower(leftBackPower);
+        rightBackDrive.setPower(rightBackPower);
+
+    }
+
+
+}