diff --git a/HARDWARE.md b/HARDWARE.md new file mode 100644 index 0000000..4ac0fdc --- /dev/null +++ b/HARDWARE.md @@ -0,0 +1,38 @@ +# FTC Hardware Config 2023 + +**DISCLAIMER:** View the robot like this. + +![Bird's eye view of robot.](/Robot.png "Bird's eye view of robot") + +Configuration Name: **cometBoTsChassis2023** + +Below are the following configurations for our robots + +| physical port | hub | robot part | robot part location | robot software config name | +|--------------- |---------------|--------------------|-----------------------|----------------------------| +| motor0 | control | HD hex motor | right front motor | Drive front rt | +| motor1 | control | HD hex motor | right back motor | Drive back rt | +| motor2 | control | HD hex motor | left front motor | Drive front lt | +| motor3 | control | HD hex motor | left back motor | Drive back lt | + | motor4 | control | expansion hub | expansion hub | | +| motor5 | control | ||| + | motor6 | control | | | | +| motor7 | control | | | | +| motor8 | control | Ultraplanetary | Back left high motor | arm raise | +| motor9 | control | |||| +| motor10 | control | |||| +| motor11 | control | |||| +| I2C0 | control | Color sensor | Left | color left | + | I2C1 | control | Color sensor | Right | color right | + | I2C0 | expansion | 2m distance sensor | Middle Back distance | +| motor0 | expansion | rev encoder | right dead wheel | | +| motor1 | expansion | rev encoder | back dead wheel | +| motor2 | expansion | rev encoder | left dead wheel | +| motor3 | expansion| |||| +| Servo 0 | expansion | Servo | on arm | wrist | +| Servo 1 | expansion | Servo | on arm | gripper | + + + + + diff --git a/Robot.png b/Robot.png new file mode 100644 index 0000000..a77147b Binary files /dev/null and b/Robot.png differ 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