From 05741115b9bd56b99eb70ab53bff2880a9284436 Mon Sep 17 00:00:00 2001 From: claireg303 <79113847+claireg303@users.noreply.github.com> Date: Mon, 15 Feb 2021 15:35:27 -0500 Subject: [PATCH 1/8] Add files via upload --- TeleOpCurrent.java | 263 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 263 insertions(+) create mode 100644 TeleOpCurrent.java diff --git a/TeleOpCurrent.java b/TeleOpCurrent.java new file mode 100644 index 0000000..7d9ba09 --- /dev/null +++ b/TeleOpCurrent.java @@ -0,0 +1,263 @@ +package org.firstinspires.ftc.teamcode; + + 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; + import com.qualcomm.robotcore.hardware.DcMotorSimple; + import com.qualcomm.robotcore.util.Range; + + 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; + import com.qualcomm.robotcore.hardware.DcMotorSimple; + import com.qualcomm.robotcore.hardware.DigitalChannel; + import com.qualcomm.robotcore.util.ElapsedTime; + import com.qualcomm.robotcore.util.Range; + import com.qualcomm.robotcore.hardware.Servo; + +@TeleOp(name ="TeleOpCurrent", group = "TeleOP") +public class TeleOpCurrent extends OpMode { + DcMotor frontRight; + DcMotor frontLeft; + DcMotor backRight; + DcMotor backLeft; + DcMotor raiseArm1; + DcMotor raiseArm2; + DcMotor extendArm; + CRServo claw1; + CRServo claw2; + CRServo wrist; + boolean powerControl = false; + double powerGiven =0; + boolean clamp = false; + int powerButton; + CRServo drag1, drag2; + + double armPowerMultiplier = 0.5; + + + public void init() { + //hardware map is for phone + + // touchSense = hardwareMap.get(DigitalChannel.class, "sensor_digital"); + frontRight = hardwareMap.dcMotor.get("front right"); + frontLeft = hardwareMap.dcMotor.get("front left"); + backRight = hardwareMap.dcMotor.get("back right"); + backLeft = hardwareMap.dcMotor.get("back left"); + // raiseArm1 = hardwareMap.dcMotor.get("raise arm 1"); + // raiseArm2 = hardwareMap.dcMotor.get("raise arm 2"); + // extendArm = hardwareMap.dcMotor.get("extend arm"); + // claw1 = hardwareMap.crservo.get("claw 1"); + //claw2 = hardwareMap.crservo.get("claw 2"); + // drag1 = hardwareMap.crservo.get("drag front"); + // drag2 = hardwareMap.crservo.get("drag back"); + // wrist = hardwareMap.crservo.get("wrist"); + } + + private void setRaiseArmPower(float armPower, double multiplier){ + raiseArm1.setPower(armPower*multiplier); + raiseArm2.setPower(armPower*multiplier); + return; + } + + public void loop() { + // -----STICK VARIABLES----- + //For driving + float move = -gamepad1.left_stick_y; + float crabWalk = gamepad1.left_stick_x; + float rotation = -gamepad1.right_stick_x; + + //For arm raising + float rawRaiseValue = -gamepad2.left_stick_y; + + + + + // -----WHEEL LOGIC----- + //Wheels: Holonomic drive formula uses values of gamestick position to move + double fLeftPower = Range.clip(move + rotation + crabWalk, -1.0, 1.0); + double bLeftPower = Range.clip(move + rotation - crabWalk, -1.0, 1.0); + double fRightPower = Range.clip(move - rotation - crabWalk, -1.0, 1.0); + double bRightPower = Range.clip(move - rotation + crabWalk, -1.0, 1.0); + //Assignment of motor power in relation to wheels + frontLeft.setPower(fLeftPower/powerButton); + frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); + + backLeft.setPower(bLeftPower/powerButton); + backLeft.setDirection(DcMotorSimple.Direction.REVERSE); + + frontRight.setPower(fRightPower/powerButton); + + backRight.setPower(bRightPower/powerButton); + + //raiseArm1.setDirection(DcMotorSimple.Direction.FORWARD); + //raiseArm2.setDirection(DcMotorSimple.Direction.REVERSE); + + + + // -----GAME PAD 1----- + + // ###SPEED BOOST### + if(gamepad1.right_trigger>0.1){ + powerButton=1; + }else{ + powerButton =2; + } + + // ###DRAG SERVOS### + /* if(gamepad1.a){ + drag1.setPower(.5); + drag2.setPower(-.5); + } */ + /* else if(gamepad1.b){ + drag1.setPower(-.5); + drag2.setPower(.5); + } */ + /* else{ + drag1.setPower(0); + drag2.setPower(0); + } */ + + /* if(gamepad1.x){ + drag2.setPower(.5); + } + else if(gamepad1.y){ + drag2.setPower(-.5); + } + else{ + drag2.setPower(0); + }*/ + + + + + // -----GAME PAD 2----- + + // ###CLAMPS### + /* if (gamepad2.x){ + clamp = true; + } */ + /* if (gamepad2.y){ + claw1.setPower(1); + claw2.setPower(-1); + clamp = false; + } */ + /* else if (!clamp){ + claw1.setPower(0); + claw2.setPower(0); + } */ + /* if (clamp){ + claw1.setPower(-1); + claw2.setPower(1); + } */ + + + + //claw1: 1=open, 0=closed + //claw2: 0=open, 1=closed + + //open + /* if (gamepad2.y){ + claw1.setPosition(0.6); + claw2.setPosition(0.4); + } + //close + else if (gamepad2.x){ + claw1.setPosition(0.4); + claw2.setPosition(0.6); + } */ + + + + // ###ARM EXTENSION### + + // extendArm.setPower(-gamepad2.right_stick_y); + + // ###WRIST### + + /* if (gamepad2.right_bumper){ + wrist.setPower(0.5); + } */ + /* else if (gamepad2.left_bumper){ + wrist.setPower(-0.5); + } */ + /* else { + wrist.setPower(0); + } */ + + + // ###ARM RAISING### + + // Fast raise arm mode + /* if (gamepad2.right_trigger>0){ + //If the driver is trying to move the arm up: + if (rawRaiseValue > 0) { + setRaiseArmPower(rawRaiseValue, 0.6); + } + //If the driver is trying to move the arm down: + else if (rawRaiseValue < 0) { + setRaiseArmPower(0.1f, 0.35); + } + //If the driver is not moving the arm + else { + setRaiseArmPower(0.23f, 1); + } + }*/ + // Slow raise arm mode + /* else { + //If the driver is trying to move the arm up: + if (rawRaiseValue > 0) { + setRaiseArmPower(rawRaiseValue, 0.35); + } + //If the driver is trying to move the arm down: + else if (rawRaiseValue < 0) { + setRaiseArmPower(0f, 1); + } + //If the driver is not moving the arm + else { + setRaiseArmPower(0.23f, 1); + } + }*/ + /* + // Fast raise arm mode + if (gamepad2.right_trigger>0){ + //If the driver is trying to move the arm up: + if (rawRaiseValue > 0) { + setRaiseArmPower(rawRaiseValue, 0.6); + } + //If the driver is trying to move the arm down: + else if (rawRaiseValue < 0) { + setRaiseArmPower(0.1f, 0.35); + } + //If the driver is not moving the arm + /*else { + setRaiseArmPower(0.23f, 1); + } + } + // Slow raise arm mode + /* else { + //If the driver is trying to move the arm up: + if (rawRaiseValue > 0) { + setRaiseArmPower(rawRaiseValue, 0.35); + } + //If the driver is trying to move the arm down: + else if (rawRaiseValue < 0) { + setRaiseArmPower(0f, 1); + } + //If the driver is not moving the arm + /* else { + setRaiseArmPower(0.23f, 1); + } + }*/ + + + + + } + + + + +} \ No newline at end of file From 6e3c49bbfea170c624237bb765a8fb498f02841e Mon Sep 17 00:00:00 2001 From: claireg303 <79113847+claireg303@users.noreply.github.com> Date: Sun, 21 Feb 2021 17:12:39 -0500 Subject: [PATCH 2/8] Add files via upload Autonomous code from meeting 2/21/21 --- AutonomousTest.java | 104 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 104 insertions(+) create mode 100644 AutonomousTest.java diff --git a/AutonomousTest.java b/AutonomousTest.java new file mode 100644 index 0000000..2091229 --- /dev/null +++ b/AutonomousTest.java @@ -0,0 +1,104 @@ +package org.firstinspires.ftc.teamcode; +//THIS IS FOR THE BLUE SIDE +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.ColorSensor; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import com.qualcomm.robotcore.hardware.DigitalChannel; +import java.util.ArrayList; +import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator; +import com.qualcomm.robotcore.hardware.DistanceSensor; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/* +Parallel Lions 9681 Autonomous Blue Code +author: 9681 Software +GOALS: Place the wobble goal in the zone and put rings in lowest goal +DESCRIPTION: This code is used for our autonomous when we are located on the blue side + */ +@Autonomous(name="FULL AUTO BLUTRAY", group="Iterative Opmode") +public class AutonomousTest extends OpMode +{ + + + /* + ---MOTORS--- + */ + DcMotor leftFront; + DcMotor rightFront; + DcMotor leftBack; + DcMotor rightBack; + + ArrayList motors = new ArrayList(); + + private StateMachine machine; + + driveState strafeLeft1; + + + public void init() { + + /* + ---HARDWARE MAP--- + */ + rightFront = hardwareMap.dcMotor.get("right front"); + leftFront = hardwareMap.dcMotor.get("left front"); + rightBack = hardwareMap.dcMotor.get("right back"); + leftBack = hardwareMap.dcMotor.get("left back"); + + /* + ---MOTOR DIRECTIONS--- + */ + rightBack.setDirection(DcMotor.Direction.REVERSE); + rightFront.setDirection(DcMotor.Direction.REVERSE); + + /* + ---GROUPING--- + */ + motors.add(rightFront); + motors.add(leftFront); + motors.add(rightBack); + motors.add(leftBack); + + + /* + ---USING STATES--- + */ + + strafeLeft1 = new driveState(70, 1.0, motors, "left"); //first move left + //drive forward a little, then turn left 90 degrees, raise and extend arm. dispose rings, move backwards + + strafeLeft1.setNextState(null); + + + } + + + @Override + public void start(){ + + machine = new StateMachine(moveState); + + } + + + + public void loop() { + + machine.update(); + + } + + public void wait(int time) { + try { + Thread.sleep(time * 1000);//milliseconds + } catch (InterruptedException e) { + e.printStackTrace(); + } + } +} \ No newline at end of file From ddf4ce145a6f5de6e794735652fb39614f108793 Mon Sep 17 00:00:00 2001 From: claireg303 <79113847+claireg303@users.noreply.github.com> Date: Sun, 7 Mar 2021 17:41:08 -0500 Subject: [PATCH 3/8] Create TeleOpCurrentNew.java --- TeleOpCurrentNew.java | 263 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 263 insertions(+) create mode 100644 TeleOpCurrentNew.java diff --git a/TeleOpCurrentNew.java b/TeleOpCurrentNew.java new file mode 100644 index 0000000..2cf22b5 --- /dev/null +++ b/TeleOpCurrentNew.java @@ -0,0 +1,263 @@ +package org.firstinspires.ftc.teamcode; + + 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; + import com.qualcomm.robotcore.hardware.DcMotorSimple; + import com.qualcomm.robotcore.util.Range; + + 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; + import com.qualcomm.robotcore.hardware.DcMotorSimple; + import com.qualcomm.robotcore.hardware.DigitalChannel; + import com.qualcomm.robotcore.util.ElapsedTime; + import com.qualcomm.robotcore.util.Range; + import com.qualcomm.robotcore.hardware.Servo; + +@TeleOp(name ="TeleOpCurrentNew", group = "TeleOP") +public class TeleOpCurrentNew extends OpMode { + DcMotor frontRight; + DcMotor frontLeft; + DcMotor backRight; + DcMotor backLeft; + DcMotor raiseArm1; + DcMotor raiseArm2; + DcMotor extendArm; + CRServo claw1; + CRServo claw2; + CRServo wrist; + boolean powerControl = false; + double powerGiven =0; + boolean clamp = false; + int powerButton; + CRServo drag1, drag2; + + double armPowerMultiplier = 0.5; + + + public void init() { + //hardware map is for phone + + // touchSense = hardwareMap.get(DigitalChannel.class, "sensor_digital"); + frontRight = hardwareMap.dcMotor.get("front right"); + frontLeft = hardwareMap.dcMotor.get("front left"); + backRight = hardwareMap.dcMotor.get("back right"); + backLeft = hardwareMap.dcMotor.get("back left"); + // raiseArm1 = hardwareMap.dcMotor.get("raise arm 1"); + // raiseArm2 = hardwareMap.dcMotor.get("raise arm 2"); + // extendArm = hardwareMap.dcMotor.get("extend arm"); + // claw1 = hardwareMap.crservo.get("claw 1"); + //claw2 = hardwareMap.crservo.get("claw 2"); + // drag1 = hardwareMap.crservo.get("drag front"); + // drag2 = hardwareMap.crservo.get("drag back"); + // wrist = hardwareMap.crservo.get("wrist"); + } + + private void setRaiseArmPower(float armPower, double multiplier){ + raiseArm1.setPower(armPower*multiplier); + raiseArm2.setPower(armPower*multiplier); + return; + } + + public void loop() { + // -----STICK VARIABLES----- + //For driving + float move = -gamepad1.left_stick_y; + float crabWalk = gamepad1.left_stick_x; + float rotation = -gamepad1.right_stick_x; + + //For arm raising + float rawRaiseValue = -gamepad2.left_stick_y; + + + + + // -----WHEEL LOGIC----- + //Wheels: Holonomic drive formula uses values of gamestick position to move + double fLeftPower = Range.clip(move + rotation + crabWalk, -1.0, 1.0); + double bLeftPower = Range.clip(move + rotation - crabWalk, -1.0, 1.0); + double fRightPower = Range.clip(move - rotation - crabWalk, -1.0, 1.0); + double bRightPower = Range.clip(move - rotation + crabWalk, -1.0, 1.0); + //Assignment of motor power in relation to wheels + frontLeft.setPower(fLeftPower/powerButton); + frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); + + backLeft.setPower(bLeftPower/powerButton); + backLeft.setDirection(DcMotorSimple.Direction.REVERSE); + + frontRight.setPower(fRightPower/powerButton); + + backRight.setPower(bRightPower/powerButton); + + //raiseArm1.setDirection(DcMotorSimple.Direction.FORWARD); + //raiseArm2.setDirection(DcMotorSimple.Direction.REVERSE); + + + + // -----GAME PAD 1----- + + // ###SPEED BOOST### + if(gamepad1.right_trigger>0.1){ + powerButton=1; + }else{ + powerButton =2; + } + + // ###DRAG SERVOS### + /* if(gamepad1.a){ + drag1.setPower(.5); + drag2.setPower(-.5); + } */ + /* else if(gamepad1.b){ + drag1.setPower(-.5); + drag2.setPower(.5); + } */ + /* else{ + drag1.setPower(0); + drag2.setPower(0); + } */ + + /* if(gamepad1.x){ + drag2.setPower(.5); + } + else if(gamepad1.y){ + drag2.setPower(-.5); + } + else{ + drag2.setPower(0); + }*/ + + + + + // -----GAME PAD 2----- + + // ###CLAMPS### + /* if (gamepad2.x){ + clamp = true; + } */ + /* if (gamepad2.y){ + claw1.setPower(1); + claw2.setPower(-1); + clamp = false; + } */ + /* else if (!clamp){ + claw1.setPower(0); + claw2.setPower(0); + } */ + /* if (clamp){ + claw1.setPower(-1); + claw2.setPower(1); + } */ + + + + //claw1: 1=open, 0=closed + //claw2: 0=open, 1=closed + + //open + /* if (gamepad2.y){ + claw1.setPosition(0.6); + claw2.setPosition(0.4); + } + //close + else if (gamepad2.x){ + claw1.setPosition(0.4); + claw2.setPosition(0.6); + } */ + + + + // ###ARM EXTENSION### + + // extendArm.setPower(-gamepad2.right_stick_y); + + // ###WRIST### + + /* if (gamepad2.right_bumper){ + wrist.setPower(0.5); + } */ + /* else if (gamepad2.left_bumper){ + wrist.setPower(-0.5); + } */ + /* else { + wrist.setPower(0); + } */ + + + // ###ARM RAISING### + + // Fast raise arm mode + /* if (gamepad2.right_trigger>0){ + //If the driver is trying to move the arm up: + if (rawRaiseValue > 0) { + setRaiseArmPower(rawRaiseValue, 0.6); + } + //If the driver is trying to move the arm down: + else if (rawRaiseValue < 0) { + setRaiseArmPower(0.1f, 0.35); + } + //If the driver is not moving the arm + else { + setRaiseArmPower(0.23f, 1); + } + }*/ + // Slow raise arm mode + /* else { + //If the driver is trying to move the arm up: + if (rawRaiseValue > 0) { + setRaiseArmPower(rawRaiseValue, 0.35); + } + //If the driver is trying to move the arm down: + else if (rawRaiseValue < 0) { + setRaiseArmPower(0f, 1); + } + //If the driver is not moving the arm + else { + setRaiseArmPower(0.23f, 1); + } + }*/ + /* + // Fast raise arm mode + if (gamepad2.right_trigger>0){ + //If the driver is trying to move the arm up: + if (rawRaiseValue > 0) { + setRaiseArmPower(rawRaiseValue, 0.6); + } + //If the driver is trying to move the arm down: + else if (rawRaiseValue < 0) { + setRaiseArmPower(0.1f, 0.35); + } + //If the driver is not moving the arm + /*else { + setRaiseArmPower(0.23f, 1); + } + } + // Slow raise arm mode + /* else { + //If the driver is trying to move the arm up: + if (rawRaiseValue > 0) { + setRaiseArmPower(rawRaiseValue, 0.35); + } + //If the driver is trying to move the arm down: + else if (rawRaiseValue < 0) { + setRaiseArmPower(0f, 1); + } + //If the driver is not moving the arm + /* else { + setRaiseArmPower(0.23f, 1); + } + }*/ + + + + + } + + + + +} From 837c10cc302408e1e9b0630e849d381377e9d22a Mon Sep 17 00:00:00 2001 From: claireg303 <79113847+claireg303@users.noreply.github.com> Date: Sun, 7 Mar 2021 17:42:59 -0500 Subject: [PATCH 4/8] Update TeleOpCurrentNew.java --- TeleOpCurrentNew.java | 68 +++++++++++++++++++++---------------------- 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/TeleOpCurrentNew.java b/TeleOpCurrentNew.java index 2cf22b5..dc41933 100644 --- a/TeleOpCurrentNew.java +++ b/TeleOpCurrentNew.java @@ -46,14 +46,14 @@ public void init() { frontLeft = hardwareMap.dcMotor.get("front left"); backRight = hardwareMap.dcMotor.get("back right"); backLeft = hardwareMap.dcMotor.get("back left"); - // raiseArm1 = hardwareMap.dcMotor.get("raise arm 1"); - // raiseArm2 = hardwareMap.dcMotor.get("raise arm 2"); - // extendArm = hardwareMap.dcMotor.get("extend arm"); - // claw1 = hardwareMap.crservo.get("claw 1"); - //claw2 = hardwareMap.crservo.get("claw 2"); + raiseArm1 = hardwareMap.dcMotor.get("raise arm 1"); + raiseArm2 = hardwareMap.dcMotor.get("raise arm 2"); + extendArm = hardwareMap.dcMotor.get("extend arm"); + claw1 = hardwareMap.crservo.get("claw 1"); + claw2 = hardwareMap.crservo.get("claw 2"); // drag1 = hardwareMap.crservo.get("drag front"); // drag2 = hardwareMap.crservo.get("drag back"); - // wrist = hardwareMap.crservo.get("wrist"); + wrist = hardwareMap.crservo.get("wrist"); } private void setRaiseArmPower(float armPower, double multiplier){ @@ -92,8 +92,8 @@ public void loop() { backRight.setPower(bRightPower/powerButton); - //raiseArm1.setDirection(DcMotorSimple.Direction.FORWARD); - //raiseArm2.setDirection(DcMotorSimple.Direction.REVERSE); + raiseArm1.setDirection(DcMotorSimple.Direction.FORWARD); + raiseArm2.setDirection(DcMotorSimple.Direction.REVERSE); @@ -136,22 +136,22 @@ else if(gamepad1.y){ // -----GAME PAD 2----- // ###CLAMPS### - /* if (gamepad2.x){ + if (gamepad2.x){ clamp = true; - } */ - /* if (gamepad2.y){ + } + if (gamepad2.y){ claw1.setPower(1); claw2.setPower(-1); clamp = false; - } */ - /* else if (!clamp){ + } + else if (!clamp){ claw1.setPower(0); claw2.setPower(0); - } */ - /* if (clamp){ + } + if (clamp){ claw1.setPower(-1); claw2.setPower(1); - } */ + } @@ -159,7 +159,7 @@ else if(gamepad1.y){ //claw2: 0=open, 1=closed //open - /* if (gamepad2.y){ + if (gamepad2.y){ claw1.setPosition(0.6); claw2.setPosition(0.4); } @@ -167,31 +167,31 @@ else if(gamepad1.y){ else if (gamepad2.x){ claw1.setPosition(0.4); claw2.setPosition(0.6); - } */ + } // ###ARM EXTENSION### - // extendArm.setPower(-gamepad2.right_stick_y); + extendArm.setPower(-gamepad2.right_stick_y); // ###WRIST### - /* if (gamepad2.right_bumper){ + if (gamepad2.right_bumper){ wrist.setPower(0.5); - } */ - /* else if (gamepad2.left_bumper){ + } + else if (gamepad2.left_bumper){ wrist.setPower(-0.5); - } */ - /* else { + } + else { wrist.setPower(0); - } */ + } // ###ARM RAISING### // Fast raise arm mode - /* if (gamepad2.right_trigger>0){ + if (gamepad2.right_trigger>0){ //If the driver is trying to move the arm up: if (rawRaiseValue > 0) { setRaiseArmPower(rawRaiseValue, 0.6); @@ -204,9 +204,9 @@ else if (rawRaiseValue < 0) { else { setRaiseArmPower(0.23f, 1); } - }*/ + } // Slow raise arm mode - /* else { + else { //If the driver is trying to move the arm up: if (rawRaiseValue > 0) { setRaiseArmPower(rawRaiseValue, 0.35); @@ -219,8 +219,8 @@ else if (rawRaiseValue < 0) { else { setRaiseArmPower(0.23f, 1); } - }*/ - /* + } + // Fast raise arm mode if (gamepad2.right_trigger>0){ //If the driver is trying to move the arm up: @@ -232,12 +232,12 @@ else if (rawRaiseValue < 0) { setRaiseArmPower(0.1f, 0.35); } //If the driver is not moving the arm - /*else { + { setRaiseArmPower(0.23f, 1); } } // Slow raise arm mode - /* else { + else { //If the driver is trying to move the arm up: if (rawRaiseValue > 0) { setRaiseArmPower(rawRaiseValue, 0.35); @@ -247,10 +247,10 @@ else if (rawRaiseValue < 0) { setRaiseArmPower(0f, 1); } //If the driver is not moving the arm - /* else { + else { setRaiseArmPower(0.23f, 1); } - }*/ + } From 70c2cbc2c2f07fe7706e17eb034dfe176f01b5d0 Mon Sep 17 00:00:00 2001 From: claireg303 <79113847+claireg303@users.noreply.github.com> Date: Sun, 7 Mar 2021 17:44:14 -0500 Subject: [PATCH 5/8] Update TeleOpCurrentNew.java NEW CODE WITH ARM AND WRIST FOR TELEOP --- TeleOpCurrentNew.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TeleOpCurrentNew.java b/TeleOpCurrentNew.java index dc41933..6eae58c 100644 --- a/TeleOpCurrentNew.java +++ b/TeleOpCurrentNew.java @@ -1,3 +1,5 @@ +// THIS IS THE NEW CODE CLAIRE MADE ON SUNDAY 3/7/21 THAT INCLUDES THE TWO MOTORS FOR THE ARM AND THE 3 SERVOS FOR THE WRIST + package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.eventloop.opmode.OpMode; From 7099d557d4224b2fb5e4beef2bae94384649b330 Mon Sep 17 00:00:00 2001 From: claireg303 <79113847+claireg303@users.noreply.github.com> Date: Mon, 8 Mar 2021 20:26:45 -0500 Subject: [PATCH 6/8] Create TeleOpCurrentNew2.java --- TeleOpCurrentNew2.java | 147 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 147 insertions(+) create mode 100644 TeleOpCurrentNew2.java diff --git a/TeleOpCurrentNew2.java b/TeleOpCurrentNew2.java new file mode 100644 index 0000000..7b01db8 --- /dev/null +++ b/TeleOpCurrentNew2.java @@ -0,0 +1,147 @@ +package org.firstinspires.ftc.teamcode; +//THIS IS FOR THE BLUE SIDE +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.ColorSensor; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import com.qualcomm.robotcore.hardware.DigitalChannel; +import java.util.ArrayList; +import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator; +import com.qualcomm.robotcore.hardware.DistanceSensor; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/* +Parallel Lions 9681 Autonomous Blue Code +author: 9681 Software +GOALS: Place the wobble goal in the zone and put rings in lowest goal +DESCRIPTION: This code is used for our autonomous when we are located on the blue side + */ +@Autonomous(name="AutoTest", group="Iterative Opmode") +public class AutonomousTest extends OpMode +{ + + + /* + ---MOTORS--- + */ + DcMotor leftFront; + DcMotor rightFront; + DcMotor leftBack; + DcMotor rightBack; //make sure these are the right motors + DcMotor extendArm; + CRServo claw1, claw2; + DcMotor raiseArm2; + DcMotor raiseArmMotor; + + + ArrayList motors = new ArrayList(); + ArrayList servos = new ArrayList(); + + private StateMachine machine; + + driveState strafeLeft; + driveState forward1; + driveState turnLeft; + extendArmState raiseArm1; + extendArmState extendFirst; + + CRServoState open1; + driveState moveBackwards1; + + + + public void init() { + + /* + ---HARDWARE MAP--- + */ + rightFront=hardwareMap.dcMotor.get("right front"); + leftFront = hardwareMap.dcMotor.get("left front"); + rightBack = hardwareMap.dcMotor.get("right back"); + leftBack = hardwareMap.dcMotor.get("left back"); + raiseArmMotor = hardwareMap.dcMotor.get("raise arm"); + extendArm = hardwareMap.dcMotor.get("extend arm"); + + claw1 = hardwareMap.crservo.get("claw 1"); + claw2 = hardwareMap.crservo.get("claw 2"); + //get the CRSERVO + + + /* + ---MOTOR DIRECTIONS--- + */ + rightBack.setDirection(DcMotor.Direction.REVERSE); + rightFront.setDirection(DcMotor.Direction.REVERSE); +//could be wrong!!!! test + /* + ---GROUPING--- + */ + motors.add(rightFront); + motors.add(leftFront); + motors.add(rightBack); + motors.add(leftBack); + // CRServos.add(claw1); + // CRServos.add(claw2); + //extend, CRServos + //maybe add an extra time?????? it was in colorstone + /* + ---USING STATES--- + */ + + strafeLeft = new driveState(70, 1.0, motors, "left"); //first move left + //drive forward a little, then turn left 90 degrees, raise and extend arm. dispose rings, move backwards + forward1 = new driveState(20, 1.0, motors, "forward"); + turnLeft = new driveState(10, 1.0, motors, "turnLeft"); + raiseArm1 = new extendArmState(500, -0.5, raiseArmMotor); //figure this out + extendFirst = new extendArmState(400, 1.0, extendArm); + open1 = new CRServoState(2000, 1.0, 1.0, servos);//ask abby if there are two servos + moveBackwards1 = new driveState(5, 1.0, motors, "backward"); + //open the claws + //back up + + + + strafeLeft.setNextState(forward1); + forward1.setNextState(turnLeft); + turnLeft.setNextState(raiseArm1); + raiseArm1.setNextState(extendFirst); + extendFirst.setNextState(open1); + open1.setNextState(moveBackwards1); + moveBackwards1.setNextState(null); + + + + + + } + + + @Override + public void start(){ + + machine = new StateMachine(strafeLeft); + + } + + + + public void loop() { + + machine.update(); + + } + + public void wait(int time) { + try { + Thread.sleep(time * 1000);//milliseconds + } catch (InterruptedException e) { + e.printStackTrace(); + } + } +} From 91e93d37f708a74c9aa3f68c0523f3e5f3df0709 Mon Sep 17 00:00:00 2001 From: claireg303 <79113847+claireg303@users.noreply.github.com> Date: Mon, 8 Mar 2021 20:27:19 -0500 Subject: [PATCH 7/8] Update TeleOpCurrentNew2.java --- TeleOpCurrentNew2.java | 331 +++++++++++++++++++++++++++-------------- 1 file changed, 218 insertions(+), 113 deletions(-) diff --git a/TeleOpCurrentNew2.java b/TeleOpCurrentNew2.java index 7b01db8..5cf36c1 100644 --- a/TeleOpCurrentNew2.java +++ b/TeleOpCurrentNew2.java @@ -1,147 +1,252 @@ +// THIS IS THE NEW CODE CLAIRE MADE ON SUNDAY 3/7/21 THAT INCLUDES THE TWO MOTORS FOR THE ARM AND THE 3 SERVOS FOR THE WRIST + package org.firstinspires.ftc.teamcode; -//THIS IS FOR THE BLUE SIDE -import com.qualcomm.hardware.bosch.BNO055IMU; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.hardware.ColorSensor; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.Range; + +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.Servo; -import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.DigitalChannel; -import java.util.ArrayList; -import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator; -import com.qualcomm.robotcore.hardware.DistanceSensor; - -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; - -/* -Parallel Lions 9681 Autonomous Blue Code -author: 9681 Software -GOALS: Place the wobble goal in the zone and put rings in lowest goal -DESCRIPTION: This code is used for our autonomous when we are located on the blue side - */ -@Autonomous(name="AutoTest", group="Iterative Opmode") -public class AutonomousTest extends OpMode -{ - - - /* - ---MOTORS--- - */ - DcMotor leftFront; - DcMotor rightFront; - DcMotor leftBack; - DcMotor rightBack; //make sure these are the right motors - DcMotor extendArm; - CRServo claw1, claw2; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; +import com.qualcomm.robotcore.hardware.Servo; + +@TeleOp(name ="TeleOpCurrentNew", group = "TeleOP") +public class TeleOpCurrentNew extends OpMode { + DcMotor frontRight; + DcMotor frontLeft; + DcMotor backRight; + DcMotor backLeft; + DcMotor raiseArm1; DcMotor raiseArm2; - DcMotor raiseArmMotor; + DcMotor extendArm; + CRServo claw1; + CRServo claw2; + CRServo wrist; + boolean powerControl = false; + double powerGiven =0; + boolean clamp = false; + int powerButton; + CRServo drag1, drag2; + double armPowerMultiplier = 0.5; - ArrayList motors = new ArrayList(); - ArrayList servos = new ArrayList(); - private StateMachine machine; + public void init() { + //hardware map is for phone + + // touchSense = hardwareMap.get(DigitalChannel.class, "sensor_digital"); + frontRight = hardwareMap.dcMotor.get("front right"); + frontLeft = hardwareMap.dcMotor.get("front left"); + backRight = hardwareMap.dcMotor.get("back right"); + backLeft = hardwareMap.dcMotor.get("back left"); + raiseArm1 = hardwareMap.dcMotor.get("raise arm"); + //raiseArm2 = hardwareMap.dcMotor.get("raise arm 2"); + extendArm = hardwareMap.dcMotor.get("extend arm"); + claw1 = hardwareMap.crservo.get("claw 1"); + claw2 = hardwareMap.crservo.get("claw 2"); + // drag1 = hardwareMap.crservo.get("drag front"); + // drag2 = hardwareMap.crservo.get("drag back"); + wrist = hardwareMap.crservo.get("wrist"); + } - driveState strafeLeft; - driveState forward1; - driveState turnLeft; - extendArmState raiseArm1; - extendArmState extendFirst; + private void setRaiseArmPower(double armPower, double multiplier){ + raiseArm1.setPower(armPower*multiplier); + // raiseArm2.setPower(armPower*multiplier); + return; + } - CRServoState open1; - driveState moveBackwards1; + public void loop() { + // -----STICK VARIABLES----- + //For driving + float move = -gamepad1.left_stick_y; + float crabWalk = gamepad1.left_stick_x; + float rotation = -gamepad1.right_stick_x; + //For arm raising + float rawRaiseValue = -gamepad2.left_stick_y; - public void init() { - /* - ---HARDWARE MAP--- - */ - rightFront=hardwareMap.dcMotor.get("right front"); - leftFront = hardwareMap.dcMotor.get("left front"); - rightBack = hardwareMap.dcMotor.get("right back"); - leftBack = hardwareMap.dcMotor.get("left back"); - raiseArmMotor = hardwareMap.dcMotor.get("raise arm"); - extendArm = hardwareMap.dcMotor.get("extend arm"); - claw1 = hardwareMap.crservo.get("claw 1"); - claw2 = hardwareMap.crservo.get("claw 2"); - //get the CRSERVO - - - /* - ---MOTOR DIRECTIONS--- - */ - rightBack.setDirection(DcMotor.Direction.REVERSE); - rightFront.setDirection(DcMotor.Direction.REVERSE); -//could be wrong!!!! test - /* - ---GROUPING--- - */ - motors.add(rightFront); - motors.add(leftFront); - motors.add(rightBack); - motors.add(leftBack); - // CRServos.add(claw1); - // CRServos.add(claw2); - //extend, CRServos - //maybe add an extra time?????? it was in colorstone - /* - ---USING STATES--- - */ - - strafeLeft = new driveState(70, 1.0, motors, "left"); //first move left - //drive forward a little, then turn left 90 degrees, raise and extend arm. dispose rings, move backwards - forward1 = new driveState(20, 1.0, motors, "forward"); - turnLeft = new driveState(10, 1.0, motors, "turnLeft"); - raiseArm1 = new extendArmState(500, -0.5, raiseArmMotor); //figure this out - extendFirst = new extendArmState(400, 1.0, extendArm); - open1 = new CRServoState(2000, 1.0, 1.0, servos);//ask abby if there are two servos - moveBackwards1 = new driveState(5, 1.0, motors, "backward"); - //open the claws - //back up - - - - strafeLeft.setNextState(forward1); - forward1.setNextState(turnLeft); - turnLeft.setNextState(raiseArm1); - raiseArm1.setNextState(extendFirst); - extendFirst.setNextState(open1); - open1.setNextState(moveBackwards1); - moveBackwards1.setNextState(null); + // -----WHEEL LOGIC----- + //Wheels: Holonomic drive formula uses values of gamestick position to move + double fLeftPower = Range.clip(move + rotation + crabWalk, -1.0, 1.0); + double bLeftPower = Range.clip(move + rotation - crabWalk, -1.0, 1.0); + double fRightPower = Range.clip(move - rotation - crabWalk, -1.0, 1.0); + double bRightPower = Range.clip(move - rotation + crabWalk, -1.0, 1.0); + //Assignment of motor power in relation to wheels + frontLeft.setPower(fLeftPower/powerButton); + frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); + backLeft.setPower(bLeftPower/powerButton); + backLeft.setDirection(DcMotorSimple.Direction.REVERSE); + frontRight.setPower(fRightPower/powerButton); + backRight.setPower(bRightPower/powerButton); + raiseArm1.setDirection(DcMotorSimple.Direction.FORWARD); + // + // + // + // + // + // raiseArm2.setDirection(DcMotorSimple.Direction.REVERSE); - } - @Override - public void start(){ + // -----GAME PAD 1----- - machine = new StateMachine(strafeLeft); + // ###SPEED BOOST### + if(gamepad1.right_trigger>0.1){ + powerButton=1; + }else{ + powerButton =2; + } - } + // ###DRAG SERVOS### + /* if(gamepad1.a){ + drag1.setPower(.5); + drag2.setPower(-.5); + } */ + /* else if(gamepad1.b){ + drag1.setPower(-.5); + drag2.setPower(.5); + } */ + /* else{ + drag1.setPower(0); + drag2.setPower(0); + } */ + + /* if(gamepad1.x){ + drag2.setPower(.5); + } + else if(gamepad1.y){ + drag2.setPower(-.5); + } + else{ + drag2.setPower(0); + }*/ - public void loop() { - machine.update(); + // -----GAME PAD 2----- - } + // ###CLAMPS### + if (gamepad2.x){ + clamp = true; + } + if (gamepad2.y){ + claw1.setPower(1); + claw2.setPower(-1); + clamp = false; + } + else if (!clamp){ + claw1.setPower(0); + claw2.setPower(0); + } + if (clamp){ + claw1.setPower(-1); + claw2.setPower(1); + } - public void wait(int time) { - try { - Thread.sleep(time * 1000);//milliseconds - } catch (InterruptedException e) { - e.printStackTrace(); + + + //claw1: 1=open, 0=closed + //claw2: 0=open, 1=closed + + //open + if (gamepad2.y){ + claw1.setPower(0.6); + claw2.setPower(0.4); + } + //close + else if (gamepad2.x){ + claw1.setPower(0.4); + claw2.setPower(0.6); + } + + + + // ###ARM EXTENSION### + + extendArm.setPower(-gamepad2.right_stick_y); + + // ###WRIST### + + if (gamepad2.right_bumper){ + wrist.setPower(0.5); + } + else if (gamepad2.left_bumper){ + wrist.setPower(-0.5); + } + else { + wrist.setPower(0); + } + + + // ###ARM RAISING### + + // Fast raise arm mode + if (gamepad2.right_trigger>0){ + //If the driver is trying to move the arm up: + setRaiseArmPower(rawRaiseValue, 0.6); + } + // Slow raise arm mode + + //setRaiseArmPower(rawRaiseValue, 0.35); + else { + //If the driver is trying to move the arm up: + setRaiseArmPower(rawRaiseValue, 0.35); } + + // Fast raise arm mode + if (gamepad2.right_trigger>0){ + //If the driver is trying to move the arm up: + if (rawRaiseValue > 0) { + setRaiseArmPower(rawRaiseValue, 0.6); + } + //If the driver is trying to move the arm down: + else if (rawRaiseValue < 0) { + setRaiseArmPower(0.1, 0.35); + } + //If the driver is not moving the arm + else{ + setRaiseArmPower(0.0, 1); + } + } + // Slow raise arm mode + else { + //If the driver is trying to move the arm up: + if (rawRaiseValue > 0) { + setRaiseArmPower(rawRaiseValue, 0.35); + } + //If the driver is trying to move the arm down: + else if (rawRaiseValue < 0) { + setRaiseArmPower(0, 1); + } + //If the driver is not moving the arm + else { + setRaiseArmPower(0.0, 1); + } + } + + + + } + + + + } From bd9469d7ba9a454a9c895468de88cacae7f05612 Mon Sep 17 00:00:00 2001 From: claireg303 <79113847+claireg303@users.noreply.github.com> Date: Mon, 8 Mar 2021 23:17:04 -0500 Subject: [PATCH 8/8] Update AutonomousTest.java --- AutonomousTest.java | 69 ++++++++++++++++++++++++++++++++++++--------- 1 file changed, 56 insertions(+), 13 deletions(-) diff --git a/AutonomousTest.java b/AutonomousTest.java index 2091229..7b01db8 100644 --- a/AutonomousTest.java +++ b/AutonomousTest.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; @@ -21,7 +22,7 @@ GOALS: Place the wobble goal in the zone and put rings in lowest goal DESCRIPTION: This code is used for our autonomous when we are located on the blue side */ -@Autonomous(name="FULL AUTO BLUTRAY", group="Iterative Opmode") +@Autonomous(name="AutoTest", group="Iterative Opmode") public class AutonomousTest extends OpMode { @@ -32,13 +33,27 @@ public class AutonomousTest extends OpMode DcMotor leftFront; DcMotor rightFront; DcMotor leftBack; - DcMotor rightBack; + DcMotor rightBack; //make sure these are the right motors + DcMotor extendArm; + CRServo claw1, claw2; + DcMotor raiseArm2; + DcMotor raiseArmMotor; + ArrayList motors = new ArrayList(); + ArrayList servos = new ArrayList(); private StateMachine machine; - driveState strafeLeft1; + driveState strafeLeft; + driveState forward1; + driveState turnLeft; + extendArmState raiseArm1; + extendArmState extendFirst; + + CRServoState open1; + driveState moveBackwards1; + public void init() { @@ -46,17 +61,24 @@ public void init() { /* ---HARDWARE MAP--- */ - rightFront = hardwareMap.dcMotor.get("right front"); + rightFront=hardwareMap.dcMotor.get("right front"); leftFront = hardwareMap.dcMotor.get("left front"); rightBack = hardwareMap.dcMotor.get("right back"); leftBack = hardwareMap.dcMotor.get("left back"); - + raiseArmMotor = hardwareMap.dcMotor.get("raise arm"); + extendArm = hardwareMap.dcMotor.get("extend arm"); + + claw1 = hardwareMap.crservo.get("claw 1"); + claw2 = hardwareMap.crservo.get("claw 2"); + //get the CRSERVO + + /* ---MOTOR DIRECTIONS--- */ rightBack.setDirection(DcMotor.Direction.REVERSE); rightFront.setDirection(DcMotor.Direction.REVERSE); - +//could be wrong!!!! test /* ---GROUPING--- */ @@ -64,16 +86,37 @@ public void init() { motors.add(leftFront); motors.add(rightBack); motors.add(leftBack); - - + // CRServos.add(claw1); + // CRServos.add(claw2); + //extend, CRServos + //maybe add an extra time?????? it was in colorstone /* ---USING STATES--- */ - strafeLeft1 = new driveState(70, 1.0, motors, "left"); //first move left + strafeLeft = new driveState(70, 1.0, motors, "left"); //first move left //drive forward a little, then turn left 90 degrees, raise and extend arm. dispose rings, move backwards + forward1 = new driveState(20, 1.0, motors, "forward"); + turnLeft = new driveState(10, 1.0, motors, "turnLeft"); + raiseArm1 = new extendArmState(500, -0.5, raiseArmMotor); //figure this out + extendFirst = new extendArmState(400, 1.0, extendArm); + open1 = new CRServoState(2000, 1.0, 1.0, servos);//ask abby if there are two servos + moveBackwards1 = new driveState(5, 1.0, motors, "backward"); + //open the claws + //back up + + + + strafeLeft.setNextState(forward1); + forward1.setNextState(turnLeft); + turnLeft.setNextState(raiseArm1); + raiseArm1.setNextState(extendFirst); + extendFirst.setNextState(open1); + open1.setNextState(moveBackwards1); + moveBackwards1.setNextState(null); + + - strafeLeft1.setNextState(null); } @@ -81,8 +124,8 @@ public void init() { @Override public void start(){ - - machine = new StateMachine(moveState); + + machine = new StateMachine(strafeLeft); } @@ -101,4 +144,4 @@ public void wait(int time) { e.printStackTrace(); } } -} \ No newline at end of file +}