@@ -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);
}

}
}