| @@ -0,0 +1,111 @@ | ||
| package com.qualcomm.ftcrobotcontroller.NewStructure; | ||
|
|
||
| import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; | ||
|
|
||
| /** | ||
| * Created by ethan on 4/8/2016. | ||
| */ | ||
| public class NewAutonomous extends LinearOpMode { | ||
|
|
||
| AutoBot autoBot; | ||
| int side = 1; | ||
|
|
||
| @Override | ||
| public void runOpMode() throws InterruptedException { | ||
| autoBot = new AutoBot(side, this); | ||
|
|
||
| waitForStart(); | ||
| autoBot.start(); | ||
| sleep(100+ autoBot.getDelay()); | ||
| if (autoBot.isStartNearRamp()) { //near ramp position | ||
| autoBot.drive(1600, .7, 0); | ||
| autoBot.pivot(38.5, 1, 0.5); | ||
| autoBot.drive(4500, 1, 0); | ||
| } else { //far ramp position | ||
| autoBot.drive(1600, .7, 0); | ||
| autoBot.pivot(52.5, 1, 0.5); | ||
| autoBot.drive(6500, 1, 0); | ||
| } | ||
| //autoBot.newGyroTurn();(42, 1); | ||
| autoBot.drive(2000, .2, 1); //drives to white line | ||
| autoBot.drive(60, -0.2, 0); | ||
| autoBot.newGyroTurn(90, 2); | ||
| // stopMotors(); | ||
| //debrisCounter.interrupt(); | ||
| autoBot.collector.setPower(0); | ||
| autoBot.cameraController.startBackCam(); | ||
| autoBot. beaconR.setPosition(0); | ||
| autoBot.beaconL.setPosition(1); | ||
| autoBot.driveUntilUltra(15, 0.1, 1200); //drives until 15 cm from wall | ||
| autoBot.drive(200, .2,0); | ||
| autoBot.climberDumper.setPosition(1); //dumps climbers | ||
| sleep(2000); | ||
| autoBot.climberDumper.setPosition(0.4); | ||
| telemetry.addData("before drive after climbers", ""); | ||
| autoBot.drive(500, -0.1, 0); | ||
| autoBot.climberDumper.setPosition(0.5); | ||
| telemetry.addData("after drive before beacon", ""); | ||
| if (autoBot.isTriggerBeacon()) { //goes to trigger beacon | ||
| sleep(100); | ||
| telemetry.addData("before camera call",""); | ||
| int leftred = autoBot.cameraController.getLeftRed();//read image | ||
| telemetry.addData("after left camera call", ""); | ||
| int rightred = autoBot.cameraController.getRightRed(); | ||
|
|
||
| telemetry.addData("Colors", "Left " + leftred / 1000 + " Right: " + rightred / 1000); | ||
| if (leftred > rightred){ //left side is red | ||
| if (side == -1) { //on red team | ||
| autoBot.beaconL.setPosition(0.6); | ||
| } else { //on blue team | ||
| autoBot.beaconR.setPosition(0.3); | ||
| } | ||
| } else { //right side is red | ||
| if (side == -1) { //on red team | ||
| autoBot.beaconR.setPosition(0.3); | ||
| } else { //on blue team | ||
| autoBot.beaconL.setPosition(0.6); | ||
| } | ||
| } | ||
| sleep(100); | ||
| } | ||
| telemetry.addData("beacon check", ""); | ||
| autoBot.driveUntilUltra(15, 0.1, 200); //presses buttons | ||
| autoBot.drive(50, 0.2, 0); | ||
| autoBot.drive(80, -0.2, 0); | ||
|
|
||
| autoBot.beaconR.setPosition(.7); | ||
| autoBot.beaconL.setPosition(.2); | ||
| //autoBot.drive(40, 0.2,0); | ||
| if (autoBot.isGoToRamp()) { //goes to ramp | ||
| if (side==1){ | ||
| autoBot.sideArms.setSideArmLpos(0.5f); | ||
| } | ||
| else { | ||
| autoBot.sideArms.setSideArmLpos(0.3f); | ||
| } | ||
| autoBot.drive(1000, -0.25, 0); | ||
| autoBot.pivot(200, 1, 2); | ||
| autoBot.collector.setPower(-1); | ||
| autoBot.drive(2200, 1, 0); | ||
| autoBot.newGyroTurn(-45, 2); | ||
|
|
||
|
|
||
| autoBot.arm.setArmPower(-1); | ||
| autoBot.drive(1000, -1,0); | ||
| autoBot.arm.setArmPower(0); | ||
| if(side == -1){ | ||
| autoBot.sideArms.setSideArmLpos(0); | ||
| } | ||
| else{ | ||
| autoBot.sideArms.setSideArmRpos(1); | ||
| } | ||
| autoBot.drive(5000, -1, 0); | ||
| } else { //goes into place next to ramp | ||
| autoBot.newGyroTurn(180,2); | ||
| autoBot.drive(1500, 1, 0); | ||
| } | ||
|
|
||
| } | ||
| } | ||
|
|
||
|
|
| @@ -0,0 +1,27 @@ | ||
| package com.qualcomm.ftcrobotcontroller.NewStructure; | ||
|
|
||
| import com.qualcomm.robotcore.eventloop.opmode.OpMode; | ||
|
|
||
| /** | ||
| * Created by ethan on 4/8/2016. | ||
| */ | ||
| public class NewTeleop extends OpMode { | ||
|
|
||
| TeleOpBot teleOpBot; | ||
|
|
||
| @Override | ||
| public void init() { | ||
| teleOpBot = new TeleOpBot(-1, this); | ||
| } | ||
|
|
||
| @Override | ||
| public void loop() { | ||
|
|
||
| teleOpBot.functloop(); | ||
|
|
||
| } | ||
|
|
||
| public void start(){ | ||
| teleOpBot.startBot(); | ||
| } | ||
| } |
| @@ -0,0 +1,156 @@ | ||
| package com.qualcomm.ftcrobotcontroller.NewStructure.Parts; | ||
|
|
||
| import com.qualcomm.ftcrobotcontroller.BrainstormersOpmodes.AdafruitIMUmethods; | ||
| import com.qualcomm.robotcore.eventloop.opmode.OpMode; | ||
| import com.qualcomm.robotcore.hardware.DcMotor; | ||
| import com.qualcomm.robotcore.hardware.DcMotorController; | ||
| import com.qualcomm.robotcore.hardware.Servo; | ||
|
|
||
| /** | ||
| * Created by ethan on 4/5/2016. | ||
| */ | ||
| public class Arm { | ||
| OpMode opMode; | ||
| DcMotor pullUp1; | ||
| DcMotor pullUp2; | ||
| DcMotor armAngleMotor; | ||
| Servo lock1; | ||
| Servo lock2; | ||
|
|
||
| boolean wasDown=false; | ||
| boolean lockDown=false; | ||
|
|
||
|
|
||
| public Arm (OpMode varopmode ){ | ||
| opMode =varopmode; | ||
| pullUp1 = opMode.hardwareMap.dcMotor.get("pullUp1"); | ||
| pullUp2 = opMode.hardwareMap.dcMotor.get("pullUp2"); | ||
| armAngleMotor = opMode.hardwareMap.dcMotor.get("ext"); | ||
| lock1 = opMode.hardwareMap.servo.get("lock1"); | ||
| lock2 = opMode.hardwareMap.servo.get("lock2"); | ||
|
|
||
|
|
||
|
|
||
| } | ||
|
|
||
| public void runArmAngleEncoders(){ | ||
| armAngleMotor.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS); | ||
| } | ||
| public void resetArmAngleEncoders(){ | ||
| armAngleMotor.setMode(DcMotorController.RunMode.RESET_ENCODERS); | ||
| } | ||
|
|
||
| public void runArmEncoders(){ | ||
| pullUp2.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS); | ||
| pullUp1.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS); | ||
| } | ||
| public void resetArmEncoders(){ | ||
| pullUp1.setMode(DcMotorController.RunMode.RESET_ENCODERS); | ||
| pullUp2.setMode(DcMotorController.RunMode.RESET_ENCODERS); | ||
| } | ||
|
|
||
| public void setArmPower(float power){ | ||
| pullUp1.setPower(power); | ||
| pullUp2.setPower(-power); | ||
|
|
||
| } | ||
| public void setArmAnglePower(float power){ | ||
| armAngleMotor.setPower(power); | ||
| } | ||
| public void setLockDown(){ | ||
| lock1.setPosition(0); | ||
| lock2.setPosition(1); | ||
| } | ||
| public void setLockUp(){ | ||
| lock1.setPosition(1); | ||
| lock2.setPosition(0); | ||
| } | ||
|
|
||
| public float getArmAnglePos(){ | ||
| return armAngleMotor.getCurrentPosition(); | ||
| } | ||
| public float getArmPos(){ | ||
| return pullUp1.getCurrentPosition(); | ||
| } | ||
| public double getLockPos(){ | ||
| return lock2.getPosition(); | ||
| } | ||
|
|
||
|
|
||
|
|
||
| public void startTeleArm(){ | ||
|
|
||
| resetArmAngleEncoders(); | ||
| runArmAngleEncoders(); | ||
| resetArmEncoders(); | ||
| runArmAngleEncoders(); | ||
|
|
||
| lock1.setPosition(1); | ||
| lock2.setPosition(0); | ||
| } | ||
|
|
||
|
|
||
| public void angleArm() { | ||
| if (!opMode.gamepad1.y) { | ||
| if ((opMode.gamepad2.left_stick_y > .03 && getArmAnglePos()<0) || opMode.gamepad2.a) { | ||
| setArmAnglePower(1); | ||
| } else if (opMode.gamepad2.left_stick_y < -.03) { | ||
| setArmAnglePower(-1); | ||
| } else { | ||
| setArmAnglePower(0); | ||
| } | ||
| } | ||
| } | ||
|
|
||
|
|
||
| public void armControl( AdafruitIMUmethods gyro,double gyroOffset, DcMotor fr) { | ||
| if (opMode.gamepad1.y) { | ||
| if (opMode.gamepad1.dpad_down && !wasDown) { | ||
| wasDown = true; | ||
| lockDown = !lockDown; | ||
| } | ||
| if (!opMode.gamepad1.dpad_down && wasDown) { | ||
| wasDown = false; | ||
| } | ||
| if (lockDown) { | ||
| setLockDown(); | ||
| } else { | ||
| setLockUp(); | ||
| } | ||
|
|
||
| } | ||
| else if (opMode.gamepad2.left_trigger != 0) { | ||
| setArmPower(opMode.gamepad2.left_trigger); | ||
| } | ||
| else if ((opMode.gamepad1.right_trigger==1 || (gyro.getRoll() + gyroOffset) > 2 )&& fr.getPower()>0) { | ||
| if (Math.abs(getArmPos()) < 2500 ) { | ||
|
|
||
| setArmPower(-1); | ||
| } else { | ||
| setArmPower(0); | ||
| } | ||
| } | ||
| else if (opMode.gamepad2.right_trigger != 0) { | ||
| setArmPower(-opMode.gamepad2.right_trigger); | ||
| } else if (!opMode.gamepad1.y) { | ||
| setArmPower(0); | ||
| } | ||
| } | ||
| public void hang() { | ||
|
|
||
| if (opMode.gamepad1.y) { | ||
| setArmPower(1); | ||
| if (getArmAnglePos() < 0) { | ||
| setArmAnglePower(1); | ||
| } else { | ||
| setArmAnglePower(0); | ||
| } | ||
| } | ||
| } | ||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
| } |
| @@ -0,0 +1,133 @@ | ||
| package com.qualcomm.ftcrobotcontroller.NewStructure.Parts; | ||
|
|
||
| import com.qualcomm.robotcore.eventloop.opmode.OpMode; | ||
| import com.qualcomm.robotcore.hardware.Servo; | ||
|
|
||
| /** | ||
| * Created by ethan on 4/5/2016. | ||
| */ | ||
| public class Dumper { | ||
| Servo doorR; | ||
| Servo doorL; | ||
| Servo dumpingBlock; | ||
| int side; | ||
| OpMode opMode; | ||
|
|
||
| final int RED = -1; | ||
| final int BLUE = 1; | ||
|
|
||
| public Dumper(int side , OpMode varopmode){ | ||
| this.side = side; | ||
| opMode = varopmode; | ||
| dumpingBlock = opMode.hardwareMap.servo.get("dumper"); | ||
| doorR = opMode.hardwareMap.servo.get("doorR"); | ||
| doorL = opMode.hardwareMap.servo.get("doorL"); | ||
|
|
||
| } | ||
|
|
||
| public void startDumper(){ | ||
|
|
||
| doorR.setPosition(0.85); | ||
| doorL.setPosition(0.15); | ||
| dumpingBlock.setPosition(0.5); | ||
| } | ||
|
|
||
| int moveCount = 0; | ||
| int oldCount = 51; | ||
| int moveCountAllClear = 0; | ||
| int oldCountAllClear = 51; | ||
|
|
||
| final int MINCOUNT = 51; | ||
| final int MAXMOVECOUNT = 250; | ||
|
|
||
| public void resetDumpingBlock(){ | ||
| if (side==-1){ | ||
| dumpingBlock.setPosition(0.35); | ||
| } else { | ||
| dumpingBlock.setPosition(0.55); | ||
| } | ||
| } | ||
|
|
||
|
|
||
| /** | ||
| * configures our dumping system based on what team we're on | ||
| */ | ||
| public void dumping() { | ||
| opMode.telemetry.addData("Moving", "move: " + moveCount + "old: " + oldCount); | ||
| if (side == BLUE) { | ||
| if (opMode.gamepad2.dpad_right || opMode.gamepad2.right_stick_x > .15) { | ||
| doorR.setPosition(0.3); | ||
| } | ||
| else { | ||
| doorR.setPosition(0.85); | ||
| doorL.setPosition(0.15); | ||
| } | ||
| } | ||
| else if (side == RED) { | ||
| if (opMode.gamepad2.dpad_left || opMode.gamepad2.right_stick_x < -.15) { | ||
| doorL.setPosition(0.7); | ||
| } | ||
| else { | ||
| doorR.setPosition(0.85); | ||
| doorL.setPosition(0.15); | ||
| } | ||
| } | ||
|
|
||
|
|
||
| if (side == BLUE) { | ||
| if (opMode.gamepad2.dpad_right) { | ||
| dumpingBlock.setPosition(0); | ||
| moveCount++; | ||
|
|
||
| if(moveCount > MAXMOVECOUNT){ | ||
| moveCount = MAXMOVECOUNT; | ||
| } | ||
|
|
||
| oldCount = moveCount; | ||
| } else if (opMode.gamepad2.dpad_left) { | ||
| dumpingBlock.setPosition(1); | ||
|
|
||
| } | ||
| else { | ||
| if(moveCount > 0 && opMode.gamepad2.right_stick_x < .15){ | ||
| dumpingBlock.setPosition(1); | ||
| moveCount--; | ||
| } | ||
| else { | ||
| dumpingBlock.setPosition(0.5); | ||
| oldCount = MINCOUNT; | ||
| } | ||
| } | ||
|
|
||
|
|
||
| } else if(side == RED) { | ||
| if (opMode.gamepad2.dpad_left) { | ||
| moveCount++; | ||
| dumpingBlock.setPosition(1); | ||
|
|
||
| if(moveCount > MAXMOVECOUNT){ | ||
| moveCount = MAXMOVECOUNT; | ||
| } | ||
| oldCount = moveCount; | ||
| } | ||
| else if (opMode.gamepad2.dpad_right) { | ||
| dumpingBlock.setPosition(0); | ||
| } | ||
| else { | ||
| if(moveCount > 0 && opMode.gamepad2.right_stick_x > -.15){ | ||
| dumpingBlock.setPosition(0); | ||
| moveCount--; | ||
| } | ||
| else { | ||
| dumpingBlock.setPosition(0.5); | ||
| oldCount = MINCOUNT; | ||
| } | ||
| } | ||
| } | ||
| } | ||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
| } |
| @@ -0,0 +1,73 @@ | ||
| package com.qualcomm.ftcrobotcontroller.NewStructure.Parts; | ||
|
|
||
| import com.qualcomm.ftcrobotcontroller.BrainstormersOpmodes.AdafruitIMUmethods; | ||
| import com.qualcomm.robotcore.eventloop.opmode.OpMode; | ||
| import com.qualcomm.robotcore.hardware.DcMotor; | ||
| import com.qualcomm.robotcore.hardware.Servo; | ||
|
|
||
| /** | ||
| * Created by ethan on 4/5/2016. | ||
| */ | ||
| public class SideArms { | ||
| Servo sideArmL; | ||
| Servo sideArmR; | ||
| OpMode opMode; | ||
| int side; | ||
|
|
||
|
|
||
| public SideArms (int side,OpMode varopMode ){ | ||
|
|
||
| opMode = varopMode; | ||
| this.side = side; | ||
| sideArmL = opMode.hardwareMap.servo.get("sideArmL"); | ||
| sideArmR = opMode.hardwareMap.servo.get("sideArmR"); | ||
| } | ||
|
|
||
| public void initSideArms() { | ||
|
|
||
| sideArmL.setPosition(0.8); | ||
| sideArmR.setPosition(0.05); | ||
| } | ||
|
|
||
| public void sideArm( AdafruitIMUmethods gyro,double gyroOffset, DcMotor fr){ | ||
| if (fr.getPower()>0) { | ||
| if (side == 1) { | ||
| if (opMode.gamepad1.right_trigger != 0 || (gyro.getRoll() + gyroOffset) > 3.5 || opMode.gamepad1.y) { | ||
| sideArmL.setPosition(0.8); | ||
| sideArmR.setPosition(1); | ||
| } | ||
| else { | ||
| sideArmL.setPosition(0.8); | ||
| sideArmR.setPosition(0.05); | ||
| } | ||
| } | ||
| else if (side == -1) { | ||
| if (opMode.gamepad1.right_trigger != 0 || (gyro.getRoll() + gyroOffset) > 3.5 || opMode.gamepad1.y) { | ||
| sideArmL.setPosition(0); | ||
| sideArmR.setPosition(0.05); | ||
| } | ||
| else { | ||
| sideArmL.setPosition(0.8); | ||
| sideArmR.setPosition(0.05); | ||
| } | ||
| } | ||
| } | ||
| else if (opMode.gamepad2.right_trigger !=0 || opMode.gamepad2.left_trigger !=0){ | ||
| sideArmL.setPosition(0.5); | ||
| sideArmR.setPosition(0.5); | ||
| } | ||
| else { | ||
| sideArmL.setPosition(0.8); | ||
| sideArmR.setPosition(0.05); | ||
| } | ||
| } | ||
|
|
||
| public void setSideArmLpos(float pos){ | ||
| sideArmL.setPosition(pos); | ||
| } | ||
| public void setSideArmRpos(float pos){ | ||
| sideArmR.setPosition(pos); | ||
| } | ||
|
|
||
|
|
||
| } |
| @@ -0,0 +1,160 @@ | ||
| package com.qualcomm.ftcrobotcontroller.NewStructure.Parts; | ||
|
|
||
| import com.qualcomm.robotcore.eventloop.opmode.OpMode; | ||
| import com.qualcomm.robotcore.hardware.DcMotor; | ||
| import com.qualcomm.robotcore.hardware.DcMotorController; | ||
| import com.qualcomm.robotcore.util.Range; | ||
|
|
||
| /** | ||
| * Created by ethan on 4/6/2016. | ||
| */ | ||
| public class WheelBase { | ||
| OpMode opMode; | ||
| DcMotor fl; | ||
| DcMotor fr; | ||
| DcMotor br; | ||
| DcMotor bl; | ||
|
|
||
| int fRold; | ||
| int bRold; | ||
| int fLold; | ||
| int bLold; | ||
| int delay=0; | ||
|
|
||
| float frPower; | ||
| float brPower; | ||
| float flPower; | ||
| float blPower; | ||
|
|
||
| float yPower; | ||
| float xPower; | ||
|
|
||
| float driveMod = 1; | ||
|
|
||
|
|
||
| public WheelBase(OpMode varopmode){ | ||
|
|
||
| opMode = varopmode; | ||
|
|
||
| fr = opMode.hardwareMap.dcMotor.get("fr"); | ||
| fl = opMode.hardwareMap.dcMotor.get("fl"); | ||
| br = opMode.hardwareMap.dcMotor.get("br"); | ||
| bl = opMode.hardwareMap.dcMotor.get("bl"); | ||
|
|
||
| } | ||
|
|
||
|
|
||
| public DcMotor getFr(){ | ||
| return fr; | ||
| } | ||
|
|
||
|
|
||
| public void driveBackwards(){ | ||
| fr.setPower(1); | ||
| br.setPower(1); | ||
| fl.setPower(-1); | ||
| bl.setPower(-1); | ||
| } | ||
|
|
||
| public void setInput(float x, float y){ | ||
|
|
||
| yPower = Range.clip(y, -1, 1); | ||
| xPower = Range.clip(x, -1, 1); | ||
|
|
||
| /** | ||
| * combines the rotation and speed together | ||
| */ | ||
| frPower = yPower + xPower; | ||
| brPower = yPower + xPower; | ||
| flPower = -yPower + xPower; | ||
| blPower = -yPower + xPower; | ||
|
|
||
| if (!opMode.gamepad1.y){ | ||
| fr.setPower(Range.clip(frPower, -1, 1) * driveMod); | ||
| br.setPower(Range.clip(brPower, -1, 1) * driveMod); | ||
| fl.setPower(Range.clip(flPower, -1, 1) * driveMod); | ||
| bl.setPower(Range.clip(blPower, -1, 1) * driveMod); | ||
| } | ||
|
|
||
| } | ||
|
|
||
| public void setDriveMod(float driveMod){ | ||
| this.driveMod = driveMod; | ||
|
|
||
| } | ||
|
|
||
| public void resetDriveEncoders() { | ||
|
|
||
| fl.setMode(DcMotorController.RunMode.RESET_ENCODERS); | ||
| fr.setMode(DcMotorController.RunMode.RESET_ENCODERS); | ||
| bl.setMode(DcMotorController.RunMode.RESET_ENCODERS); | ||
| br.setMode(DcMotorController.RunMode.RESET_ENCODERS); | ||
|
|
||
|
|
||
| } | ||
|
|
||
|
|
||
|
|
||
| /** | ||
| * sets all drive encoders to run using encoders mode | ||
| */ | ||
| //Run using Encoders. | ||
| public void runUsingEncoders() { | ||
|
|
||
| fl.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS); | ||
| fr.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS); | ||
| bl.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS); | ||
| br.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS); | ||
| } | ||
| public void setLeftPower(double power) { | ||
| power = Range.clip(power, -1, 1); | ||
| runUsingEncoders(); | ||
| fl.setPower(power); | ||
| bl.setPower(power); | ||
| } | ||
|
|
||
| public void setRightPower(double power) { | ||
| power = Range.clip(power, -1, 1); //because david is trash at building robots and it turns by itself we need to slow this side down, you know it would be so much more effective if you did it in hardware you ass - August | ||
| runUsingEncoders(); | ||
| fr.setPower(-power); | ||
| br.setPower(-power); | ||
| } | ||
|
|
||
| public void superstopMotors() throws InterruptedException { | ||
| int iterations = 0; | ||
| while ((fr.isBusy() || br.isBusy() || fl.isBusy() || bl.isBusy()) && iterations < 10) { | ||
| iterations++; | ||
| opMode.telemetry.addData("motor status", fr.isBusy() + " " + br.isBusy() + " " + fl.isBusy() + " " + bl.isBusy()); | ||
| runUsingEncoders(); | ||
| br.setPower(0); | ||
| bl.setPower(0); | ||
| fl.setPower(0); | ||
| fr.setPower(0); | ||
|
|
||
| } | ||
| opMode.telemetry.addData("stopping", "complete"); | ||
| } | ||
|
|
||
| public void stopMotors(){ | ||
| br.setPower(0); | ||
| bl.setPower(0); | ||
| fl.setPower(0); | ||
| fr.setPower(0); | ||
| } | ||
|
|
||
| public void resetEncoderDelta() { | ||
| fRold = fr.getCurrentPosition(); | ||
| bRold = br.getCurrentPosition(); | ||
| fLold = fl.getCurrentPosition(); | ||
| bLold = bl.getCurrentPosition(); | ||
| } | ||
|
|
||
| public float getBrPos(){ | ||
| return br.getCurrentPosition(); | ||
| } | ||
|
|
||
| public float getBlPos(){ | ||
| return bl.getCurrentPosition(); | ||
| } | ||
|
|
||
| } |
| @@ -0,0 +1,100 @@ | ||
| package com.qualcomm.ftcrobotcontroller.NewStructure; | ||
|
|
||
| import com.qualcomm.ftcrobotcontroller.BrainstormersOpmodes.AdafruitIMUmethods; | ||
| import com.qualcomm.ftcrobotcontroller.BrainstormersOpmodes.BackCameraController; | ||
| import com.qualcomm.ftcrobotcontroller.BrainstormersOpmodes.FrontCameraController; | ||
| import com.qualcomm.ftcrobotcontroller.NewStructure.Parts.Arm; | ||
| import com.qualcomm.ftcrobotcontroller.NewStructure.Parts.Dumper; | ||
| import com.qualcomm.ftcrobotcontroller.NewStructure.Parts.SideArms; | ||
| import com.qualcomm.ftcrobotcontroller.NewStructure.Parts.WheelBase; | ||
| import com.qualcomm.robotcore.eventloop.opmode.OpMode; | ||
| import com.qualcomm.robotcore.hardware.ColorSensor; | ||
| import com.qualcomm.robotcore.hardware.DcMotor; | ||
| import com.qualcomm.robotcore.hardware.Servo; | ||
| import com.qualcomm.robotcore.hardware.UltrasonicSensor; | ||
|
|
||
| /** | ||
| * Created by ethan on 4/5/2016. | ||
| */ | ||
| public class Robot { | ||
| //Drive | ||
|
|
||
|
|
||
| int minpullup = 0; | ||
| int side; | ||
| double gyroOffset=0; | ||
|
|
||
| //Controllers | ||
| AdafruitIMUmethods adaFruitGyro; | ||
| BackCameraController cameraController; | ||
| FrontCameraController frontCam; | ||
| OpMode opMode; | ||
|
|
||
| Arm arm; | ||
| Dumper dumper; | ||
| SideArms sideArms; | ||
| WheelBase wheelBase; | ||
|
|
||
| //Driving Motors | ||
|
|
||
| DcMotor collector; | ||
| ColorSensor colorSensor; | ||
| UltrasonicSensor ultra1; | ||
| UltrasonicSensor ultra2; | ||
|
|
||
| //Servo | ||
| Servo beaconR; | ||
| Servo beaconL; | ||
| Servo allClear; | ||
| Servo climberDumper; | ||
|
|
||
|
|
||
| Servo armHook; | ||
|
|
||
| //Variables | ||
| int turnDirection = 1; | ||
| private boolean didEncodersReset = false; | ||
|
|
||
|
|
||
| public Robot (int side , OpMode varopMode) { | ||
|
|
||
|
|
||
| this.side = side; | ||
| this.opMode = varopMode; | ||
|
|
||
|
|
||
| arm = new Arm(opMode); | ||
| dumper = new Dumper(side, opMode); | ||
| sideArms = new SideArms( side,opMode); | ||
| wheelBase = new WheelBase(opMode); | ||
|
|
||
| //Sensors | ||
| collector = opMode.hardwareMap.dcMotor.get("collect"); | ||
| climberDumper = opMode.hardwareMap.servo.get("climberDumper"); | ||
| beaconR = opMode.hardwareMap.servo.get("beacon right"); | ||
| beaconL = opMode.hardwareMap.servo.get("beacon left"); | ||
|
|
||
|
|
||
| armHook = opMode.hardwareMap.servo.get("armHook"); | ||
| colorSensor = opMode.hardwareMap.colorSensor.get("cs2"); | ||
| ultra1 = opMode.hardwareMap.ultrasonicSensor.get("ultraL"); | ||
| ultra2 = opMode.hardwareMap.ultrasonicSensor.get("ultraR"); | ||
|
|
||
| //Motors | ||
|
|
||
|
|
||
| adaFruitGyro = new AdafruitIMUmethods(opMode); | ||
|
|
||
| allClear = opMode.hardwareMap.servo.get("allClear"); | ||
|
|
||
| } | ||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
| } |
| @@ -0,0 +1,133 @@ | ||
| package com.qualcomm.ftcrobotcontroller.NewStructure; | ||
|
|
||
| import com.qualcomm.robotcore.eventloop.opmode.OpMode; | ||
|
|
||
| /** | ||
| * Created by ethan on 4/5/2016. | ||
| */ | ||
| public class TeleOpBot extends Robot { | ||
|
|
||
| public TeleOpBot(int side , OpMode varopMode){ | ||
| super(side, varopMode); | ||
| arm.startTeleArm(); | ||
| dumper.startDumper(); | ||
| sideArms.initSideArms(); | ||
|
|
||
| climberDumper.setPosition(0.5); | ||
| beaconR.setPosition(1); | ||
| beaconL.setPosition(0) ; | ||
| armHook.setPosition(0.3); | ||
| climberDumper.setPosition(0); | ||
| allClear.setPosition(0.5); | ||
|
|
||
| while (!adaFruitGyro.initDone) { | ||
| adaFruitGyro.initIMU(); | ||
| } | ||
| opMode.telemetry.addData("Initialization Done", ""); | ||
| } | ||
|
|
||
| long oldTime; | ||
| public void functloop(){ | ||
|
|
||
| if (System.currentTimeMillis()-oldTime<100){ | ||
| climberDumper.setPosition(1); | ||
| } else{ | ||
| climberDumper.setPosition(0.5); | ||
| } | ||
| arm.runArmEncoders(); | ||
|
|
||
| attachments(); | ||
| drive(); | ||
| hang(); | ||
|
|
||
| if (arm.getLockPos()>0.9) | ||
| beaconL.setPosition(0.5); | ||
| else { | ||
| beaconL.setPosition(0); | ||
| } | ||
| opMode.telemetry.addData("Arm Angle", "" + arm.getArmAnglePos()); | ||
| opMode.telemetry.addData("PullUp1", "" + arm.getArmPos()); | ||
| opMode.telemetry.addData("min", "min" + minpullup); | ||
| } | ||
|
|
||
| public void startBot(){ | ||
| oldTime = System.currentTimeMillis(); | ||
| } | ||
|
|
||
| private void drive() { | ||
| wheelBase.setInput(opMode.gamepad1.right_stick_x,opMode.gamepad1.left_stick_y ); | ||
| slowRobot(); | ||
| } | ||
|
|
||
| public void attachments() { | ||
| collector(); | ||
| dumper.dumping(); | ||
| sideArms.sideArm(adaFruitGyro,gyroOffset,wheelBase.getFr()); | ||
| arm.armControl(adaFruitGyro, gyroOffset,wheelBase.getFr()); | ||
| hook(); | ||
| arm.angleArm(); | ||
| allClearSymbol(); | ||
|
|
||
| } | ||
|
|
||
| public void collector() { | ||
| if (opMode.gamepad2.right_bumper) { | ||
| collector.setPower(1); | ||
| gyroOffset = -adaFruitGyro.getRoll(); | ||
| //collection out | ||
| } else if (opMode.gamepad2.left_bumper) { | ||
| collector.setPower(-0.4); | ||
| gyroOffset = -adaFruitGyro.getRoll(); | ||
| //resting | ||
| } else collector.setPower(0); | ||
| } | ||
| /** | ||
| * Makes the hook on the end of the arm go out to allow us to hang | ||
| */ | ||
| private void hook(){ | ||
| if (opMode.gamepad1.left_bumper){ | ||
| armHook.setPosition(0.6); | ||
| } | ||
| else { | ||
| armHook.setPosition(0.2); | ||
| } | ||
| } | ||
| /** | ||
| * we autonomized hang to put less stress on our drivers and hopefully let us hang 100% of the time | ||
| */ | ||
| private void hang(){ | ||
| arm.hang(); | ||
| wheelBase.driveBackwards(); | ||
|
|
||
| opMode.telemetry.addData("Arm Angle", "" + arm.getArmAnglePos()); | ||
| } | ||
|
|
||
| private void slowRobot() { | ||
| opMode.telemetry.addData("GyroPitch", " " + (adaFruitGyro.getRoll() + gyroOffset)); | ||
| if(opMode.gamepad1.right_trigger == 1 || ((adaFruitGyro.getRoll() + gyroOffset) > 6 && (adaFruitGyro.getRoll() + gyroOffset) < 50)) { | ||
|
|
||
| if (wheelBase.getFr().getPower() > 0) { | ||
| wheelBase.setDriveMod(1f); | ||
|
|
||
| } | ||
| else if (wheelBase.getFr().getPower() < 0) { | ||
| wheelBase.setDriveMod(0.3f); | ||
|
|
||
| } | ||
| } | ||
| else | ||
| wheelBase.setDriveMod(1); | ||
| } | ||
|
|
||
| private void allClearSymbol() { | ||
| if (opMode.gamepad2.x) { | ||
| allClear.setPosition(1); | ||
| } else if (opMode.gamepad2.y){ | ||
| allClear.setPosition(0); | ||
|
|
||
| } else { | ||
| allClear.setPosition(0.5); | ||
| } | ||
|
|
||
| } | ||
| } |