From 82cecba8f1ffbcb04fd02080a89bd465d9b419af Mon Sep 17 00:00:00 2001 From: robotics3 Date: Sat, 7 Oct 2023 08:54:23 -0700 Subject: [PATCH] Added new file with arm servo code --- .../ftc/teamcode/FullRobotControl.java | 185 ++++++++++++++++++ 1 file changed, 185 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FullRobotControl.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FullRobotControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FullRobotControl.java new file mode 100644 index 0000000..6cbc362 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FullRobotControl.java @@ -0,0 +1,185 @@ +package org.firstinspires.ftc.teamcode; + + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + + + +@TeleOp( name = "fullRobotControl") +public class FullRobotControl extends OpMode { + public double axial; + public double lateral; + public double yaw; + DcMotor frontRight; + DcMotor backRight; + DcMotor frontLeft; + DcMotor backLeft; + DcMotor armMotor; + Servo gripper; + Servo wrist; + public ElapsedTime runtime = new ElapsedTime(); + + + /** + * this function takes a long milliseconds parameter and sleeps + * @param millis milliseconds to sleep + */ + public void sleepmillis(long millis) { + try { + Thread.sleep(millis); + } catch (Exception e) { + } + } + + /** + * stops all drive motors + */ + public void off() { + frontRight.setPower(0); + backRight.setPower(0); + frontLeft.setPower(0); + backLeft.setPower(0); + } + /** + * User defined init method + * This method will be called once when the INIT button is pressed. + */ + + public void init() { + + + telemetry.addData("Status","In Init()"); + telemetry.update(); + frontRight = hardwareMap.dcMotor.get("Drive front rt"); + backRight = hardwareMap.dcMotor.get("Drive back rt"); + frontLeft = hardwareMap.dcMotor.get("Drive front lt"); + backLeft = hardwareMap.dcMotor.get("Drive back lt"); + armMotor = hardwareMap.dcMotor.get("armMotor"); + gripper = hardwareMap.servo.get("gripper"); + gripper.setPosition(1); + wrist = hardwareMap.servo.get("wrist"); + wrist.setPosition(1); + + } + + /** + * User defined init_loop method + * This method will be called repeatedly when the INIT button is pressed. + * This method is optional. By default this method takes no action. + */ + public void init_loop(){ + // Wait for the game to start (driver presses PLAY) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + } + /** + * User defined start method. + * This method will be called once when the PLAY button is first pressed. + * This method is optional. By default this method takes not action. Example usage: Starting another thread. + */ + public void start() { + + } + + /** + * User defined stop method + * This method will be called when this op mode is first disabled. + * The stop method is optional. By default this method takes no action. + */ + public void stop(){ + + } + + + /** + * User defined loop method. + * This method will be called repeatedly in a loop while this op mode is running + */ + double num = 2; + String speed = "plaid"; + public void loop() { + + frontLeft.setDirection(DcMotor.Direction.REVERSE); + backLeft.setDirection(DcMotor.Direction.REVERSE); + frontRight.setDirection(DcMotor.Direction.FORWARD); + backRight.setDirection(DcMotor.Direction.REVERSE); + + if(gamepad1.a){ + num = 2.5; + speed = "medium"; + } + if(gamepad1.b){ + num = 2; + speed = "fast"; + } + if(gamepad1.x){ + num = 1.75; + speed = "Ludicrous"; + } + if(gamepad1.y ){ + num = 1.5; + speed = "plaid"; + } + axial = -gamepad1.left_stick_y/num; // Note: pushing stick forward gives negative value + lateral = gamepad1.left_stick_x/num; + yaw = gamepad1.right_stick_x/(num+0.5); + + + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double leftFrontPower = axial + lateral + yaw; + double rightFrontPower = axial - lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; + double armPower = gamepad1.right_stick_y/3; + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + double max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + if(gamepad1.dpad_down){ + wrist.setPosition(1); + + } + if(gamepad1.dpad_up){ + + wrist.setPosition(0.5); + } + if(gamepad1.left_bumper){ + gripper.setPosition(0.25); + } + if(gamepad1.right_bumper){ + gripper.setPosition(1); + } + + + + + frontLeft.setPower(leftFrontPower); + frontRight.setPower(rightFrontPower); + backLeft.setPower(leftBackPower); + backRight.setPower(rightBackPower); + armMotor.setPower(armPower); + // Show the elapsed game time and wheel power + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + telemetry.addData("Back left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + telemetry.addData("Speed", speed); + + + telemetry.update(); + } +} \ No newline at end of file