diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 301ff649463..87e39acb8a0 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -7,9 +7,9 @@ // the build definitions, you can place those customizations in this file, but // please think carefully as to whether such customizations are really necessary // before doing so. - - // Custom definitions may go here - // Include common definitions from above. apply from: '../build.common.gradle' +dependencies { + +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoStillPhone.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoStillPhone.java new file mode 100644 index 00000000000..986f8b3ee46 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoStillPhone.java @@ -0,0 +1,83 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +@Autonomous(name="AutoStillPhone", group="AutoStillPhone") + +public class AutoStillPhone extends LinearOpMode { + + /* Declare OpMode members. */ + private DcMotor Left; + private DcMotor Right; + private ElapsedTime runtime = new ElapsedTime(); + + + static final double FORWARD_SPEED = 0.6; + static final double TURN_SPEED = 0.5; + + public void runOpMode() { + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + + //@Override + //public void runOpMode() { + + /* + * Initialize the drive system variables. + * The init() method of the hardware class does all the work here + */ + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Ready to run"); // + telemetry.update(); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + // Step through each leg of the path, ensuring that the Auto mode has not been stopped along the way + + // Step 1: Drive forward for 7 seconds + Right.setPower(-FORWARD_SPEED); + Left.setPower(FORWARD_SPEED); + runtime.reset(); + while (opModeIsActive() && (runtime.seconds() < 2)) { + telemetry.addData("Path", "Leg 1: %2.5f S Elapsed", runtime.seconds()); + telemetry.update(); + } + + + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTurn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTurn.java new file mode 100644 index 00000000000..cf9949f2a03 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTurn.java @@ -0,0 +1,99 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +@Autonomous(name="AutoTurn", group="AutoTurn") + +public class AutoTurn extends LinearOpMode { + + /* Declare OpMode members. */ + private DcMotor Left; + private DcMotor Right; + private ElapsedTime runtime = new ElapsedTime(); + + + static final double FORWARD_SPEED = 0.6; + static final double TURN_SPEED = 0.5; + + public void runOpMode() { + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + + //@Override + //public void runOpMode() { + + /* + * Initialize the drive system variables. + * The init() method of the hardware class does all the work here + */ + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Ready to run"); // + telemetry.update(); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + // Step through each leg of the path, ensuring that the Auto mode has not been stopped along the way + + // Step 1: Drive forward for .5 seconds + Right.setPower(-FORWARD_SPEED); + Left.setPower(FORWARD_SPEED); + runtime.reset(); + while (opModeIsActive() && (runtime.seconds() < 0.8)) { + telemetry.addData("Path", "Leg 1: %2.5f S Elapsed", runtime.seconds()); + telemetry.update(); + } + + // Step 2: Spin right for 1 seconds + Right.setPower(-TURN_SPEED); + Left.setPower(-TURN_SPEED); + runtime.reset(); + while (opModeIsActive() && (runtime.seconds() < 0.6)) { + telemetry.addData("Path", "Leg 2: %2.5f S Elapsed", runtime.seconds()); + telemetry.update(); + } + + // Step 3: Drive Forward for 1 Second + Right.setPower(-FORWARD_SPEED); + Left.setPower(FORWARD_SPEED); + runtime.reset(); + while (opModeIsActive() && (runtime.seconds() < 3.5)) { + telemetry.addData("Path", "Leg 3: %2.5f S Elapsed", runtime.seconds()); + telemetry.update(); + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeft.java new file mode 100644 index 00000000000..f1b6e22e128 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeft.java @@ -0,0 +1,45 @@ +package org.firstinspires.ftc.teamcode; + +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.hardware.Servo; + +/** + * Created by jxfio on 12/15/2017. + */ +@Autonomous(name = "Blue Left/Red Right", group = "robot2") +public class BlueLeft extends LinearOpMode{ + private DcMotor Left; + private DcMotor Right; + private Servo RF; + private Servo LF; + @Override + public void runOpMode() { + RF = hardwareMap.get(Servo.class, "right finger"); + LF = hardwareMap.get(Servo.class, "left finger"); + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + + waitForStart(); + Driver driver = new DriverWithEncoder(Left,Right,2.5, 15.375); + Left.setDirection(DcMotor.Direction.REVERSE); + // run until the end of the match (driver presses STOP) + RF.setPosition(.5); + LF.setPosition(.5); + driver.forward(34,.5); + RF.setPosition(.3); + LF.setPosition(.7); + while (opModeIsActive()) { + driver.forward(-1,.5); + driver.turn(40,.4); + driver.forward(3,.5); + driver.turn(-40,.4); + driver.forward(3,.5); + driver.turn(40,.4); + driver.forward(-1,.5); + driver.turn(-40,.4); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueRight.java new file mode 100644 index 00000000000..01bef87b9f9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueRight.java @@ -0,0 +1,55 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; + +/** + * Created by jxfio on 12/2/2017. + */ + +@Autonomous(name="Blue Right", group="robot2") +public class BlueRight extends LinearOpMode { + + // Declare OpMode members. + private DcMotor Left; + private DcMotor Right; + private Servo RF; + private Servo LF; + @Override + public void runOpMode() { + RF = hardwareMap.get(Servo.class, "right finger"); + LF = hardwareMap.get(Servo.class, "left finger"); + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + + waitForStart(); + Driver driver = new DriverWithEncoder(Left,Right,2.5, 15.375); + // run until the end of the match (driver presses STOP) + Left.setDirection(DcMotor.Direction.REVERSE); + //if you need to do keep it the right size comment 33/34 uncomment 35/36/38/39 + RF.setPosition(.5); + LF.setPosition(.5); + //RF.setPosition(.3); + //LF.setPosition(.7); + driver.forward(31,.5); + //RF.setPosition(.5); + //LF.setPosition(.5); + driver.turn(90,.2); + RF.setPosition(.3); + LF.setPosition(.7); + driver.forward(15,.5); + while (opModeIsActive()) { + driver.forward(-1,.5); + driver.turn(40,.4); + driver.forward(3,.5); + driver.turn(-40,.4); + driver.forward(3,.5); + driver.turn(40,.4); + driver.forward(-1,.5); + driver.turn(-40,.4); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleanTank.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleanTank.java new file mode 100644 index 00000000000..14520ad02e8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleanTank.java @@ -0,0 +1,41 @@ +package org.firstinspires.ftc.teamcode; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.teamcode.RoboticsUtils.PID; +//import org.firstinspires.ftc.robotcontroller.external.samples.HardwareK9bot; +/** + * Created by emilyhinds on 12/14/18. + */ +@TeleOp(name="CleanTank", group="CleanTank") +public class CleanTank extends LinearOpMode { + + // Declare OpMode members. + private DcMotor Left; + private DcMotor Right; + private DcMotor Shoulder; + private DcMotor Elbow; + + @Override + public void runOpMode() { + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + Shoulder = hardwareMap.get(DcMotor.class, "shoulder"); + Elbow = hardwareMap.get(DcMotor.class, "elbow"); + waitForStart(); + + while (opModeIsActive()) { + Left.setPower(Range.clip(gamepad1.left_stick_y, -1, 1)); + Right.setPower(Range.clip(-gamepad1.right_stick_y, -1, 1)); + Shoulder.setPower(.5 * Range.clip(gamepad2.left_stick_y, -1, 1)); + Elbow.setPower(.5 * Range.clip(-gamepad2.right_stick_y, -1, 1)); + + } + } + + + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java new file mode 100644 index 00000000000..ecbc20752ef --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java @@ -0,0 +1,256 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.IntegratingGyroscope; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.teamcode.RoboticsUtils.PID; + +import static android.content.Context.*; +import static java.lang.Math.abs; +import static java.lang.Math.pow; +import static java.lang.Math.sin; +import static java.lang.Math.sqrt; +import static java.lang.Math.toRadians; + + +/** + * Created by jxfio on 1/21/2018. + */ +@TeleOp(name="Climber", group="Linear Opmode") +public class ClimberBot2018 extends LinearOpMode{ + + // Declare OpMode members. + //packageContext(org.firstinspires.ftc.teamcode, 0); + ElapsedTime timer = new ElapsedTime(); + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor left; + private DcMotor back; + private DcMotor right; + private DcMotor wheelLift; + private DcMotor swivel; + private DcMotor arm; + private DcMotor leftClimb; + private DcMotor rightClimb; + private Servo lift; + private Servo leftFinger; + private Servo rightFinger; + private Servo backSwivel; + private IntegratingGyroscope gyro; + private ModernRoboticsI2cGyro MRI2CGyro; + PID θPID = new PID(.5,0,0);//changed from .5, .0001, 0.1 + @Override + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + MRI2CGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); + gyro = (IntegratingGyroscope)MRI2CGyro; + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must correspond to the names assigned during the robot configuration + // step (using the FTC Robot Controller app on the phone). + left = hardwareMap.get(DcMotor.class, "B1"); + back = hardwareMap.get(DcMotor.class, "B3"); + right = hardwareMap.get(DcMotor.class, "B2"); + wheelLift = hardwareMap.get(DcMotor.class, "B0"); + swivel = hardwareMap.get(DcMotor.class,"R0"); + arm = hardwareMap.get(DcMotor.class, "R1"); + leftClimb = hardwareMap.get(DcMotor.class, "R2"); + rightClimb = hardwareMap.get(DcMotor.class, "R3"); + lift = hardwareMap.get(Servo.class,"S2"); + leftFinger = hardwareMap.get(Servo.class, "S3"); + rightFinger = hardwareMap.get(Servo.class, "S4"); + backSwivel = hardwareMap.get(Servo.class, "S5"); + MRI2CGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); + gyro = (IntegratingGyroscope)MRI2CGyro; + telemetry.log().add("Gyro Calibrating. Do Not Move!"); + MRI2CGyro.calibrate(); + // Wait until the gyro calibration is complete + timer.reset(); + while (!isStopRequested() && MRI2CGyro.isCalibrating()) { + telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|"); + telemetry.update(); + sleep(50); + } + + telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start."); + telemetry.clear(); telemetry.update(); + + waitForStart(); + runtime.reset(); + telemetry.log().add("Press A & B to reset heading"); + // run until the end of the match (driver presses STOP) + double temp; + double leftPwr; + double backPwr; + double rightPwr; + double dt; + double Desiredθ = 0; + double TurnPWR; + double PrevTime = time; //previous time value + boolean curResetState = false; + boolean lastResetState = false; + boolean upRailMode = false; + boolean onRail = false; + boolean y = false; + double pos; + double prevpos=0; + double prevderiv=0; + double wheelLiftZero=0; + final double wheelLiftZerotoGround=-20; + PID wheelLiftPID = new PID(.01,0,0);//set all the ps to .01 instead of 1 + PID armPID = new PID(.01,0,0);//originally 1, ,00001,.1 (armswivel same) + PID armSwivelPID = new PID(.01,0,0); + boolean x=false; + boolean b=false; + boolean armSwivelZeroingState; + boolean lastArmSwivelZeroingState=false; + double armSwivelZeroState=0; + boolean armZeroingState; + boolean lastArmZeroingState=false; + double armZeroState=0; + boolean leftOpen=true; + boolean rightOpen = true; + // wheelLift.setPower(.01);//changed from 0.25 because it was slamming + // sleep(1000); //changed from 1000 because it was pushing into robot for too long + // wheelLiftZero = wheelLift.getCurrentPosition(); + // wheelLift.setPower(-.01); + // commented all of this out because no matter what we do it would just slam on the robot and we want to drive first and foremost. + + while (opModeIsActive()) { + //Gyro reset + curResetState = (gamepad1.a && gamepad1.b); + if (curResetState && !lastResetState) { + MRI2CGyro.resetZAxisIntegrator(); + } + lastResetState = curResetState; + armZeroingState = (gamepad2.a && gamepad2.b); + if (armZeroingState&&!lastArmZeroingState){ + armZeroState=arm.getCurrentPosition(); + } + lastArmZeroingState =armZeroingState; + armSwivelZeroingState = (gamepad2.y && gamepad2.a); + if (armSwivelZeroingState&&!lastArmSwivelZeroingState){ + armSwivelZeroState=arm.getCurrentPosition(); + } + lastArmSwivelZeroingState =armSwivelZeroingState; + if(gamepad1.y&&! y){ + upRailMode=true; + } + y = gamepad1.y; + if(gamepad2.y){ + lift.setPosition(0); + }else if(gamepad2.a){ + lift.setPosition(1); + }else{ + lift.setPosition(.5); + } + if(gamepad2.x&&!x){ + leftOpen=!leftOpen; + } + x=gamepad2.x; + if(gamepad2.b&&!b){ + rightOpen=!rightOpen; + } + b=gamepad2.b; + if(leftOpen){ + leftFinger.setPosition(1); + }else{ + leftFinger.setPosition(.25); + } + if(rightOpen){ + rightFinger.setPosition(0); + }else{ + rightFinger.setPosition(.75); + } + while (upRailMode){ + double dp; + dt = time - PrevTime; + PrevTime = time; + time = getRuntime(); + if(gamepad1.y&&! y){ + upRailMode=false; + } + armPID.iteratePID(arm.getCurrentPosition()-armZeroState+70,dt); + armSwivelPID.iteratePID(swivel.getCurrentPosition()-armSwivelZeroState,dt); + arm.setPower(Range.clip(armPID.getPID(),-1,1)); + swivel.setPower(Range.clip(armSwivelPID.getPID(),-1,1)); + y = gamepad1.y; + backSwivel.setPosition(1); + leftClimb.setPower(-1); + rightClimb.setPower(1); + pos = rightClimb.getCurrentPosition(); + dp = pos-prevpos; + prevpos = pos; + if(Math.abs(dp/dt-prevderiv)>10){ + onRail = true; + } + + while (onRail){ + dt = time - PrevTime; + PrevTime = time; + time = getRuntime(); + wheelLiftPID.iteratePID(wheelLift.getCurrentPosition()- wheelLiftZero,dt); + wheelLift.setPower(Range.clip(wheelLiftPID.getPID(),-1,1)); + leftClimb.setPower(Range.clip(-gamepad1.left_stick_y,-1,1)); + rightClimb.setPower(Range.clip(gamepad1.left_stick_y,-1,1)); + if(Math.abs((pos-prevpos)/dt-prevderiv)>10){ + onRail = false; + } + wheelLiftPID.iteratePID(wheelLift.getCurrentPosition()-wheelLiftZero,dt); + armPID.iteratePID(arm.getCurrentPosition()-armZeroState+70,dt); + armSwivelPID.iteratePID(swivel.getCurrentPosition()-armSwivelZeroState,dt); + arm.setPower(Range.clip(armPID.getPID(),-1,1)); + swivel.setPower(Range.clip(armSwivelPID.getPID(),-1,1)); + wheelLift.setPower(-Range.clip(wheelLiftPID.getPID(),-1,1)); + } + wheelLiftPID.iteratePID(wheelLift.getCurrentPosition()-wheelLiftZero+wheelLiftZerotoGround,dt); + wheelLift.setPower(-Range.clip(wheelLiftPID.getPID(),-1,1)); + prevderiv=(pos-prevpos)/dt; + back.setPower(Range.clip(gamepad1.left_stick_y,-1,1)); + } + double θ = MRI2CGyro.getIntegratedZValue(); + double drivey = -gamepad1.left_stick_y; //should be neg here + double drivex = -gamepad1.left_stick_x; + //double turn = gamepad1.right_stick_x; + //negative sign because y goes in wrong direction + double time = getRuntime(); + double driveθ = Math.atan2(drivex,drivey); //direction //took out neg + double driveV = sqrt(pow(drivey,2)+pow(drivex,2)); //magnitude -pythagorean theorem + driveV = Range.clip(driveV,-1,1); + //integrating for angle + dt = time - PrevTime; + Desiredθ += (-gamepad1.right_stick_x * dt * 90); // 45 degrees per second + PrevTime = time; + //PID + wheelLiftPID.iteratePID(wheelLift.getCurrentPosition()-wheelLiftZero+wheelLiftZerotoGround,dt); + wheelLift.setPower(Range.clip(-wheelLiftPID.getPID(),-1,1)); + θPID.iteratePID(θ-Desiredθ,dt); + TurnPWR = θPID.getPID(); + //TurnPWR = (θ - Desiredθ)/-100; + TurnPWR = Range.clip(TurnPWR, -.75, .75); + //drivebase powers + temp= 1 - TurnPWR; + leftPwr= TurnPWR + temp * driveV*sin(driveθ+toRadians(60)); //power to send motor- proportional to the sign of the angle to drive at + backPwr= TurnPWR + temp * driveV*sin(driveθ+toRadians(180)); + rightPwr= TurnPWR + temp * driveV*sin(driveθ-toRadians(60)); + // Send calculated power to motors + swivel.setPower(Range.clip(gamepad2.left_stick_x,-1,1)); + arm.setPower(Range.clip(gamepad2.left_stick_y,-1,1)); + backSwivel.setPosition(1/2); + left.setPower(Range.clip(leftPwr, -1, 1)); //range.clip concerns for ratios?- put in in case + back.setPower(Range.clip(backPwr, -1, 1)); + right.setPower(Range.clip(rightPwr, -1, 1)); + //Telemetry Data + /*telemetry.addData("path0","driveθ:"); + telemetry.addData("path1","A Power:" + String.valueOf(leftPwr) + " B Power:" + String.valueOf(backPwr) + " C Power:" +String.valueOf(rightPwr)); + telemetry.addData("path2","Integrated Z:" + String.valueOf(MRI2CGyro.getIntegratedZValue()) + " Desiredθ: " + String.valueOf(Desiredθ)); + telemetry.addData("path3","Time: "+ toString().valueOf(time)); + telemetry.update();// prints above stuff to phone*/ + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComputerVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComputerVision.java new file mode 100644 index 00000000000..5f75eacaa62 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComputerVision.java @@ -0,0 +1,341 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.matrices.MatrixF; +import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; +//import org.firstinspires.ftc.ftccommon.external.navigation.VuforiaTrackables.RelicRecoveryVuMark; + +import java.util.ArrayList; +import java.util.List; + +/** + * This 2016-2017 OpMode illustrates the basics of using the Vuforia localizer to determine + * positioning and orientation of robot on the FTC field. + * The code is structured as a LinearOpMode + * + * Vuforia uses the phone's camera to inspect it's surroundings, and attempt to locate target images. + * + * When images are located, Vuforia is able to determine the position and orientation of the + * image relative to the camera. This sample code than combines that information with a + * knowledge of where the target images are on the field, to determine the location of the camera. + * + * This example assumes a "diamond" field configuration where the red and blue alliance stations + * are adjacent on the corner of the field furthest from the audience. + * From the Audience perspective, the Red driver station is on the right. + * The two vision target are located on the two walls closest to the audience, facing in. + * The Stones are on the RED side of the field, and the Chips are on the Blue side. + * + * A final calculation then uses the location of the camera on the robot to determine the + * robot's location and orientation on the field. + * + * @see VuforiaLocalizer + * @see VuforiaTrackableDefaultListener + * see ftc_app/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list. + * + * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as + * is explained below. + */ + +@Autonomous(name="Computer Vision", group ="Concept") +public class ComputerVision extends LinearOpMode { + + public static final String TAG = "Vuforia Navigation Sample"; + + OpenGLMatrix lastLocation = null; + + /** + * {@link #vuforia} is the variable we will use to store our instance of the Vuforia + * localization engine. + */ + VuforiaLocalizer vuforia; + + @Override public void runOpMode() { + /* + * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone); + * If no camera monitor is desired, use the parameterless constructor instead (commented out below). + */ + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); + + // OR... Do Not Activate the Camera Monitor View, to save power + // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + + /* + * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which + * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function. + * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer + * web site at https://developer.vuforia.com/license-manager. + * + * Vuforia license keys are always 380 characters long, and look as if they contain mostly + * random data. As an example, here is a example of a fragment of a valid key: + * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ... + * Once you've obtained a license key, copy the string from the Vuforia web site + * and paste it in to your code onthe next line, between the double quotes. + */ + parameters.vuforiaLicenseKey = "AaPSQTf/////AAABmSC/czqVrkugjRHMuUPfIJ0G0Ew2pPxpfDhUTwNDDO3ZH4ZJ5tTwd+z0MV5DJOT0zc5TBJtsyyYucFpdGvJzHeiT/+XudkoYbayPMUsXox/Ql7dnAx0/fMJYYzR5uaiafhyqZNRVJICBkRFCd4xcT+7s+1jp/OCZT3kizHMtgyvO4KNgGF/UOzRt5GOnu4IiALplY+0ppb21RR8p7t9v6NXbKrhHUTSw0Wsx4OAvbFwsnrxcxOn7lfohME/rb8wZcMCsdwrPSrk+NGPmQBZILXL78eK1dqDVdWv0NYosY9TgNhqGodJAhteozrvuDBHGBn2PYHRBLKKI1Un/Yz075NQBT9yi+Z007APlAH/UVsP4"; + + /* + * We also indicate which camera on the RC that we wish to use. + * Here we chose the back (HiRes) camera (for greater range), but + * for a competition robot, the front camera might be more convenient. + */ + parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK; + this.vuforia = ClassFactory.createVuforiaLocalizer(parameters); + + /** + * Load the data sets that for the trackable objects we wish to track. These particular data + * sets are stored in the 'assets' part of our application (you'll see them in the Android + * Studio 'Project' view over there on the left of the screen). You can make your own datasets + * with the Vuforia Target Manager: https://developer.vuforia.com/target-manager. PDFs for the + * example "StonesAndChips", datasets can be found in in this project in the + * documentation directory. + */ + VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark"); + VuforiaTrackable relicTemplate = relicTrackables.get(0); + relicTemplate.setName("relicVuMarkTemplate"); // can help in debugging; otherwise not necessary + + telemetry.addData(">", "Press Play to start"); telemetry.update(); + waitForStart(); + relicTrackables.activate(); + + + /** + * We use units of mm here because that's the recommended units of measurement for the + * size values specified in the XML for the ImageTarget trackables in data sets. E.g.: + * + * You don't *have to* use mm here, but the units here and the units used in the XML + * target configuration files *must* correspond for the math to work out correctly. + */ + float mmPerInch = 25.4f; + float mmBotWidth = 18 * mmPerInch; // ... or whatever is right for your robot + float mmFTCFieldWidth = (12*12 - 2) * mmPerInch; // the FTC field is ~11'10" center-to-center of the glass panels + + /** + * In order for localization to work, we need to tell the system where each target we + * wish to use for navigation resides on the field, and we need to specify where on the robot + * the phone resides. These specifications are in the form of transformation matrices. + * Transformation matrices are a central, important concept in the math here involved in localization. + * See Transformation Matrix + * for detailed information. Commonly, you'll encounter transformation matrices as instances + * of the {@link OpenGLMatrix} class. + * + * For the most part, you don't need to understand the details of the math of how transformation + * matrices work inside (as fascinating as that is, truly). Just remember these key points: + *
    + * + *
  1. You can put two transformations together to produce a third that combines the effect of + * both of them. If, for example, you have a rotation transform R and a translation transform T, + * then the combined transformation matrix RT which does the rotation first and then the translation + * is given by {@code RT = T.multiplied(R)}. That is, the transforms are multiplied in the + * reverse of the chronological order in which they applied.
    2. + * + *
  2. A common way to create useful transforms is to use methods in the {@link OpenGLMatrix} + * class and the Orientation class. See, for example, {@link OpenGLMatrix#translation(float, + * float, float)}, {@link OpenGLMatrix#rotation(AngleUnit, float, float, float, float)}, and + * {@link Orientation#getRotationMatrix(AxesReference, AxesOrder, AngleUnit, float, float, float)}. + * Related methods in {@link OpenGLMatrix}, such as {@link OpenGLMatrix#rotated(AngleUnit, + * float, float, float, float)}, are syntactic shorthands for creating a new transform and + * then immediately multiplying the receiver by it, which can be convenient at times.
    3. + * + *
  3. If you want to break open the black box of a transformation matrix to understand + * what it's doing inside, use {@link MatrixF#getTranslation()} to fetch how much the + * transform will move you in x, y, and z, and use {@link Orientation#getOrientation(MatrixF, + * AxesReference, AxesOrder, AngleUnit)} to determine the rotational motion that the transform + * will impart. See {@link #format(OpenGLMatrix)} below for an example.
    4. + * + *
+ * + * This example places the "stones" image on the perimeter wall to the Left + * of the Red Driver station wall. Similar to the Red Beacon Location on the Res-Q + * + * This example places the "chips" image on the perimeter wall to the Right + * of the Blue Driver station. Similar to the Blue Beacon Location on the Res-Q + * + * See the doc folder of this project for a description of the field Axis conventions. + * + * Initially the target is conceptually lying at the origin of the field's coordinate system + * (the center of the field), facing up. + * + * In this configuration, the target's coordinate system aligns with that of the field. + * + * In a real situation we'd also account for the vertical (Z) offset of the target, + * but for simplicity, we ignore that here; for a real robot, you'll want to fix that. + * + * To place the Stones Target on the Red Audience wall: + * - First we rotate it 90 around the field's X axis to flip it upright + * - Then we rotate it 90 around the field's Z access to face it away from the audience. + * - Finally, we translate it back along the X axis towards the red audience wall. + */ + /* OpenGLMatrix redTargetLocationOnField = OpenGLMatrix + /* Then we translate the target off to the RED WALL. Our translation here + is a negative translation in X. + .translation(-mmFTCFieldWidth/2, 0, 0) + .multiplied(Orientation.getRotationMatrix( + /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z + AxesReference.EXTRINSIC, AxesOrder.XZX, + AngleUnit.DEGREES, 90, 90, 0)); + redTarget.setLocation(redTargetLocationOnField); + RobotLog.ii(TAG, "Red Target=%s", format(redTargetLocationOnField)); + + /* + * To place the Stones Target on the Blue Audience wall: + * - First we rotate it 90 around the field's X axis to flip it upright + * - Finally, we translate it along the Y axis towards the blue audience wall. + */ + //OpenGLMatrix blueTargetLocationOnField = OpenGLMatrix + /* Then we translate the target off to the Blue Audience wall. + Our translation here is a positive translation in Y. + .translation(0, mmFTCFieldWidth/2, 0) + .multiplied(Orientation.getRotationMatrix( + /* First, in the fixed (field) coordinate system, we rotate 90deg in X + AxesReference.EXTRINSIC, AxesOrder.XZX, + AngleUnit.DEGREES, 90, 0, 0)); + blueTarget.setLocation(blueTargetLocationOnField); + RobotLog.ii(TAG, "Blue Target=%s", format(blueTargetLocationOnField)); + + /** + * Create a transformation matrix describing where the phone is on the robot. Here, we + * put the phone on the right hand side of the robot with the screen facing in (see our + * choice of BACK camera above) and in landscape mode. Starting from alignment between the + * robot's and phone's axes, this is a rotation of -90deg along the Y axis. + * + * When determining whether a rotation is positive or negative, consider yourself as looking + * down the (positive) axis of rotation from the positive towards the origin. Positive rotations + * are then CCW, and negative rotations CW. An example: consider looking down the positive Z + * axis towards the origin. A positive rotation about Z (ie: a rotation parallel to the the X-Y + * plane) is then CCW, as one would normally expect from the usual classic 2D geometry. + + OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix + .translation(mmBotWidth/2,0,0) + .multiplied(Orientation.getRotationMatrix( + AxesReference.EXTRINSIC, AxesOrder.YZY, + AngleUnit.DEGREES, -90, 0, 0)); + RobotLog.ii(TAG, "phone=%s", format(phoneLocationOnRobot)); + + + * Let the trackable listeners we care about know where the phone is. We know that each + * listener is a {@link VuforiaTrackableDefaultListener} and can so safely cast because + * we have not ourselves installed a listener of a different type. + + ((VuforiaTrackableDefaultListener)redTarget.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); + ((VuforiaTrackableDefaultListener)blueTarget.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); + + /** + * A brief tutorial: here's how all the math is going to work: + * + * C = phoneLocationOnRobot maps phone coords -> robot coords + * P = tracker.getPose() maps image target coords -> phone coords + * L = redTargetLocationOnField maps image target coords -> field coords + * + * So + * + * C.inverted() maps robot coords -> phone coords + * P.inverted() maps phone coords -> imageTarget coords + * + * Putting that all together, + * + * L x P.inverted() x C.inverted() maps robot coords to field coords. + * + * @see VuforiaTrackableDefaultListener#getRobotLocation() + */ + + + /** Wait for the game to begin */ + telemetry.addData(">", "Press Play to start tracking"); + telemetry.update(); + waitForStart(); + + /** Start tracking the data sets we care about. */ + relicTrackables.activate(); + + while (opModeIsActive()) { + + /* for (VuforiaTrackable trackable : allTrackables) { + /** + * getUpdatedRobotLocation() will return null if no new information is available since + * the last time that call was made, or if the trackable is not currently visible. + * getRobotLocation() will return null if the trackable is not currently visible. + + telemetry.addData(trackable.getName(), ((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible() ? "Visible" : "Not Visible"); // + + OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation(); + if (robotLocationTransform != null) { + lastLocation = robotLocationTransform; + } + } + /** + * Provide feedback as to where the robot was last located (if we know). + */ + + RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate); + if (vuMark != RelicRecoveryVuMark.UNKNOWN) { +/* Found an instance of the template. In the actual game, you will probably +* loop until this condition occurs, then move on to act accordingly depending * on which VuMark was visible. */ + telemetry.addData("VuMark", "%s visible", vuMark); + } + + /* if (lastLocation != null) { + // RobotLog.vv(TAG, "robot=%s", format(lastLocation)); + telemetry.addData("Pos", format(lastLocation)); + } else { + telemetry.addData("Pos", "Unknown"); + }*/ + telemetry.update(); + } + } + + /** + * A simple utility that extracts positioning information from a transformation matrix + * and formats it in a form palatable to a human being. + */ + //String format(OpenGLMatrix transformationMatrix) { + //return transformationMatrix.formatAsTransform(); +} + +//super helpful website https://www.firstinspires.org/sites/default/files/uploads/resource_library/ftc/using-vumarks.pdf \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Driver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Driver.java new file mode 100644 index 00000000000..6378ac884ba --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Driver.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.teamcode; + +/** + * Created by jxfio on 12/15/2017. + */ + +public abstract class Driver { + public abstract void forward(double distance, double power); + public abstract void turn(double degrees, double power); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverNoEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverNoEncoder.java new file mode 100644 index 00000000000..8950cad7ece --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverNoEncoder.java @@ -0,0 +1,35 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +/** + * Created by jxfio on 12/15/2017. + */ + +public class DriverNoEncoder extends Driver { + public DcMotor leftMotor; + public DcMotor rightMotor; + + public DriverNoEncoder(DcMotor left, DcMotor right){ + leftMotor = left; + rightMotor = right; + } + //by distance we mean time + public void forward(double distance, double power) { + ElapsedTime runtime = new ElapsedTime(); + while (runtime.milliseconds() < distance) { + leftMotor.setPower(power); + rightMotor.setPower(-power); + } + } + //by degrees we mean time + public void turn(double degrees, double power) { + ElapsedTime runtime = new ElapsedTime(); + while (runtime.milliseconds() < degrees) { + leftMotor.setPower(power); + rightMotor.setPower(power); + } + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverWithEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverWithEncoder.java new file mode 100644 index 00000000000..caeee4daa5d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverWithEncoder.java @@ -0,0 +1,63 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.Range; + +/** + * Created by jxfio on 12/15/2017. + */ + +public class DriverWithEncoder extends Driver{ + public DcMotor leftMotor; + public DcMotor rightMotor; + public double wheelradius; + public double ticsPerRevolution = 1416; + public double wheelSeperation; + public DriverWithEncoder(DcMotor left, DcMotor right, double radius, double wheelSep){ + leftMotor = left; + rightMotor = right; + wheelradius = radius; + leftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + wheelSeperation = wheelSep; + } + public void forward(double distance, double power){ + double wheelcirc = 2 * Math.PI * wheelradius; + double distanceInTics = (distance/wheelcirc)* ticsPerRevolution; + double RightTicsGoal = distanceInTics + rightMotor.getCurrentPosition(); + double LeftTicsGoal = distanceInTics + leftMotor.getCurrentPosition(); + double RTtG = RightTicsGoal - rightMotor.getCurrentPosition(); + double LTtG = LeftTicsGoal - leftMotor.getCurrentPosition(); + double eps = 5; + leftMotor.setTargetPosition(((int)LeftTicsGoal)); + rightMotor.setTargetPosition((int)RightTicsGoal); + leftMotor.setPower(power); + rightMotor.setPower(power); + while (Math.abs(RTtG)>eps && Math.abs(LTtG)>eps){ + RTtG = RightTicsGoal - rightMotor.getCurrentPosition(); + LTtG = LeftTicsGoal - leftMotor.getCurrentPosition(); + } + leftMotor.setPower(0); + rightMotor.setPower(0); + } + public void turn (double degrees, double power){ + double arcLength = 1.2*degrees*wheelSeperation*Math.PI/360; + double wheelcirc = 2 * Math.PI * wheelradius; + double arcLengthInTics = (arcLength/wheelcirc)* ticsPerRevolution; + double RightTicsGoal = rightMotor.getCurrentPosition() + arcLengthInTics; + double LeftTicsGoal = leftMotor.getCurrentPosition() - arcLengthInTics; + double RTtG = RightTicsGoal - rightMotor.getCurrentPosition(); + double LTtG = LeftTicsGoal - leftMotor.getCurrentPosition(); + double eps = 5; + leftMotor.setTargetPosition(((int)LeftTicsGoal)); + rightMotor.setTargetPosition((int)RightTicsGoal); + leftMotor.setPower(power); + rightMotor.setPower(power); + while (Math.abs(RTtG)>eps && Math.abs(LTtG)>eps){ + RTtG = RightTicsGoal - rightMotor.getCurrentPosition(); + LTtG = LeftTicsGoal - leftMotor.getCurrentPosition(); + } + leftMotor.setPower(0); + rightMotor.setPower(0); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java new file mode 100644 index 00000000000..70e1e7dfa35 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java @@ -0,0 +1,55 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; + +/** + * Created by jxfio on 12/15/2017. + */ + +@Autonomous(name="Red Left", group="robot2") +public class RedLeft extends LinearOpMode { + + // Declare OpMode members. + private DcMotor Left; + private DcMotor Right; + private Servo RF; + private Servo LF; + @Override + public void runOpMode() { + RF = hardwareMap.get(Servo.class, "right finger"); + LF = hardwareMap.get(Servo.class, "left finger"); + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + + waitForStart(); + Driver driver = new DriverWithEncoder(Left,Right,2.5, 15.375); + // run until the end of the match (driver presses STOP) + Left.setDirection(DcMotor.Direction.REVERSE); + //if you need to do keep it the right size comment 33/34 uncomment 35/36/38/39 + RF.setPosition(.5); + LF.setPosition(.5); + //RF.setPosition(.3); + //LF.setPosition(.7); + driver.forward(31,.5); + //RF.setPosition(.5); + //LF.setPosition(.5); + driver.turn(-90,.2); + RF.setPosition(.3); + LF.setPosition(.7); + driver.forward(15,.5); + while (opModeIsActive()) { + driver.forward(-1,.5); + driver.turn(40,.4); + driver.forward(3,.5); + driver.turn(-40,.4); + driver.forward(3,.5); + driver.turn(40,.4); + driver.forward(-1,.5); + driver.turn(-40,.4); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils/PID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils/PID.java new file mode 100644 index 00000000000..85a63df0a0e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils/PID.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode.RoboticsUtils; + +public class PID { + public double kp; + public double ki; + public double kd; + public double pi=0; + public double ii=0; + public double di=0; + public double preverror = 0; + public PID(double kp, double ki, double kd){ + this.kp = kp; //making 3 number variables to set later on + this.ki = ki; + this.kd = kd; + } + public void iteratePID (double error,double dt){ + pi=kp*error; + ii+=ki*error*dt; + kd = di*error/preverror; + this.preverror = error; + } + public double getPID(){ + return pi+ki+di; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java new file mode 100644 index 00000000000..5d8962981bc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java @@ -0,0 +1,133 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.IntegratingGyroscope; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; +import java.lang.Math; + +import static java.lang.Math.cos; +import static java.lang.Math.sin; +@Disabled +@TeleOp(name="Shoddier Tele OP", group="Linear Opmode") +public class ShoddierTeleOP extends LinearOpMode { + + // Declare OpMode members. + ElapsedTime timer = new ElapsedTime(); + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor ADrive; + private DcMotor BDrive; + private DcMotor CDrive; + private IntegratingGyroscope gyro; + private ModernRoboticsI2cGyro MRI2CGyro; + private DcMotor Shoulder; + private DcMotor Elbow; + private Servo RF; + private Servo LF; + private Servo Wrist; + @Override + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must correspond to the names assigned during the robot configuration + // step (using the FTC Robot Controller app on the phone). + ADrive = hardwareMap.get(DcMotor.class, "A"); + BDrive = hardwareMap.get(DcMotor.class, "B"); + CDrive = hardwareMap.get(DcMotor.class, "C"); + Shoulder = hardwareMap.get(DcMotor.class, "shoulder"); + Elbow = hardwareMap.get(DcMotor.class, "elbow"); + RF = hardwareMap.get(Servo.class, "right finger"); + LF = hardwareMap.get(Servo.class, "left finger"); + Wrist = hardwareMap.get(Servo.class, "wrist"); + MRI2CGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); + gyro = (IntegratingGyroscope)MRI2CGyro; + telemetry.log().add("Gyro Calibrating. Do Not Move!"); + MRI2CGyro.calibrate(); + // Wait until the gyro calibration is complete + timer.reset(); + while (!isStopRequested() && MRI2CGyro.isCalibrating()) { + telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|"); + telemetry.update(); + sleep(50); + } + + telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start."); + telemetry.clear(); telemetry.update(); + + waitForStart(); + runtime.reset(); + telemetry.log().add("Press A & B to reset heading"); + // run until the end of the match (driver presses STOP) + double temp; + double APwr; + double BPwr; + double CPwr; + double Fingeroffset = 0; + double Wristoffset = 0; + boolean Lb = false; + boolean Lx = false; + boolean LRbumper = false; + boolean LLbumper = false; + while (opModeIsActive()) { + double drivey = -gamepad1.left_stick_y; + double drivex = gamepad1.left_stick_x; + double turn = gamepad1.right_stick_x; + double time = getRuntime(); + //drivebase powers + APwr= drivex+drivey+turn; + BPwr= drivex*cos((120)*Math.PI/180)+drivey*sin((120)*Math.PI/180)+turn; + CPwr= drivex*cos((-120)*Math.PI/180)+drivey*sin((-120)*Math.PI/180)+turn; + if(Math.max(Math.abs(APwr), Math.max(Math.abs(BPwr), Math.abs(CPwr)))>1) { + temp = 1 / Math.max(Math.abs(APwr), Math.max(Math.abs(BPwr), Math.abs(CPwr))); + }else{ + temp=1; + } + APwr = APwr*temp; + BPwr = BPwr*temp; + CPwr = CPwr*temp; + // Send calculated power to wheels + ADrive.setPower(Range.clip(APwr, -1, 1)); + BDrive.setPower(Range.clip(BPwr, -1, 1)); + CDrive.setPower(Range.clip(CPwr, -1, 1)); + //Shoddy arm code + Elbow.setPower(.1*Range.clip(-1*gamepad2.left_stick_y,-1,1)); + Shoulder.setPower(.1*Range.clip(-1*gamepad2.right_stick_y, -1,1)); + boolean b = gamepad2.b; + if(b && !Lb){ + Fingeroffset += .1; + } + Lb = b; + boolean x = gamepad2.x; + if(Lx && x){ + Fingeroffset -= .1; + } + Lx = x; + boolean Rbumper = gamepad2.right_bumper; + if(Rbumper && !LRbumper){ + Wristoffset += .1; + } + LRbumper = Rbumper; + boolean Lbumper = gamepad2.left_bumper; + if(Lbumper && !LLbumper){ + Wristoffset -= .1; + } + LLbumper = Lbumper; + if(Math.abs(Fingeroffset)>.5){ + Fingeroffset = .5*Math.abs(Fingeroffset)/Fingeroffset; + } + if(Math.abs(Wristoffset)>.5){ + Wristoffset = .5*Math.abs(Wristoffset)/Wristoffset; + } + RF.setPosition(Range.clip(0.5 - Fingeroffset, 0,1)); + LF.setPosition(Range.clip(Fingeroffset + 0.5, 0,1)); + Wrist.setPosition(Range.clip(Wristoffset + 0.5, 0,1)); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java new file mode 100644 index 00000000000..2ba80d8e1a7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java @@ -0,0 +1,97 @@ +package org.firstinspires.ftc.teamcode; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.teamcode.RoboticsUtils.PID; +//import org.firstinspires.ftc.robotcontroller.external.samples.HardwareK9bot; +/** + * Created by emilyhinds on 9/9/18. + */ +@TeleOp(name="TankDriveRoverRuckus", group="TankDriveRoverRuckus") +public class TankDriveRoverRuckus extends LinearOpMode { + // Declare OpMode members. + private DcMotor Left; + private DcMotor Right; + private DcMotor Shoulder; + private DcMotor Elbow; + //private DcMotor Wrist; + // private Servo LGrabber; + //private Servo RGrabber; + //private DcMotor Winch; + //private DcMotor Sideways; + @Override + public void runOpMode() { + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + Shoulder = hardwareMap.get(DcMotor.class, "shoulder"); + Elbow = hardwareMap.get(DcMotor.class, "elbow"); + //Wrist = hardwareMap.get(DcMotor.class,"wrist"); + //LGrabber = hardwareMap.get(Servo.class, "lgrab"); + // RGrabber = hardwareMap.get(Servo.class, "rgrab"); + //Winch = hardwareMap.get(DcMotor.class,"winch"); + //Sideways = hardwareMap.get(DcMotor.class,"sideways"); + //boolean bumper = false; + //int wristdir = 1; + waitForStart(); + /* boolean b = true; + boolean x = true; + double loffset = 0; + double roffset = 0; + double prevTime= 0; + double currentTime = 0; + double elbow = Elbow.getCurrentPosition(); + double shoulder = Shoulder.getCurrentPosition(); + //PID elbowPID = new PID(1,.0000000001,.00001); + //PID shoulderPID = new PID(1,.0000000001,.00001); + // run until the end of the match (driver presses STOP) */ + while (opModeIsActive()) { + Left.setPower(Range.clip(gamepad1.left_stick_y, -1, 1)); + Right.setPower(Range.clip(-gamepad1.right_stick_y, -1, 1)); + //Shoulder.setPower(Range.clip(shoulderPID.getPID(), -1,1)); + //Elbow.setPower(Range.clip(elbowPID.getPID(), -1,1)); + Shoulder.setPower(.5*Range.clip(gamepad2.left_stick_y, -1,1)); + Elbow.setPower(.5*Range.clip(gamepad2.right_stick_y, -1,1)); + /*if(gamepad2.right_bumper&&!bumper){ + wristdir = wristdir*-1; + } + bumper = gamepad2.right_bumper; + //Wrist.setPower(Range.clip(.5*gamepad2.right_trigger*wristdir,-1,1)); */ + /*if(gamepad2.b&&!b){ + roffset = roffset*-1; + }*/ + /* if(gamepad2.b&&!b&&loffset==0){ + roffset=.6; + }else if(gamepad2.b&&!b){ + roffset=0; + } + b = gamepad2.b; + RGrabber.setPosition(Range.clip(RGrabber.MAX_POSITION-roffset,RGrabber.MIN_POSITION,RGrabber.MAX_POSITION)); + if(gamepad2.x&&!x&&loffset==0){ + loffset=.6; + }else if(gamepad2.x&&!x){ + loffset=0; + } + x = gamepad2.x; + LGrabber.setPosition(Range.clip(LGrabber.MIN_POSITION+loffset,LGrabber.MIN_POSITION,LGrabber.MAX_POSITION)); + prevTime = currentTime; + currentTime =getRuntime(); */ + //elbow-=gamepad1.right_stick_y*(currentTime-prevTime)*4/100; + //shoulder-=gamepad1.left_stick_y*(currentTime-prevTime)*4/100; + //elbowPID.iteratePID(elbow-Elbow.getCurrentPosition(), currentTime-prevTime); + //shoulderPID.iteratePID(shoulder-Shoulder.getCurrentPosition(), currentTime-prevTime); + //Winch code- up: right trigger, down: left trigger + //Winch.setPower(Range.clip(gamepad1.right_trigger-gamepad1.left_trigger,-1,1)); + //Sideways- front wheel, push both sticks to control + // both gamepads are inversed, so need to minus + //Sideways.setPower(Range.clip(-gamepad1.right_stick_y+gamepad1.left_stick_y,-1,1)); + //telemetry.addData("bools",liftmode+" "+a); + //telemetry.addData("lift pos", Lift.getCurrentPosition()); + //telemetry.addData("loffset",loffset); + //telemetry.addData("roffset", roffset); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java new file mode 100644 index 00000000000..9d7f6cb4691 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java @@ -0,0 +1,127 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.IntegratingGyroscope; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +import static java.lang.Math.abs; +import static java.lang.Math.cos; +import static java.lang.Math.pow; +import static java.lang.Math.sin; +import static java.lang.Math.sqrt; +import static java.lang.Math.toRadians; + +/** + * Created by jxfio on 1/21/2018. + */ +@TeleOp(name="tribot", group="Linear Opmode") +public class TriangleRobot extends LinearOpMode{ + // Declare OpMode members. + ElapsedTime timer = new ElapsedTime(); + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor ADrive; + private DcMotor BDrive; + private DcMotor CDrive; + private IntegratingGyroscope gyro; + private ModernRoboticsI2cGyro MRI2CGyro; + private Servo F; + @Override + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + MRI2CGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); + gyro = (IntegratingGyroscope)MRI2CGyro; + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must correspond to the names assigned during the robot configuration + // step (using the FTC Robot Controller app on the phone). + ADrive = hardwareMap.get(DcMotor.class, "A"); + BDrive = hardwareMap.get(DcMotor.class, "B"); + CDrive = hardwareMap.get(DcMotor.class, "C"); + MRI2CGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); + gyro = (IntegratingGyroscope)MRI2CGyro; + telemetry.log().add("Gyro Calibrating. Do Not Move!"); + MRI2CGyro.calibrate(); + // Wait until the gyro calibration is complete + F = hardwareMap.get(Servo.class, "F"); + timer.reset(); + while (!isStopRequested() && MRI2CGyro.isCalibrating()) { + telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|"); + telemetry.update(); + sleep(50); + } + + telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start."); + telemetry.clear(); telemetry.update(); + + waitForStart(); + runtime.reset(); + telemetry.log().add("Press A & B to reset heading"); + // run until the end of the match (driver presses STOP) + double temp; + double APwr; + double BPwr; + double CPwr; + double dt; + double Desiredθ = 0; + double Fingeroffset = 0; + double TurnPWR; + double PrevTime = time; //previous time value + boolean curResetState = false; + boolean lastResetState = false; + boolean b = false; + boolean x = false; + while (opModeIsActive()) { + curResetState = (gamepad1.a && gamepad1.b); + if (curResetState && !lastResetState) { + MRI2CGyro.resetZAxisIntegrator(); + } + lastResetState = curResetState; + double θ = MRI2CGyro.getIntegratedZValue(); + double drivey = -gamepad1.left_stick_y; //should be neg here + double drivex = -gamepad1.left_stick_x; + //double turn = gamepad1.right_stick_x; + //negative sign because y goes in wrong direction + double time = getRuntime(); + double driveθ = Math.atan2(drivex,drivey); //direction //took out neg + double driveV = (sqrt(pow(drivey,2)+pow(drivex,2))); //magnitude -pythagorean theorem + driveV = Range.clip(driveV,-1,1); + //integrating for angle + dt = time - PrevTime; + Desiredθ += (-gamepad1.right_stick_x * dt * 90); // 45 degrees per second + PrevTime = time; + //PIDθ + TurnPWR = (θ - Desiredθ)/-100; + TurnPWR = Range.clip(TurnPWR, -.75, .75); + if(gamepad2.b && !b && Fingeroffset <= .4){ + Fingeroffset += .1; + } + if(gamepad2.x && !x && Fingeroffset >= .1 ){ + Fingeroffset -= .1; + } + b = gamepad2.b; + x = gamepad2.x; + //drivebase powers + temp= 1 - TurnPWR; + APwr= TurnPWR + temp * driveV*sin(driveθ); //power to send motor- proportional to the sign of the angle to drive at + BPwr= TurnPWR + temp * driveV*sin(driveθ+toRadians(120)); + CPwr= TurnPWR + temp * driveV*sin(driveθ-toRadians(120)); + F.setPosition(Range.clip(0.5 - Fingeroffset, -1,1)); + // Send calculated power to wheels + ADrive.setPower(Range.clip(APwr, -1, 1)); //range.clip concerns for ratios?- put in in case + BDrive.setPower(Range.clip(BPwr, -1, 1)); + CDrive.setPower(Range.clip(CPwr, -1, 1)); + //Telemetry Data + telemetry.addData("path0","driveθ:"); + telemetry.addData("path1","A Power:" + String.valueOf(APwr) + " B Power:" + String.valueOf(BPwr) + " C Power:" +String.valueOf(CPwr)); + telemetry.addData("path2","Integrated Z:" + String.valueOf(MRI2CGyro.getIntegratedZValue()) + " Desiredθ: " + String.valueOf(Desiredθ)); + telemetry.addData("path3","Time: "+ toString().valueOf(time)); + telemetry.update();// prints above stuff to phone + + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.java new file mode 100644 index 00000000000..971d9a3f6d4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.java @@ -0,0 +1,197 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.matrices.MatrixF; +import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; +//import org.firstinspires.ftc.ftccommon.external.navigation.VuforiaTrackables.RelicRecoveryVuMark; + +import java.util.ArrayList; +import java.util.List; + +/** + * This 2016-2017 OpMode illustrates the basics of using the Vuforia localizer to determine + * positioning and orientation of robot on the FTC field. + * The code is structured as a LinearOpMode + * + * Vuforia uses the phone's camera to inspect it's surroundings, and attempt to locate target images. + * + * When images are located, Vuforia is able to determine the position and orientation of the + * image relative to the camera. This sample code than combines that information with a + * knowledge of where the target images are on the field, to determine the location of the camera. + * + * This example assumes a "diamond" field configuration where the red and blue alliance stations + * are adjacent on the corner of the field furthest from the audience. + * From the Audience perspective, the Red driver station is on the right. + * The two vision target are located on the two walls closest to the audience, facing in. + * The Stones are on the RED side of the field, and the Chips are on the Blue side. + * + * A final calculation then uses the location of the camera on the robot to determine the + * robot's location and orientation on the field. + * + * @see VuforiaLocalizer + * @see VuforiaTrackableDefaultListener + * see ftc_app/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list. + * + * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as + * is explained below. + */ + +@Autonomous(name="VisionTest", group ="Concept") +public class VisionTest extends LinearOpMode { + private DcMotor ADrive; + private DcMotor BDrive; + private DcMotor CDrive; + public static final String TAG = "Vuforia Navigation Sample"; + + OpenGLMatrix lastLocation = null; + + /** + * {@link #vuforia} is the variable we will use to store our instance of the Vuforia + * localization engine. + */ + VuforiaLocalizer vuforia; + + @Override public void runOpMode() { + /* + * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone); + * If no camera monitor is desired, use the parameterless constructor instead (commented out below). + */ + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); + ADrive = hardwareMap.get(DcMotor.class, "A"); + BDrive = hardwareMap.get(DcMotor.class, "B"); + CDrive = hardwareMap.get(DcMotor.class, "C"); + + /* + * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which + * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function. + * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer + * web site at https://developer.vuforia.com/license-manager. + * + * Vuforia license keys are always 380 characters long, and look as if they contain mostly + * random data. As an example, here is a example of a fragment of a valid key: + * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ... + * Once you've obtained a license key, copy the string from the Vuforia web site + * and paste it in to your code onthe next line, between the double quotes. + */ + parameters.vuforiaLicenseKey = "AaPSQTf/////AAABmSC/czqVrkugjRHMuUPfIJ0G0Ew2pPxpfDhUTwNDDO3ZH4ZJ5tTwd+z0MV5DJOT0zc5TBJtsyyYucFpdGvJzHeiT/+XudkoYbayPMUsXox/Ql7dnAx0/fMJYYzR5uaiafhyqZNRVJICBkRFCd4xcT+7s+1jp/OCZT3kizHMtgyvO4KNgGF/UOzRt5GOnu4IiALplY+0ppb21RR8p7t9v6NXbKrhHUTSw0Wsx4OAvbFwsnrxcxOn7lfohME/rb8wZcMCsdwrPSrk+NGPmQBZILXL78eK1dqDVdWv0NYosY9TgNhqGodJAhteozrvuDBHGBn2PYHRBLKKI1Un/Yz075NQBT9yi+Z007APlAH/UVsP4"; + + /* + * We also indicate which camera on the RC that we wish to use. + * Here we chose the back (HiRes) camera (for greater range), but + * for a competition robot, the front camera might be more convenient. + */ + parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK; + this.vuforia = ClassFactory.createVuforiaLocalizer(parameters); + + /** + * Load the data sets that for the trackable objects we wish to track. These particular data + * sets are stored in the 'assets' part of our application (you'll see them in the Android + * Studio 'Project' view over there on the left of the screen). You can make your own datasets + * with the Vuforia Target Manager: https://developer.vuforia.com/target-manager. PDFs for the + * example "StonesAndChips", datasets can be found in in this project in the + * documentation directory. + */ + VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark"); + VuforiaTrackable relicTemplate = relicTrackables.get(0); + relicTemplate.setName("relicVuMarkTemplate"); // can help in debugging; otherwise not necessary + + telemetry.addData(">", "Press Play to start"); telemetry.update(); + waitForStart(); + relicTrackables.activate(); + + + /** + * We use units of mm here because that's the recommended units of measurement for the + * size values specified in the XML for the ImageTarget trackables in data sets. E.g.: + * + * You don't *have to* use mm here, but the units here and the units used in the XML + * target configuration files *must* correspond for the math to work out correctly. + */ + float mmPerInch = 25.4f; + float mmBotWidth = 18 * mmPerInch; // ... or whatever is right for your robot + float mmFTCFieldWidth = (12*12 - 2) * mmPerInch; // the FTC field is ~11'10" center-to-center of the glass panels + + + /** Wait for the game to begin */ + telemetry.addData(">", "Press Play to start tracking"); + telemetry.update(); + waitForStart(); + + /** Start tracking the data sets we care about. */ + relicTrackables.activate(); + double count = 0; + while (opModeIsActive()) { + count = count + 1; + RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate); + if (vuMark != RelicRecoveryVuMark.UNKNOWN) { + /* Found an instance of the template. In the actual game, you will probably + * loop until this condition occurs, then move on to act accordingly depending + * on which VuMark was visible. */ + telemetry.addData("VuMark", "%s visible", vuMark); + ADrive.setPower(0); + BDrive.setPower(.25); + CDrive.setPower(-.25); + + } + else { + + ADrive.setPower(.1); + BDrive.setPower(.1); + CDrive.setPower(.1); + } + + telemetry.update(); + } + } + +} + +//super helpful website https://www.firstinspires.org/sites/default/files/uploads/resource_library/ftc/using-vumarks.pdf \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Visionstonesnchips.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Visionstonesnchips.java new file mode 100644 index 00000000000..88a7e0faabc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Visionstonesnchips.java @@ -0,0 +1,332 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.matrices.MatrixF; +import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; + +import java.util.ArrayList; +import java.util.List; + +/** + * This 2016-2017 OpMode illustrates the basics of using the Vuforia localizer to determine + * positioning and orientation of robot on the FTC field. + * The code is structured as a LinearOpMode + * + * Vuforia uses the phone's camera to inspect it's surroundings, and attempt to locate target images. + * + * When images are located, Vuforia is able to determine the position and orientation of the + * image relative to the camera. This sample code than combines that information with a + * knowledge of where the target images are on the field, to determine the location of the camera. + * + * This example assumes a "diamond" field configuration where the red and blue alliance stations + * are adjacent on the corner of the field furthest from the audience. + * From the Audience perspective, the Red driver station is on the right. + * The two vision target are located on the two walls closest to the audience, facing in. + * The Stones are on the RED side of the field, and the Chips are on the Blue side. + * + * A final calculation then uses the location of the camera on the robot to determine the + * robot's location and orientation on the field. + * + * @see VuforiaLocalizer + * @see VuforiaTrackableDefaultListener + * see ftc_app/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list. + * + * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as + * is explained below. + */ + +@Autonomous(name="Concept: Vuforia Navigation", group ="Concept") + +public class VisionStonesNChips extends LinearOpMode { + + public static final String TAG = "Vuforia Navigation Sample"; + + OpenGLMatrix lastLocation = null; + + /** + * {@link #vuforia} is the variable we will use to store our instance of the Vuforia + * localization engine. + */ + VuforiaLocalizer vuforia; + + @Override public void runOpMode() { + /* + * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone); + * If no camera monitor is desired, use the parameterless constructor instead (commented out below). + */ + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); + + // OR... Do Not Activate the Camera Monitor View, to save power + // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + + /* + * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which + * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function. + * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer + * web site at https://developer.vuforia.com/license-manager. + * + * Vuforia license keys are always 380 characters long, and look as if they contain mostly + * random data. As an example, here is a example of a fragment of a valid key: + * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ... + * Once you've obtained a license key, copy the string from the Vuforia web site + * and paste it in to your code onthe next line, between the double quotes. + */ + parameters.vuforiaLicenseKey = "ATsODcD/////AAAAAVw2lR...d45oGpdljdOh5LuFB9nDNfckoxb8COxKSFX"; + + /* + * We also indicate which camera on the RC that we wish to use. + * Here we chose the back (HiRes) camera (for greater range), but + * for a competition robot, the front camera might be more convenient. + */ + parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK; + this.vuforia = ClassFactory.createVuforiaLocalizer(parameters); + + /** + * Load the data sets that for the trackable objects we wish to track. These particular data + * sets are stored in the 'assets' part of our application (you'll see them in the Android + * Studio 'Project' view over there on the left of the screen). You can make your own datasets + * with the Vuforia Target Manager: https://developer.vuforia.com/target-manager. PDFs for the + * example "StonesAndChips", datasets can be found in in this project in the + * documentation directory. + */ + VuforiaTrackables stonesAndChips = this.vuforia.loadTrackablesFromAsset("StonesAndChips"); + VuforiaTrackable redTarget = stonesAndChips.get(0); + redTarget.setName("RedTarget"); // Stones + + VuforiaTrackable blueTarget = stonesAndChips.get(1); + blueTarget.setName("BlueTarget"); // Chips + + /** For convenience, gather together all the trackable objects in one easily-iterable collection */ + List allTrackables = new ArrayList(); + allTrackables.addAll(stonesAndChips); + + /** + * We use units of mm here because that's the recommended units of measurement for the + * size values specified in the XML for the ImageTarget trackables in data sets. E.g.: + * + * You don't *have to* use mm here, but the units here and the units used in the XML + * target configuration files *must* correspond for the math to work out correctly. + */ + float mmPerInch = 25.4f; + float mmBotWidth = 18 * mmPerInch; // ... or whatever is right for your robot + float mmFTCFieldWidth = (12*12 - 2) * mmPerInch; // the FTC field is ~11'10" center-to-center of the glass panels + + /** + * In order for localization to work, we need to tell the system where each target we + * wish to use for navigation resides on the field, and we need to specify where on the robot + * the phone resides. These specifications are in the form of transformation matrices. + * Transformation matrices are a central, important concept in the math here involved in localization. + * See Transformation Matrix + * for detailed information. Commonly, you'll encounter transformation matrices as instances + * of the {@link OpenGLMatrix} class. + * + * For the most part, you don't need to understand the details of the math of how transformation + * matrices work inside (as fascinating as that is, truly). Just remember these key points: + *
    + * + *
  1. You can put two transformations together to produce a third that combines the effect of + * both of them. If, for example, you have a rotation transform R and a translation transform T, + * then the combined transformation matrix RT which does the rotation first and then the translation + * is given by {@code RT = T.multiplied(R)}. That is, the transforms are multiplied in the + * reverse of the chronological order in which they applied.
    2. + * + *
  2. A common way to create useful transforms is to use methods in the {@link OpenGLMatrix} + * class and the Orientation class. See, for example, {@link OpenGLMatrix#translation(float, + * float, float)}, {@link OpenGLMatrix#rotation(AngleUnit, float, float, float, float)}, and + * {@link Orientation#getRotationMatrix(AxesReference, AxesOrder, AngleUnit, float, float, float)}. + * Related methods in {@link OpenGLMatrix}, such as {@link OpenGLMatrix#rotated(AngleUnit, + * float, float, float, float)}, are syntactic shorthands for creating a new transform and + * then immediately multiplying the receiver by it, which can be convenient at times.
    3. + * + *
  3. If you want to break open the black box of a transformation matrix to understand + * what it's doing inside, use {@link MatrixF#getTranslation()} to fetch how much the + * transform will move you in x, y, and z, and use {@link Orientation#getOrientation(MatrixF, + * AxesReference, AxesOrder, AngleUnit)} to determine the rotational motion that the transform + * will impart. See {@link #format(OpenGLMatrix)} below for an example.
    4. + * + *
+ * + * This example places the "stones" image on the perimeter wall to the Left + * of the Red Driver station wall. Similar to the Red Beacon Location on the Res-Q + * + * This example places the "chips" image on the perimeter wall to the Right + * of the Blue Driver station. Similar to the Blue Beacon Location on the Res-Q + * + * See the doc folder of this project for a description of the field Axis conventions. + * + * Initially the target is conceptually lying at the origin of the field's coordinate system + * (the center of the field), facing up. + * + * In this configuration, the target's coordinate system aligns with that of the field. + * + * In a real situation we'd also account for the vertical (Z) offset of the target, + * but for simplicity, we ignore that here; for a real robot, you'll want to fix that. + * + * To place the Stones Target on the Red Audience wall: + * - First we rotate it 90 around the field's X axis to flip it upright + * - Then we rotate it 90 around the field's Z access to face it away from the audience. + * - Finally, we translate it back along the X axis towards the red audience wall. + */ + OpenGLMatrix redTargetLocationOnField = OpenGLMatrix + /* Then we translate the target off to the RED WALL. Our translation here + is a negative translation in X.*/ + .translation(-mmFTCFieldWidth/2, 0, 0) + .multiplied(Orientation.getRotationMatrix( + /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */ + AxesReference.EXTRINSIC, AxesOrder.XZX, + AngleUnit.DEGREES, 90, 90, 0)); + redTarget.setLocation(redTargetLocationOnField); + RobotLog.ii(TAG, "Red Target=%s", format(redTargetLocationOnField)); + + /* + * To place the Stones Target on the Blue Audience wall: + * - First we rotate it 90 around the field's X axis to flip it upright + * - Finally, we translate it along the Y axis towards the blue audience wall. + */ + OpenGLMatrix blueTargetLocationOnField = OpenGLMatrix + /* Then we translate the target off to the Blue Audience wall. + Our translation here is a positive translation in Y.*/ + .translation(0, mmFTCFieldWidth/2, 0) + .multiplied(Orientation.getRotationMatrix( + /* First, in the fixed (field) coordinate system, we rotate 90deg in X */ + AxesReference.EXTRINSIC, AxesOrder.XZX, + AngleUnit.DEGREES, 90, 0, 0)); + blueTarget.setLocation(blueTargetLocationOnField); + RobotLog.ii(TAG, "Blue Target=%s", format(blueTargetLocationOnField)); + + /** + * Create a transformation matrix describing where the phone is on the robot. Here, we + * put the phone on the right hand side of the robot with the screen facing in (see our + * choice of BACK camera above) and in landscape mode. Starting from alignment between the + * robot's and phone's axes, this is a rotation of -90deg along the Y axis. + * + * When determining whether a rotation is positive or negative, consider yourself as looking + * down the (positive) axis of rotation from the positive towards the origin. Positive rotations + * are then CCW, and negative rotations CW. An example: consider looking down the positive Z + * axis towards the origin. A positive rotation about Z (ie: a rotation parallel to the the X-Y + * plane) is then CCW, as one would normally expect from the usual classic 2D geometry. + */ + OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix + .translation(mmBotWidth/2,0,0) + .multiplied(Orientation.getRotationMatrix( + AxesReference.EXTRINSIC, AxesOrder.YZY, + AngleUnit.DEGREES, -90, 0, 0)); + RobotLog.ii(TAG, "phone=%s", format(phoneLocationOnRobot)); + + /** + * Let the trackable listeners we care about know where the phone is. We know that each + * listener is a {@link VuforiaTrackableDefaultListener} and can so safely cast because + * we have not ourselves installed a listener of a different type. + */ + ((VuforiaTrackableDefaultListener)redTarget.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); + ((VuforiaTrackableDefaultListener)blueTarget.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); + + /** + * A brief tutorial: here's how all the math is going to work: + * + * C = phoneLocationOnRobot maps phone coords -> robot coords + * P = tracker.getPose() maps image target coords -> phone coords + * L = redTargetLocationOnField maps image target coords -> field coords + * + * So + * + * C.inverted() maps robot coords -> phone coords + * P.inverted() maps phone coords -> imageTarget coords + * + * Putting that all together, + * + * L x P.inverted() x C.inverted() maps robot coords to field coords. + * + * @see VuforiaTrackableDefaultListener#getRobotLocation() + */ + + /** Wait for the game to begin */ + telemetry.addData(">", "Press Play to start tracking"); + telemetry.update(); + waitForStart(); + + /** Start tracking the data sets we care about. */ + stonesAndChips.activate(); + + while (opModeIsActive()) { + + for (VuforiaTrackable trackable : allTrackables) { + /** + * getUpdatedRobotLocation() will return null if no new information is available since + * the last time that call was made, or if the trackable is not currently visible. + * getRobotLocation() will return null if the trackable is not currently visible. + */ + telemetry.addData(trackable.getName(), ((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible() ? "Visible" : "Not Visible"); // + + OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation(); + if (robotLocationTransform != null) { + lastLocation = robotLocationTransform; + } + } + /** + * Provide feedback as to where the robot was last located (if we know). + */ + if (lastLocation != null) { + // RobotLog.vv(TAG, "robot=%s", format(lastLocation)); + telemetry.addData("Pos", format(lastLocation)); + } else { + telemetry.addData("Pos", "Unknown"); + } + telemetry.update(); + } + } + + /** + * A simple utility that extracts positioning information from a transformation matrix + * and formats it in a form palatable to a human being. + */ + String format(OpenGLMatrix transformationMatrix) { + return transformationMatrix.formatAsTransform(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java new file mode 100644 index 00000000000..55f79fecb46 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java @@ -0,0 +1,116 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.IntegratingGyroscope; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +import static java.lang.Math.abs; +import static java.lang.Math.cos; +import static java.lang.Math.sin; + +/** + * Created by jxfio on 11/17/2017. + */ + +@TeleOp(name="robot2", group="robot2") +public class robot2 extends LinearOpMode { + + // Declare OpMode members. + private DcMotor Left; + private DcMotor Right; + private DcMotor Tower; + private CRServo Slack; + private Servo RF; + private Servo LF; + @Override + public void runOpMode() { + + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + Tower = hardwareMap.get(DcMotor.class, "tower"); + Slack = hardwareMap.get(CRServo.class, "slack"); + RF = hardwareMap.get(Servo.class, "right finger"); + LF = hardwareMap.get(Servo.class, "left finger"); + + waitForStart(); + // run until the end of the match (driver presses STOP) + boolean limitoff=false; + boolean y = false; + double Fingeroffset = 0; + boolean b = false; + boolean x = false; + double factor=1; + double bonus = 0; + double position = 0; + double towerpower; + double max = 4416; + while (opModeIsActive()) { + bonus = 0; + towerpower = -gamepad2.left_stick_y; + /*if(towerpower>0){ + if (position>0&&position0&&positionmax && !limitoff ){ + towerpower = -1; + } + /* + //towerpower = (bonus+factor*towerpower);*/ + //towerpower += bonus; + Left.setPower(.5*Range.clip(gamepad1.left_stick_y, -1, 1)); + Right.setPower(.5*Range.clip(-gamepad1.right_stick_y, -1, 1)); + Tower.setPower(.25 * Range.clip(towerpower, -1, 1)); + Slack.setPower(-.25 *Range.clip((towerpower), -1, 1)); + //finger adjustment statements + if(gamepad2.b && !b && Fingeroffset <= .4){ + Fingeroffset += .1; + } + if(gamepad2.x && !x && Fingeroffset >= .1 ){ + Fingeroffset -= .1; + } + RF.setPosition(Range.clip(0.5 - Fingeroffset, -1,1)); + LF.setPosition(Range.clip(0.5 + Fingeroffset, -1,1)); + x = gamepad2.x; + b = gamepad2.b; + //Finger controls + if (limitoff){ + telemetry.addData("path 0","limits off"); + }else { + telemetry.addData("path 0","limits on"); + } + telemetry.addData("Path1","Tower position: " + String.valueOf(Tower.getCurrentPosition())+" factor: " + String.valueOf(factor)+"bonus: " + String.valueOf(bonus) + "tower power: " + String.valueOf(towerpower)+ " position: " + String.valueOf(position)); + telemetry.addData("Path2","Finger offset: " + String.valueOf(Fingeroffset) + " y: "+ String.valueOf(y)); + telemetry.update(); + } + } +} diff --git a/build.gradle b/build.gradle index e13ce162e19..491c8452273 100644 --- a/build.gradle +++ b/build.gradle @@ -9,7 +9,7 @@ buildscript { jcenter() } dependencies { - classpath 'com.android.tools.build:gradle:3.1.3' + classpath 'com.android.tools.build:gradle:3.2.1' } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index b6517bb1d16..e0ed7a2690e 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,6 @@ +#Sun Dec 09 14:38:58 PST 2018 distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-4.4-all.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-4.6-all.zip diff --git a/libs/Jama-1.0.3.jar b/libs/Jama-1.0.3.jar new file mode 100644 index 00000000000..c8007594cb5 Binary files /dev/null and b/libs/Jama-1.0.3.jar differ