diff --git a/FTC_Team1/LinearSlideTest1.java b/FTC_Team1/LinearSlideTest1.java index f749b27..3f28ed9 100644 --- a/FTC_Team1/LinearSlideTest1.java +++ b/FTC_Team1/LinearSlideTest1.java @@ -1,4 +1,3 @@ -package org.firstinspires.ftc.robotcontroller.external.samples; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -50,9 +49,16 @@ public class LinearSlideTest1 extends LinearOpMode { private DcMotor rightFrontDrive = null; private DcMotor rightBackDrive = null; private DcMotor slideArm = null; + private CRServo upperServo = null; + private CRServo lowerServo = null; + private DcMotor clawState = null; private double slideArmCD = 0.0; + private double clawCD = 0.0; LinearSlideStates slideArmState = LinearSlideStates.HoldingTwo; private double motorPosition = 0.0; + private DcMotor intakePivotMotor = null; + private DcMotor intakeState = null; + private DcMotor intakeCD = null; slideArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); slideArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODERS); @@ -65,6 +71,8 @@ public void runOpMode() { rightFrontDrive = hardwareMap.get(DcMotor.class, "fRight"); rightBackDrive = hardwareMap.get(DcMotor.class, "bRight"); slideArm = hardwareMap.get(DcMotor.class, "slideMotor"); + upperServo = hardwareMap.get(CRServo.class, "topIntake"); + lowerServo = hardwareMap.get(CRServo.class, "bottomIntake"); leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); leftBackDrive.setDirection(DcMotor.Direction.REVERSE); @@ -72,6 +80,8 @@ public void runOpMode() { telemetry.addData("Status", "Initialized"); telemetry.update(); + clawState = 1; + waitForStart(); runtime.reset(); @@ -109,6 +119,59 @@ public void runOpMode() { slideArmCD = runtime.time(); } + clawState = 1 + + if(gamepad1.a && runtime.time()-clawCD >= 0.2){ + if(clawState == 1){ + clawState = 2; + clawCD = runtime.time(); + } else if(clawState >= 2) { + clawState = 1; + clawCD = runtime.time(); + } + + if(clawState == 1){ + upperServo.setPower(1.0); + lowerServo.setPower(-1.0); + } else if(clawState == 2){ + lowerServo.setPower(1.0); + upperServo.setPower(-1.0); + } + + if(gamepad1.y && runtime.time()-intakeCD >= 0.2){ + intakeState = intakeState(1); + intakeCD = runtime.time(); + } + if(gamepad1.x && runtime.time()-intakeCD >= 0.2){ + intakeState = intakeState(2); + intakeCD = runtime.time(); + } + + intakeState = 0 + + if(gamepad1.y){ + intakeState = 1; + intakeCD = runtime.time(); + } + + if (gamepad1.x) { + intakeState = 2; + intakeCD = runtime.time(); + } + if (!gamepad1.y && !gamepad1.x) { + intakeState = 0 + intakeCD = runtime.time(); + } + + + if(intakeState == 0){ + intakePivotMotor.setPower(0.3); + } else if (intakeState == 1) { + intakePivotMotor.setPower(0.5); + } else if (intakeState == 2) { + intakePivotMotor.setPower(-0.3); + } + checkAndSetSlideState(); // So this is jus thte switch states code but turned neater // Send calculated power to wheels diff --git a/FTC_Team1/RobotCode/Into The Deep/AutoLongBasket.java b/FTC_Team1/RobotCode/Into The Deep/AutoLongBasket.java new file mode 100644 index 0000000..6e1ca4e --- /dev/null +++ b/FTC_Team1/RobotCode/Into The Deep/AutoLongBasket.java @@ -0,0 +1,215 @@ +package RobotCode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +//@Disabled haha you thought I would do it again lol +@Autonomous(name="Autonomous Long Basket", group="Linear Opmode") + +public class AutoLongBasket extends LinearOpMode { + private final double wheelCircumference = 75*3.14; + private final double gearReduction = 3.61*5.23; + private final double counts = 28.0; + + private final double rev = counts*gearReduction; + private final int revPerMM = (int)rev/(int)wheelCircumference; + private final double inches = revPerMM*25.4; + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotorEx leftFrontDrive = null; + private DcMotorEx leftBackDrive = null; + private DcMotorEx rightFrontDrive = null; + private DcMotorEx rightBackDrive = null; + private DcMotor clawPivotMotor = null; + private CRServo upperServo = null; + private CRServo lowerServo = null; + + @Override + + public void runOpMode() { + leftFrontDrive = hardwareMap.get(DcMotorEx.class, "fLeft"); + leftBackDrive = hardwareMap.get(DcMotorEx.class, "bLeft"); + rightFrontDrive = hardwareMap.get(DcMotorEx.class, "fRight"); + rightBackDrive = hardwareMap.get(DcMotorEx.class, "bRight"); + clawPivotMotor = hardwareMap.get(DcMotor.class, "intakePivotMotor"); + upperServo = hardwareMap.get(CRServo.class, "topIntake"); + lowerServo = hardwareMap.get(CRServo.class, "bottomIntake"); + + + leftFrontDrive.setDirection(DcMotorEx.Direction.REVERSE); + leftBackDrive.setDirection(DcMotorEx.Direction.REVERSE); + + // Wait for the game to start (driver presses PLAY) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + int runCount=0; + waitForStart(); + runtime.reset(); + + + } + +// vvvvvvvvvvvvvvvvvvvv THIS IS WHERE THE FUNCTIONS ARE vvvvvvvvvvvvvvvvvvvv + + public void input(double leftFront, double rightFront, double leftBack, double rightBack) + { + leftFrontDrive.setPower(leftFront); + rightFrontDrive.setPower(rightFront); + leftBackDrive.setPower(leftBack); + rightBackDrive.setPower(rightBack); + sleep(500); + } + public void turnLeft(int angle) + { + int convert=revPerMM*angle*(38/10)+(angle*12/10); + encoders(-convert, convert, -convert, convert); + } + public void turnRight(int angle) + { + int convert=revPerMM*angle*(38/10)+(angle*12/10); + encoders(convert, -convert, convert, -convert); + } + public void driveEncoders(int target) + { + encoders(target, target, target, target); + } + public void backEncoders(int target) + { + encoders(-target, -target, -target, -target); + } + public void rightEncoders(int target) + { + encoders(target, -target, -target, target); + } + public void leftEncoders(int target) + { + encoders(-target, target, target, -target); + } + public void leftTopDiagonal(int target) + { + encoders(0, target, target, 0); + } + public void rightTopDiagonal(int target) + { + encoders(target, 0, 0, target); + } + public void leftBottomDiagonal(int target) + { + encoders(-target, 0, 0, -target); + } + public void rightBottomDiagonal(int target) + { + encoders(0, -target, -target, 0); + } + public void encoders(int leftFront, int rightFront, int leftBack, int rightBack) + { + leftFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + leftBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + leftFrontDrive.setTargetPosition(leftFront*revPerMM); + rightFrontDrive.setTargetPosition(rightFront*revPerMM); + leftBackDrive.setTargetPosition(leftBack*revPerMM); + rightBackDrive.setTargetPosition(rightBack*revPerMM); + + leftFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + leftBackDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightBackDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + double power=0.2; + leftFrontDrive.setVelocity(1500); + rightFrontDrive.setVelocity(1500); + leftBackDrive.setVelocity(1500); + rightBackDrive.setVelocity(1500); + while (opModeIsActive()&&leftFrontDrive.isBusy()||rightFrontDrive.isBusy()||leftBackDrive.isBusy()||rightBackDrive.isBusy()) + { + } + leftFrontDrive.setPower(0); + rightFrontDrive.setPower(0); + leftBackDrive.setPower(0); + rightBackDrive.setPower(0); + } + public void drive() + { + leftFrontDrive.setPower(1.0); + rightFrontDrive.setPower(1.0); + leftBackDrive.setPower(1.0); + rightBackDrive.setPower(1.0); + //input(0.4, 0.4, 0.4, 0.4); + } + public void topRight() + { + input(0.4, 0, 0, 0.4); + } + public void right() + { + input(0.4, -0.4, -0.4, 0.4); + } + public void bottomRight() + { + input(0, -0.4, -0.4, 0); + } + public void back() + { + input(-0.4, -0.4, -0.4, -0.4); + } + public void bottomLeft() + { + input(-0.4, 0, 0, -0.4); + } + public void left() + { + input(-0.4, 0.4, 0.4, -0.4); + } + public void topLeft() + { + input(0, 0.4, 0.4, 0); + } + public void stop(double time) + { + input(0, 0, 0, 0); + } + public void basket(){ + turnLeft(90); + driveEncoders(610); + // arm to score thing here + slideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); //idk sam said to put it + slideMotor.setTargetPosition(2505); // arm up to basket + slideMotor.setVelocity(1500); //idk sam said to put it + while (slideMotor.isBusy()){ + } + slideArm.setPower(0); + intakePivotMotor.setPower(0.5); + while(intakePivotMotor.isBusy()){ + } + upperServo.setPower(1.0); + lowerServo.setPower(-1.0); + sleep(3000); + intakePivotMotor.setPower(0.3); + //this is where the scoring thing stops + turnLeft(180); + driveEncoders(2440); + turnLeft(90); + driveEncoders(1220); + rightEncoders(610); + backEncoders(1220); + } + public void observation(){ + driveEncoders(300); + turnLeft(90); + driveEncoders(1371.6); + turnLeft(45); + sleep(3000) + turnRight(45); + backEncoders(1676.2); + leftEncoders(300); + } +} diff --git a/FTC_Team1/RobotCode/Into The Deep/BasketToPark1.java b/FTC_Team1/RobotCode/Into The Deep/BasketToPark1.java new file mode 100644 index 0000000..c2b8efa --- /dev/null +++ b/FTC_Team1/RobotCode/Into The Deep/BasketToPark1.java @@ -0,0 +1,220 @@ +package RobotCode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +//@Disabled haha you thought I would do it again lol +@Autonomous(name="Basket To Park 1", group="Linear Opmode") + +public class AutoLongBasket extends LinearOpMode { + private final double wheelCircumference = 75*3.14; + private final double gearReduction = 3.61*5.23; + private final double counts = 28.0; + + private final double rev = counts*gearReduction; + private final int revPerMM = (int)rev/(int)wheelCircumference; + private final double inches = revPerMM*25.4; + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotorEx leftFrontDrive = null; + private DcMotorEx leftBackDrive = null; + private DcMotorEx rightFrontDrive = null; + private DcMotorEx rightBackDrive = null; + private DcMotor clawPivotMotor = null; + private CRServo upperServo = null; + private CRServo lowerServo = null; + + @Override + + public void runOpMode() { + leftFrontDrive = hardwareMap.get(DcMotorEx.class, "fLeft"); + leftBackDrive = hardwareMap.get(DcMotorEx.class, "bLeft"); + rightFrontDrive = hardwareMap.get(DcMotorEx.class, "fRight"); + rightBackDrive = hardwareMap.get(DcMotorEx.class, "bRight"); + clawPivotMotor = hardwareMap.get(DcMotor.class, "intakePivotMotor"); + upperServo = hardwareMap.get(CRServo.class, "topIntake"); + lowerServo = hardwareMap.get(CRServo.class, "bottomIntake"); + + + leftFrontDrive.setDirection(DcMotorEx.Direction.REVERSE); + leftBackDrive.setDirection(DcMotorEx.Direction.REVERSE); + + // Wait for the game to start (driver presses PLAY) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + int runCount=0; + waitForStart(); + runtime.reset(); + + + } + +// vvvvvvvvvvvvvvvvvvvv THIS IS WHERE THE FUNCTIONS ARE vvvvvvvvvvvvvvvvvvvv + + public void input(double leftFront, double rightFront, double leftBack, double rightBack) + { + leftFrontDrive.setPower(leftFront); + rightFrontDrive.setPower(rightFront); + leftBackDrive.setPower(leftBack); + rightBackDrive.setPower(rightBack); + sleep(500); + } + public void turnLeft(int angle) + { + int convert=revPerMM*angle*(38/10)+(angle*12/10); + encoders(-convert, convert, -convert, convert); + } + public void turnRight(int angle) + { + int convert=revPerMM*angle*(38/10)+(angle*12/10); + encoders(convert, -convert, convert, -convert); + } + public void driveEncoders(int target) + { + encoders(target, target, target, target); + } + public void backEncoders(int target) + { + encoders(-target, -target, -target, -target); + } + public void rightEncoders(int target) + { + encoders(target, -target, -target, target); + } + public void leftEncoders(int target) + { + encoders(-target, target, target, -target); + } + public void leftTopDiagonal(int target) + { + encoders(0, target, target, 0); + } + public void rightTopDiagonal(int target) + { + encoders(target, 0, 0, target); + } + public void leftBottomDiagonal(int target) + { + encoders(-target, 0, 0, -target); + } + public void rightBottomDiagonal(int target) + { + encoders(0, -target, -target, 0); + } + public void encoders(int leftFront, int rightFront, int leftBack, int rightBack) + { + leftFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + leftBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + leftFrontDrive.setTargetPosition(leftFront*revPerMM); + rightFrontDrive.setTargetPosition(rightFront*revPerMM); + leftBackDrive.setTargetPosition(leftBack*revPerMM); + rightBackDrive.setTargetPosition(rightBack*revPerMM); + + leftFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + leftBackDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightBackDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + double power=0.2; + leftFrontDrive.setVelocity(1500); + rightFrontDrive.setVelocity(1500); + leftBackDrive.setVelocity(1500); + rightBackDrive.setVelocity(1500); + while (opModeIsActive()&&leftFrontDrive.isBusy()||rightFrontDrive.isBusy()||leftBackDrive.isBusy()||rightBackDrive.isBusy()) + { + } + leftFrontDrive.setPower(0); + rightFrontDrive.setPower(0); + leftBackDrive.setPower(0); + rightBackDrive.setPower(0); + } + public void drive() + { + leftFrontDrive.setPower(1.0); + rightFrontDrive.setPower(1.0); + leftBackDrive.setPower(1.0); + rightBackDrive.setPower(1.0); + //input(0.4, 0.4, 0.4, 0.4); + } + public void topRight() + { + input(0.4, 0, 0, 0.4); + } + public void right() + { + input(0.4, -0.4, -0.4, 0.4); + } + public void bottomRight() + { + input(0, -0.4, -0.4, 0); + } + public void back() + { + input(-0.4, -0.4, -0.4, -0.4); + } + public void bottomLeft() + { + input(-0.4, 0, 0, -0.4); + } + public void left() + { + input(-0.4, 0.4, 0.4, -0.4); + } + public void topLeft() + { + input(0, 0.4, 0.4, 0); + } + public void stop(double time) + { + input(0, 0, 0, 0); + } + public void basket(){ + turnLeft(90); + driveEncoders(610); + rightEncoders(200); + // arm to score thing here + slideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); //idk sam said to put it + slideMotor.setTargetPosition(2505); // arm up to basket + slideMotor.setVelocity(1500); //idk sam said to put it + while (slideMotor.isBusy() && intakePivotMotor.isBusy()){ + } + upperServo.setPower(1.0); + lowerServo.setPower(-1.0); + sleep(3000); + intakePivotMotor.setPower(0.3); + //this is where the scoring thing stops + leftEncoders(200); + + } + public void observation(){ + driveEncoders(300); + turnLeft(90); + driveEncoders(1371.6); + turnLeft(45); + sleep(1500) + // arm to score thing here + slideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); //idk sam said to put it + slideMotor.setTargetPosition(2505); // arm up to basket + slideMotor.setVelocity(1500); //idk sam said to put it + while (slideMotor.isBusy() && intakePivotMotor.isBusy()){ + } + upperServo.setPower(1.0); + lowerServo.setPower(-1.0); + + intakePivotMotor.setPower(0.3); + turnRight(45); + backEncoders(1676.2); + leftEncoders(300); + //this is where the scoring thing stops + + } +} diff --git a/FTC_Team1/RobotCode/Into The Deep/BasketToPark2.java b/FTC_Team1/RobotCode/Into The Deep/BasketToPark2.java new file mode 100644 index 0000000..74f387d --- /dev/null +++ b/FTC_Team1/RobotCode/Into The Deep/BasketToPark2.java @@ -0,0 +1,221 @@ +package RobotCode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +//@Disabled haha you thought I would do it again lol +@Autonomous(name="Autonomous Long Basket", group="Linear Opmode") + +public class AutoLongBasket extends LinearOpMode { + private final double wheelCircumference = 75*3.14; + private final double gearReduction = 3.61*5.23; + private final double counts = 28.0; + + private final double rev = counts*gearReduction; + private final int revPerMM = (int)rev/(int)wheelCircumference; + private final double inches = revPerMM*25.4; + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotorEx leftFrontDrive = null; + private DcMotorEx leftBackDrive = null; + private DcMotorEx rightFrontDrive = null; + private DcMotorEx rightBackDrive = null; + private DcMotor clawPivotMotor = null; + private CRServo upperServo = null; + private CRServo lowerServo = null; + + @Override + + public void runOpMode() { + leftFrontDrive = hardwareMap.get(DcMotorEx.class, "fLeft"); + leftBackDrive = hardwareMap.get(DcMotorEx.class, "bLeft"); + rightFrontDrive = hardwareMap.get(DcMotorEx.class, "fRight"); + rightBackDrive = hardwareMap.get(DcMotorEx.class, "bRight"); + clawPivotMotor = hardwareMap.get(DcMotor.class, "intakePivotMotor"); + upperServo = hardwareMap.get(CRServo.class, "topIntake"); + lowerServo = hardwareMap.get(CRServo.class, "bottomIntake"); + + + leftFrontDrive.setDirection(DcMotorEx.Direction.REVERSE); + leftBackDrive.setDirection(DcMotorEx.Direction.REVERSE); + + // Wait for the game to start (driver presses PLAY) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + int runCount=0; + waitForStart(); + runtime.reset(); + + + } + +// vvvvvvvvvvvvvvvvvvvv THIS IS WHERE THE FUNCTIONS ARE vvvvvvvvvvvvvvvvvvvv + + public void input(double leftFront, double rightFront, double leftBack, double rightBack) + { + leftFrontDrive.setPower(leftFront); + rightFrontDrive.setPower(rightFront); + leftBackDrive.setPower(leftBack); + rightBackDrive.setPower(rightBack); + sleep(500); + } + public void turnLeft(int angle) + { + int convert=revPerMM*angle*(38/10)+(angle*12/10); + encoders(-convert, convert, -convert, convert); + } + public void turnRight(int angle) + { + int convert=revPerMM*angle*(38/10)+(angle*12/10); + encoders(convert, -convert, convert, -convert); + } + public void driveEncoders(int target) + { + encoders(target, target, target, target); + } + public void backEncoders(int target) + { + encoders(-target, -target, -target, -target); + } + public void rightEncoders(int target) + { + encoders(target, -target, -target, target); + } + public void leftEncoders(int target) + { + encoders(-target, target, target, -target); + } + public void leftTopDiagonal(int target) + { + encoders(0, target, target, 0); + } + public void rightTopDiagonal(int target) + { + encoders(target, 0, 0, target); + } + public void leftBottomDiagonal(int target) + { + encoders(-target, 0, 0, -target); + } + public void rightBottomDiagonal(int target) + { + encoders(0, -target, -target, 0); + } + public void encoders(int leftFront, int rightFront, int leftBack, int rightBack) + { + leftFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + leftBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + leftFrontDrive.setTargetPosition(leftFront*revPerMM); + rightFrontDrive.setTargetPosition(rightFront*revPerMM); + leftBackDrive.setTargetPosition(leftBack*revPerMM); + rightBackDrive.setTargetPosition(rightBack*revPerMM); + + leftFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + leftBackDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightBackDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + double power=0.2; + leftFrontDrive.setVelocity(1500); + rightFrontDrive.setVelocity(1500); + leftBackDrive.setVelocity(1500); + rightBackDrive.setVelocity(1500); + while (opModeIsActive()&&leftFrontDrive.isBusy()||rightFrontDrive.isBusy()||leftBackDrive.isBusy()||rightBackDrive.isBusy()) + { + } + leftFrontDrive.setPower(0); + rightFrontDrive.setPower(0); + leftBackDrive.setPower(0); + rightBackDrive.setPower(0); + } + public void drive() + { + leftFrontDrive.setPower(1.0); + rightFrontDrive.setPower(1.0); + leftBackDrive.setPower(1.0); + rightBackDrive.setPower(1.0); + //input(0.4, 0.4, 0.4, 0.4); + } + public void topRight() + { + input(0.4, 0, 0, 0.4); + } + public void right() + { + input(0.4, -0.4, -0.4, 0.4); + } + public void bottomRight() + { + input(0, -0.4, -0.4, 0); + } + public void back() + { + input(-0.4, -0.4, -0.4, -0.4); + } + public void bottomLeft() + { + input(-0.4, 0, 0, -0.4); + } + public void left() + { + input(-0.4, 0.4, 0.4, -0.4); + } + public void topLeft() + { + input(0, 0.4, 0.4, 0); + } + public void stop(double time) + { + input(0, 0, 0, 0); + } + public void basket(){ + turnLeft(90); + driveEncoders(2135); + // arm to score thing here + slideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); //idk sam said to put it + slideMotor.setTargetPosition(2505); // arm up to basket + slideMotor.setVelocity(1500); //idk sam said to put it + while (slideMotor.isBusy()){ + } + slideArm.setPower(0); + intakePivotMotor.setPower(0.5); + while(intakePivotMotor.isBusy()){ + } + upperServo.setPower(1.0); + lowerServo.setPower(-1.0); + sleep(3000); + intakePivotMotor.setPower(0.3); + //this is where the scoring thing stops + backEncoders(2135); + } + public void observation(){ + turnLeft(90); + driveEncoders(305); + // arm to score thing here + slideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); //idk sam said to put it + slideMotor.setTargetPosition(2505); // arm up to basket + slideMotor.setVelocity(1500); //idk sam said to put it + while (slideMotor.isBusy()){ + } + slideArm.setPower(0); + intakePivotMotor.setPower(0.5); + while(intakePivotMotor.isBusy()){ + } + upperServo.setPower(1.0); + lowerServo.setPower(-1.0); + sleep(3000); + intakePivotMotor.setPower(0.3); + //this is where the scoring thing stops + backEncoders(2135); + } +} +//i hate you sehwan sam minh jang \ No newline at end of file diff --git a/FTC_Team1/RobotCode/Into The Deep/autoshortbasket.java b/FTC_Team1/RobotCode/Into The Deep/autoshortbasket.java new file mode 100644 index 0000000..f8b1b6e --- /dev/null +++ b/FTC_Team1/RobotCode/Into The Deep/autoshortbasket.java @@ -0,0 +1,261 @@ + +package RobotCode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +@Autonomous(name="Basic: Omni Linear OpMode", group="Linear Opmode") + +public class MechanumAutonomous extends LinearOpMode { + private final double wheelCircumference = 75*3.14; + private final double gearReduction = 3.61*5.23; + private final double counts = 28.0; + + private int slideMotor; + + private final double rev = counts*gearReduction; + private final int revPerMM = (int)rev/(int)wheelCircumference; + private final double inches = revPerMM*25.4; + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotorEx leftFrontDrive = null; + private DcMotorEx leftBackDrive = null; + private DcMotorEx rightFrontDrive = null; + private DcMotorEx rightBackDrive = null; + private DcMotor slideArm = null; + private CRServo upperLower = null; + private CRServo lowerServo = null; + private DcMotor clawState = null; + private double slideArmCD = 0.0; + private double clawCD = 0.0; + private DcMotor intakePivotMotor = null; + private DcMotor intakeState = null; + private DcMotor intakeCD = null; + + + slideArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + slideArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODERS); + + + + @Override + public void runOpMode() { + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + leftFrontDrive = hardwareMap.get(DcMotorEx.class, "fLeft"); + leftBackDrive = hardwareMap.get(DcMotorEx.class, "bLeft"); + rightFrontDrive = hardwareMap.get(DcMotorEx.class, "fRight"); + rightBackDrive = hardwareMap.get(DcMotorEx.class, "bRight"); + + slideMotor = hardwareMap.get(CRServo.class,"armMotor"); + slideArm = hardwareMap.get(DcMotor.class, "slideMotor"); + upperServo = hardwareMap.get(CRServo.class, "topIntake"); + lowerServo = hardwareMap.get(CRServo.class, "bottomIntake"); + + + + leftFrontDrive.setDirection(DcMotorEx.Direction.REVERSE); + leftBackDrive.setDirection(DcMotorEx.Direction.REVERSE); + rightFrontDrive.setDirection(DcMotorEx.Direction.REVERSE); + rightBackDrive.setDirection(DcMotorEx.Direction.FORWARD); + // Wait for the game to start (driver presses PLAY) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + int count=0; + + clawState = 1; + + waitForStart(); + runtime.reset(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()&&count==0) { + /* + + Net + driveEncoders(2440); + turnRight(45); + armShoot(); + turnLeft(45); + backEncoders(2440) + + + + */ + //High Basket is 1092 cm (1092cm/0.436 encoders = 2504.58 encoders) + slideArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + slideArm.setTargetPosition(2505); + slideArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + slideArm.setVelocity(1500); + while (slideArm.isBusy()) + { + } + lowerServo.setPower(2505); + upperServo.setPower(2505); + + intakeState = 0 + + + if(intakeState == 0){ + intakePivotMotor.setPower(0.3); + } else if (intakeState == 1) { + intakePivotMotor.setPower(0.5); + } else if (intakeState == 2) { + intakePivotMotor.setPower(-0.3); + } + + + + } + public void input(double leftFront, double rightFront, double leftBack, double rightBack) + { + leftFrontDrive.setPower(leftFront); + rightFrontDrive.setPower(rightFront); + leftBackDrive.setPower(leftBack); + rightBackDrive.setPower(rightBack); + sleep(500); + } + public void turnLeft(int angle) + { + int convert=revPerMM*angle*(38/10)+(angle*12/10); + encoders(-convert, convert, -convert, convert); + } + public void turnRight(int angle) + { + int convert=revPerMM*angle*(38/10)+(angle*12/10); + encoders(convert, -convert, convert, -convert); + } + public void driveEncoders(int target) + { + encoders(target, target, target, target); + } + public void backEncoders(int target) + { + encoders(-target, -target, -target, -target); + } + public void rightEncoders(int target) + { + encoders(target, -target, -target, target); + } + public void leftEncoders(int target) + { + encoders(-target, target, target, -target); + } + public void leftTopDiagonal(int target) + { + encoders(0, target, target, 0); + } + public void rightTopDiagonal(int target) + { + encoders(target, 0, 0, target); + } + public void leftBottomDiagonal(int target) + { + encoders(-target, 0, 0, -target); + } + public void rightBottomDiagonal(int target) + { + encoders(0, -target, -target, 0); + } + public void encoders(int leftFront, int rightFront, int leftBack, int rightBack) + { + leftFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + leftBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + leftFrontDrive.setTargetPosition(leftFront*revPerMM); + rightFrontDrive.setTargetPosition(rightFront*revPerMM); + leftBackDrive.setTargetPosition(leftBack*revPerMM); + rightBackDrive.setTargetPosition(rightBack*revPerMM); + + leftFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + leftBackDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightBackDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + double power=0.2; + leftFrontDrive.setVelocity(1500); + rightFrontDrive.setVelocity(1500); + leftBackDrive.setVelocity(1500); + rightBackDrive.setVelocity(1500); + while (opModeIsActive()&&leftFrontDrive.isBusy()||rightFrontDrive.isBusy()||leftBackDrive.isBusy()||rightBackDrive.isBusy()) + { + } + leftFrontDrive.setPower(0); + rightFrontDrive.setPower(0); + leftBackDrive.setPower(0); + rightBackDrive.setPower(0); + } + public void drive() + { + leftFrontDrive.setPower(1.0); + rightFrontDrive.setPower(1.0); + leftBackDrive.setPower(1.0); + rightBackDrive.setPower(1.0); + //input(0.4, 0.4, 0.4, 0.4); + } + public void topRight() + { + input(0.4, 0, 0, 0.4); + } + public void right() + { + input(0.4, -0.4, -0.4, 0.4); + } + public void bottomRight() + { + input(0, -0.4, -0.4, 0); + } + public void back() + { + input(-0.4, -0.4, -0.4, -0.4); + } + public void bottomLeft() + { + input(-0.4, 0, 0, -0.4); + } + public void left() + { + input(-0.4, 0.4, 0.4, -0.4); + } + public void topLeft() + { + input(0, 0.4, 0.4, 0); + } + public void stop(double time) + { + input(0, 0, 0, 0); + } + public void observation(){ + driveEncoders(2440); + turnLeft(45); + + turnRight(45); + backEncoders(2440); + } + public void basket() { + turnLeft(90); + driveEncoders(610); + // arm to score thing here + slideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); //idk sam said to put it + slideMotor.setTargetPosition(2505); // arm up to basket + slideMotor.setVelocity(1500); //idk sam said to put it + while (slideMotor.isBusy()){ + } + slideArm.setPower(0); + intakePivotMotor.setPower(0.5); + while(intakePivotMotor.isBusy()){ + } + upperServo.setPower(1.0); + lowerServo.setPower(-1.0); + sleep(3000); + intakePivotMotor.setPower(0.3); + } +} \ No newline at end of file