diff --git a/HARDWARE.md b/HARDWARE.md index 9c57b35..4996c4c 100644 --- a/HARDWARE.md +++ b/HARDWARE.md @@ -18,10 +18,10 @@ Below are the following configurations for our robots | motor3 | control | UltraPlanetary HD hex motor | left back leg frame | Drive back lt | | I2C B0 | control | Color sensor V3 | Left outside leg frame | color left | | I2C B1 | control | Color sensor V3 | Right outside leg frame | color right | -| digital01 | control | Digital device | arm frame back right | axle encoder | | I2C B0 | expansion | 2m distance sensor | Middle Back outside leg frame | distance | | motor0 | expansion | UltraPlanetary HD hex motor | left back arm frame | arm raise | | motor1 | expansion | Core Hex Motor | right back arm frame | hang | +| motor3 | expansion | Digital device | arm frame back right | axle encoder | | Servo 0 | expansion | Servo | on arm | wrist | | Servo 1 | expansion | Servo | on arm | gripper | diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmStates.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmStates.java new file mode 100644 index 0000000..0e1166e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmStates.java @@ -0,0 +1,186 @@ +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 = "ArmState") +public class ArmStates extends OpMode { + + DcMotor arm; + DcMotor armThroughBoreEncoder; + static final double TICKS_TO_DEGREES = 0.07462686567; + static final double COUNTS_PER_MOTOR_REV = 537.6; + static final double DRIVE_GEAR_REDUCTION = 1.0; + static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI); + + + /** + * 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() { + arm.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(); + arm = hardwareMap.dcMotor.get("arm raise"); + armThroughBoreEncoder = hardwareMap.dcMotor.get("armThroughBoreEncoder"); + + } + + /** + * 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 + */ + public void loop() { +if (gamepad1.right_bumper) +{ + raisearm(200); +} + + +// int armState = 0; +// int armPos = 0; +// if(gamepad2.x) +// { +// armState = 1; +// raisearm(0 - armPos); +// armPos = 0; +// } +// if(gamepad2.y) +// { +// armState = 2; +// raisearm(1000); +// armPos = 30; +// } +// if(gamepad2.b) +// { +// armState = 2; +// raisearm(200 - armPos); +// armPos = 200; +// } +// if(gamepad2.right_bumper) +// { +// armState =+ 1; +// } +// if(gamepad2.left_bumper) +// { +// armState =- 1; +// } +// if(armState == 1) +// { +// raisearm(0 - armPos); +// armPos = 0; +// } +// if(armState == 2) +// { +// raisearm(30 - armPos); +// armPos = 30; +// } +// if(armState == 3) +// { +// raisearm(200 - armPos); +// armPos = 200; +// +// } + + + + + +// 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(); + } + public void raisearm(int degrees) + { + arm.setDirection(DcMotor.Direction.REVERSE); + armDriveWithEncoder(.2, degrees); + + } + public void armDriveWithEncoder(double speed, + double Inches) { + int newarmTarget; + + + // Determine new target position, and pass to motor controller + newarmTarget = armThroughBoreEncoder.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH); + armThroughBoreEncoder.setTargetPosition(newarmTarget); + + // Turn On RUN_TO_POSITION + arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // reset the timeout time and start motion. + arm.setPower(Math.abs(speed)); + + + arm.setPower(0); + + + // Turn off RUN_TO_POSITION + armThroughBoreEncoder.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ChassisControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ChassisControl.java index df7311e..de35c0c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ChassisControl.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ChassisControl.java @@ -92,34 +92,34 @@ public class ChassisControl extends OpMode { * User defined loop method. * This method will be called repeatedly in a loop while this op mode is running */ - double num = 1; - String speed = ""; + double num = 3; + String speed = "slow"; 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 = 3; - speed = "slow"; - } - if(gamepad1.b){ - num = 2.5; - speed = "medium"; - } - if(gamepad1.y){ - num = 2; - speed = "fast"; - } - if(gamepad1.x){ - num = 1.5; - speed = "Ludicrous"; - } - if(gamepad1.x && gamepad1.y){ - num = 1; - speed = "plaid"; - } +// if(gamepad1.a){ +// num = 3; +// speed = "slow"; +// } +//// if(gamepad1.b){ +//// num = 2.5; +//// speed = "medium"; +//// } +//// if(gamepad1.y){ +//// num = 2; +//// speed = "fast"; +//// } +//// if(gamepad1.x){ +//// num = 1.5; +//// speed = "Ludicrous"; +//// } +//// if(gamepad1.x && gamepad1.y){ +//// num = 1; +//// 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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FullRobotControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FullRobotControl.java index 69e5784..63fd0ad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FullRobotControl.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FullRobotControl.java @@ -20,10 +20,11 @@ public class FullRobotControl extends OpMode { DcMotor backRight; DcMotor frontLeft; DcMotor backLeft; - DcMotor armMotor; + //DcMotor armMotor; Servo gripper; Servo wrist; DcMotor arm; + DcMotor armThroughBoreEncoder; public ElapsedTime runtime = new ElapsedTime(); static final double TICKS_TO_DEGREES = 0.07462686567; static final double COUNTS_PER_MOTOR_REV = 537.6; @@ -50,6 +51,7 @@ public class FullRobotControl extends OpMode { backRight.setPower(0); frontLeft.setPower(0); backLeft.setPower(0); + } /** * User defined init method @@ -65,11 +67,12 @@ public class FullRobotControl extends OpMode { 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"); + //armMotor = hardwareMap.dcMotor.get("armMotor"); gripper = hardwareMap.servo.get("gripper"); gripper.setPosition(1); wrist = hardwareMap.servo.get("wrist"); arm = hardwareMap.dcMotor.get("arm raise"); + armThroughBoreEncoder = hardwareMap.dcMotor.get("arm raise"); wrist.setPosition(1); } @@ -108,7 +111,7 @@ public class FullRobotControl extends OpMode { * This method will be called repeatedly in a loop while this op mode is running */ double num = 2; - String speed = "plaid"; + String speed = "medium"; public void loop() { frontLeft.setDirection(DcMotor.Direction.REVERSE); @@ -156,57 +159,76 @@ public class FullRobotControl extends OpMode { leftBackPower /= max; rightBackPower /= max; } - if(gamepad1.dpad_down){ + if(gamepad2.dpad_down){ wrist.setPosition(1); } - if(gamepad1.dpad_up){ + if(gamepad2.dpad_up){ wrist.setPosition(0.5); } - if(gamepad1.left_bumper){ + if(gamepad2.left_trigger > 0.35) + { gripper.setPosition(0.25); } - if(gamepad1.right_bumper){ + if(gamepad2.right_trigger > 0.35){ gripper.setPosition(1); } - int armState = 0; - if(gamepad2.x) +// int armState = 0; +// int armPos = 0; +// if(gamepad2.x) +// { +// armState = 1; +// raisearm(0 - armPos); +// armPos = 0; +// } +// if(gamepad2.y) +// { +// armState = 2; +// raisearm(1000); +// armPos = 30; +// } +// if(gamepad2.b) +// { +// armState = 2; +// raisearm(200 - armPos); +// armPos = 200; +// } +// if(gamepad2.right_bumper) +// { +// armState =+ 1; +// } +// if(gamepad2.left_bumper) +// { +// armState =- 1; +// } +// if(armState == 1) +// { +// raisearm(0 - armPos); +// armPos = 0; +// } +// if(armState == 2) +// { +// raisearm(30 - armPos); +// armPos = 30; +// } +// if(armState == 3) +// { +// raisearm(200 - armPos); +// armPos = 200; +// +// } + if(gamepad2.right_stick_y > 0) { - armState = 1; - raisearm(0); + arm.setPower(armPower); } - if(gamepad2.y) + if(gamepad2.left_stick_y < 0) { - armState = 2; - raisearm(30); - } - if(gamepad2.y) - { - armState = 2; - raisearm(200); - } - if(gamepad2.right_bumper) - { - armState =+ 1; - } - if(gamepad2.left_bumper) - { - armState =- 1; - } - if(armState == 1) - { - raisearm(0); - } - if(armState == 2) - { - raisearm(30); - } - if(armState == 1) - { - raisearm(200); + arm.setPower(armPower); } + telemetry.addData("arm is at", " %7d", armThroughBoreEncoder.getCurrentPosition()); + telemetry.update(); @@ -215,29 +237,30 @@ public class FullRobotControl extends OpMode { 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); + //arm.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(); } public void raisearm(int degrees) { - armEncoder(.2, degrees*TICKS_TO_DEGREES); + arm.setDirection(DcMotor.Direction.REVERSE); + armDriveWithEncoder(.2, degrees*TICKS_TO_DEGREES); } - public void armEncoder(double speed, + public void armDriveWithEncoder(double speed, double Inches) { int newarmTarget; // Determine new target position, and pass to motor controller - newarmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH); - arm.setTargetPosition(newarmTarget); + newarmTarget = armThroughBoreEncoder.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH); + armThroughBoreEncoder.setTargetPosition(newarmTarget); // Turn On RUN_TO_POSITION arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); @@ -251,7 +274,7 @@ public class FullRobotControl extends OpMode { // Turn off RUN_TO_POSITION - arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + armThroughBoreEncoder.setMode(DcMotor.RunMode.RUN_USING_ENCODER); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/arm.java new file mode 100644 index 0000000..84175bd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/arm.java @@ -0,0 +1,181 @@ +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 = "manual control") +public class arm extends OpMode { + + DcMotor arm; + Servo gripper; + Servo wrist; + public double axial; + public double lateral; + public double yaw; + DcMotor frontRight; + DcMotor backRight; + DcMotor frontLeft; + DcMotor backLeft; + + + /** + * 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() { + arm.setPower(0); + 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(); + arm = hardwareMap.dcMotor.get("arm raise"); + gripper = hardwareMap.servo.get("gripper"); + wrist = hardwareMap.servo.get("wrist"); + 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"); + + + + } + + /** + * 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(){ + + } + + double num = 3; + /** + * User defined loop method. + * This method will be called repeatedly in a loop while this op mode is running + */ + public void loop() { + frontLeft.setDirection(DcMotor.Direction.REVERSE); + backLeft.setDirection(DcMotor.Direction.REVERSE); + frontRight.setDirection(DcMotor.Direction.FORWARD); + backRight.setDirection(DcMotor.Direction.REVERSE); + + + double armPower = gamepad2.right_stick_y/3.5; + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + + if(gamepad2.right_stick_y != 0) + { + arm.setPower(armPower); + telemetry.addData("joystick y value", gamepad2.right_stick_y); + telemetry.update(); + } + else + { + arm.setPower(0); + } + if(gamepad2.left_trigger > 0.35) + { + gripper.setPosition(0.25); + } + if(gamepad2.right_trigger > 0.35){ + gripper.setPosition(1); + } + if(gamepad2.dpad_up) + { + wrist.setPosition(0.4); + } + if(gamepad2.dpad_down) + { + wrist.setPosition(1); + } + if(gamepad2.dpad_right) + { + wrist.setPosition(0); + } + 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; + // 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; + } + frontLeft.setPower(leftFrontPower); + frontRight.setPower(rightFrontPower); + backLeft.setPower(leftBackPower); + backRight.setPower(rightBackPower); + // Show the elapsed game time and wheel power + telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + telemetry.addData("Back left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + telemetry.update(); + + + + + + + } +}