473 FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java
View file
@@ -1,220 +1,327 @@
package com.qualcomm.ftcrobotcontroller.opmodes;


//import android.hardware.Sensor;

import com.qualcomm.robotcore.hardware.DcMotorController;
import com.qualcomm.robotcore.hardware.LegacyModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.LegacyModule;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.UltrasonicSensor;

//import android.content.Context;
//import android.hardware.Sensor;
//import android.hardware.SensorEvent;
//import android.hardware.SensorEventListener;
//import android.hardware.SensorManager;

import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorController;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;

import java.text.SimpleDateFormat;
import java.util.Date;


import java.util.UnknownFormatConversionException;

/**
* Start backwards and align with climber thing
* Created by bk on 9/21/2015.
*/
public class EncoderTest extends LinearOpMode {
public class EncoderTest extends OpMode {
private String startDate;
private ElapsedTime runTime = new ElapsedTime();


DcMotor backLeftDrive;
DcMotor backRightDrive;
DcMotor frontLeftDrive;
DcMotor frontRightDrive;
DcMotor tape;
Servo adjust;
Servo deposit;
DcMotor backLeftDrive;
DcMotor backRightDrive;

int ANDYMARK_TICKS_PER_REV = 1680;
double Inches_per_Rotation = 13.125;
public EncoderTest() {

@Override public void runOpMode() throws InterruptedException{
//Initialize hardware
backLeftDrive = hardwareMap.dcMotor.get("backLeftDrive");
backRightDrive = hardwareMap.dcMotor.get("backRightDrive");
frontLeftDrive = hardwareMap.dcMotor.get("frontLeftDrive");
frontRightDrive = hardwareMap.dcMotor.get("frontRightDrive");
//tape = hardwareMap.dcMotor.get("tape");

//adjust = hardwareMap.servo.get("adjust");
//deposit = hardwareMap.servo.get("deposit");
}

backLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
backRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
//frontLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
//frontRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
//tape.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
@Override public void init() {
startDate = new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(new Date());

backRightDrive = hardwareMap.dcMotor.get("backRightDrive");
backLeftDrive = hardwareMap.dcMotor.get("backLeftDrive");
backLeftDrive.setDirection(DcMotor.Direction.REVERSE);
frontLeftDrive.setDirection(DcMotor.Direction.REVERSE);

//Wait for the game to start
waitForStart();
frontRightDrive = hardwareMap.dcMotor.get("frontRightDrive");
frontLeftDrive = hardwareMap.dcMotor.get("frontLeftDrive");
frontLeftDrive.setDirection(DcMotor.Direction.REVERSE);

//deposit.setPosition(Servo.MAX_POSITION);
// The strings must match names given in Settings->Configure Robot

// backwards(.5);
// sleep(2500);
// stopDrive();
// sleep(250);
//
// forward(.5);
// sleep(700);
// stopDrive();
// sleep(250);
//
// turnLeft(.5);
// sleep(800);
// stopDrive();
// sleep(250);
// forward(.75);
// sleep(1500);
// stopDrive();

telemetry.addData("ticks", "" + findTicks(24));
driveForwardDistance(1,findTicks(24));
stop();

}
// Do not do RESET_ENCODERS and RUN_WITHOUT_ENCODERS
// If you do - it will not run.
// setDriveMode(DcMotorController.RunMode.RESET_ENCODERS);
setDriveMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
setDrivePower(0.0, 0.0);

public int findTicks(double distance){
return (int) (2 * ANDYMARK_TICKS_PER_REV * distance / Inches_per_Rotation);
runTime.reset();
}

public void forward(double val) {
@Override public void loop() {
// Negative is up on the joystick,
// Positive is down on the joystick,
// Send the "negative" of the joystick so up is now positive
setDrivePower(-gamepad1.left_stick_y,-gamepad1.right_stick_y);

frontLeftDrive.setPower(val);
frontRightDrive.setPower(val);
backLeftDrive.setPower(val);
backRightDrive.setPower(val);
telemetry.addData("1 Motor 1", backRightDrive.getCurrentPosition());
telemetry.addData("2 Motor 2", backLeftDrive.getCurrentPosition());
}

public void driveForwardDistance(double power, int distance)
{
//frontRightDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);
//frontLeftDrive.setMode(DcMotorController. RunMode.RESET_ENCODERS);
backLeftDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);
backRightDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);

//frontLeftDrive.setTargetPosition(distance);
//frontRightDrive.setTargetPosition(distance);
backLeftDrive.setTargetPosition(distance);
backRightDrive.setTargetPosition(distance);

//frontLeftDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
//frontRightDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
backLeftDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
backRightDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);

forward(power);

while(backLeftDrive.isBusy() && backRightDrive.isBusy()){

}

stopDrive();
backLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
backRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
//frontLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
//frontRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
@Override public void stop() {
setDrivePower(0.0, 0.0);
}

public void turnLeftDistance(double power, int distance)
{
frontRightDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);
frontLeftDrive.setMode(DcMotorController. RunMode.RESET_ENCODERS);
backLeftDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);
backRightDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);

frontLeftDrive.setTargetPosition(-distance);
frontRightDrive.setTargetPosition(distance);
backLeftDrive.setTargetPosition(-distance);
backRightDrive.setTargetPosition(distance);

frontLeftDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
frontRightDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
backLeftDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
backRightDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);

turnLeft(power);

while(backLeftDrive.isBusy() && backRightDrive.isBusy() && frontLeftDrive.isBusy() && frontRightDrive.isBusy()){

}

stopDrive();
backLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
backRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
frontLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
frontRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
/**
* Set the power to left and right motors, the values must range
* between -1 and 1.
* @param left
* @param right
*/
public void setDrivePower(double left, double right) {
// This assumes power is given as -1 to 1
// The range clip will make sure it is between -1 and 1
// An incorrect value can cause the program to exception
backLeftDrive.setPower(Range.clip(left, -1.0, 1.0));
backRightDrive.setPower(Range.clip(right, -1.0, 1.0));
}

public void turnRightDistance(double power, int distance)
{
frontRightDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);
frontLeftDrive.setMode(DcMotorController. RunMode.RESET_ENCODERS);
backLeftDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);
backRightDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);

frontLeftDrive.setTargetPosition(distance);
frontRightDrive.setTargetPosition(-distance);
backLeftDrive.setTargetPosition(distance);
backRightDrive.setTargetPosition(-distance);

frontLeftDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
frontRightDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
backLeftDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
backRightDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);

turnRight(power);

while(backLeftDrive.isBusy() && backRightDrive.isBusy() && frontLeftDrive.isBusy() && frontRightDrive.isBusy()){

/**
* Sets the drive mode for each motor.
* The types of Run Modes are
* DcMotorController.RunMode.RESET_ENCODERS
* Resets the Encoder Values to 0
* DcMotorController.RunMode.RUN_TO_POSITION
* Runs until the encoders are equal to the target position
* DcMotorController.RunMode.RUN_USING_ENCODERS
* Attempts to keep the robot running straight utilizing
* the PID the reduces the maximum power by about 15%
* DcMotorController.RunMode.RUN_WITHOUT_ENCODERS
* Applies the power directly
* @param mode
*/
public void setDriveMode(DcMotorController.RunMode mode) {
if (backLeftDrive.getChannelMode() != mode) {
backLeftDrive.setChannelMode(mode);
}

stopDrive();
backLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
backRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
frontLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
frontRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
}

public void backwards(double val) {
frontLeftDrive.setPower(-val);
frontRightDrive.setPower(-val);
backLeftDrive.setPower(-val);
backRightDrive.setPower(-val);
}

public void turnLeft(double val) {
frontLeftDrive.setPower(-val);
frontRightDrive.setPower(val);
backLeftDrive.setPower(-val);
backRightDrive.setPower(val);
}

public void turnRight(double val) {
frontLeftDrive.setPower(val);
frontRightDrive.setPower(-val);
backLeftDrive.setPower(val);
backRightDrive.setPower(-val);
if (backRightDrive.getChannelMode() != mode) {
backRightDrive.setChannelMode(mode);
}
}
}

public void stopDrive(){
forward(0);
}

}
//
//
////import android.hardware.Sensor;
//
//import com.qualcomm.robotcore.hardware.DcMotorController;
//import com.qualcomm.robotcore.hardware.LegacyModule;
//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
//import com.qualcomm.robotcore.hardware.DcMotor;
//import com.qualcomm.robotcore.hardware.LegacyModule;
//import com.qualcomm.robotcore.hardware.Servo;
//import com.qualcomm.robotcore.hardware.UltrasonicSensor;
//
//import android.content.Context;
//import android.hardware.Sensor;
//import android.hardware.SensorEvent;
//import android.hardware.SensorEventListener;
//import android.hardware.SensorManager;
//
//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
//
//import java.text.SimpleDateFormat;
//import java.util.Date;
//
//
//import java.util.UnknownFormatConversionException;
//
///**
// * Start backwards and align with climber thing
// */
//public class EncoderTest extends LinearOpMode {
//
// DcMotor backLeftDrive;
// DcMotor backRightDrive;
// DcMotor frontLeftDrive;
// DcMotor frontRightDrive;
// DcMotor tape;
// Servo adjust;
// Servo deposit;
//
// int ANDYMARK_TICKS_PER_REV = 1680;
// double Inches_per_Rotation = 13.125;
//
// @Override public void runOpMode() throws InterruptedException{
// //Initialize hardware
// backLeftDrive = hardwareMap.dcMotor.get("backLeftDrive");
// backRightDrive = hardwareMap.dcMotor.get("backRightDrive");
// frontLeftDrive = hardwareMap.dcMotor.get("frontLeftDrive");
// frontRightDrive = hardwareMap.dcMotor.get("frontRightDrive");
// //tape = hardwareMap.dcMotor.get("tape");
//
// //adjust = hardwareMap.servo.get("adjust");
// //deposit = hardwareMap.servo.get("deposit");
//
//// backLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
//// backRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
//// //frontLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
//// //frontRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
//// //tape.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
////
//// backLeftDrive.setDirection(DcMotor.Direction.REVERSE);
//// frontLeftDrive.setDirection(DcMotor.Direction.REVERSE);
//
// //Wait for the game to start
// waitForStart();
//
// //deposit.setPosition(Servo.MAX_POSITION);
//
//// backwards(.5);
//// sleep(2500);
//// stopDrive();
//// sleep(250);
////
//// forward(.5);
//// sleep(700);
//// stopDrive();
//// sleep(250);
////
//// turnLeft(.5);
//// sleep(800);
//// stopDrive();
//// sleep(250);
//// forward(.75);
//// sleep(1500);
//// stopDrive();
//
// //telemetry.addData("ticks", "" + findTicks(24));
// //driveForwardDistance(0.95,findTicks(24));
// stop();
//
// }
//
// public int findTicks(double distance){
// return (int) (2 * ANDYMARK_TICKS_PER_REV * distance / Inches_per_Rotation);
// }
//
// public void forward(double val) {
//
// frontLeftDrive.setPower(val);
// frontRightDrive.setPower(val);
// backLeftDrive.setPower(val);
// backRightDrive.setPower(val);
// }
//
// public void driveForwardDistance(double power, int distance)
// {
// //frontRightDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);
// //frontLeftDrive.setMode(DcMotorController. RunMode.RESET_ENCODERS);
// backLeftDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);
// backRightDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);
////
//// //frontLeftDrive.setTargetPosition(distance);
//// //frontRightDrive.setTargetPosition(distance);
//// backLeftDrive.setTargetPosition(distance);
//// backRightDrive.setTargetPosition(distance);
////
//// //frontLeftDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
//// //frontRightDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
//// backLeftDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
//// backRightDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
////
//// forward(power);
////
//// while(backLeftDrive.isBusy() && backRightDrive.isBusy()){
////
//// }
////
//// stopDrive();
//// backLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
//// backRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
//// //frontLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
//// //frontRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
//
//
// }
//
// public void turnLeftDistance(double power, int distance)
// {
// frontRightDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);
// frontLeftDrive.setMode(DcMotorController. RunMode.RESET_ENCODERS);
// backLeftDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);
// backRightDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);
//
// frontLeftDrive.setTargetPosition(-distance);
// frontRightDrive.setTargetPosition(distance);
// backLeftDrive.setTargetPosition(-distance);
// backRightDrive.setTargetPosition(distance);
//
// frontLeftDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
// frontRightDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
// backLeftDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
// backRightDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
//
// turnLeft(power);
//
// while(backLeftDrive.isBusy() && backRightDrive.isBusy() && frontLeftDrive.isBusy() && frontRightDrive.isBusy()){
//
// }
//
// stopDrive();
// backLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
// backRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
// frontLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
// frontRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
// }
//
// public void turnRightDistance(double power, int distance)
// {
// frontRightDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);
// frontLeftDrive.setMode(DcMotorController. RunMode.RESET_ENCODERS);
// backLeftDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);
// backRightDrive.setMode(DcMotorController.RunMode.RESET_ENCODERS);
//
// frontLeftDrive.setTargetPosition(distance);
// frontRightDrive.setTargetPosition(-distance);
// backLeftDrive.setTargetPosition(distance);
// backRightDrive.setTargetPosition(-distance);
//
// frontLeftDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
// frontRightDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
// backLeftDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
// backRightDrive.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
//
// turnRight(power);
//
// while(backLeftDrive.isBusy() && backRightDrive.isBusy() && frontLeftDrive.isBusy() && frontRightDrive.isBusy()){
//
// }
//
// stopDrive();
// backLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
// backRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
// frontLeftDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
// frontRightDrive.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
// }
//
// public void backwards(double val) {
// frontLeftDrive.setPower(-val);
// frontRightDrive.setPower(-val);
// backLeftDrive.setPower(-val);
// backRightDrive.setPower(-val);
// }
//
// public void turnLeft(double val) {
// frontLeftDrive.setPower(-val);
// frontRightDrive.setPower(val);
// backLeftDrive.setPower(-val);
// backRightDrive.setPower(val);
// }
//
// public void turnRight(double val) {
// frontLeftDrive.setPower(val);
// frontRightDrive.setPower(-val);
// backLeftDrive.setPower(val);
// backRightDrive.setPower(-val);
// }
//
// public void stopDrive(){
// forward(0);
// }
//
//}
625 FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest2.java
View file

Large diffs are not rendered by default.

2 ...otController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java
View file
@@ -58,7 +58,7 @@ public void register(OpModeManager manager) {
manager.register("VisionTest", BasicVisionSample.class);
manager.register("LightTest", BasicLightSample.class);
manager.register("Accel", AccelerometerComponent.class);
manager.register("Encoders", EncoderTest.class);
manager.register("Encoders", EncoderTest2.class);
manager.register("Straight Line", navXRotateToAnglePIDLoopOp.class);
manager.register("Line Follow", K9Line.class);
// manager.register("NullOp", NullOp.class);
26 ...obotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/PushBotHardware.java
View file
@@ -127,32 +127,6 @@ public PushBotHardware ()
//
double l_hand_position = 0.5;

try
{
v_servo_left_hand = hardwareMap.servo.get ("left_hand");
v_servo_left_hand.setPosition (l_hand_position);
}
catch (Exception p_exeception)
{
m_warning_message ("left_hand");
DbgLog.msg (p_exeception.getLocalizedMessage ());

v_servo_left_hand = null;
}

try
{
v_servo_right_hand = hardwareMap.servo.get ("right_hand");
v_servo_right_hand.setPosition (l_hand_position);
}
catch (Exception p_exeception)
{
m_warning_message ("right_hand");
DbgLog.msg (p_exeception.getLocalizedMessage ());

v_servo_right_hand = null;
}

} // init

//--------------------------------------------------------------------------
207 ...otController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/TankDriveTapeTest.java
View file
@@ -11,17 +11,23 @@ public class TankDriveTapeTest extends OpMode{
DcMotor frontRightDrive;
DcMotor backLeftDrive;
DcMotor backRightDrive;
DcMotor tapeRight;
DcMotor tapeLeft;

Servo leftAdjust;
Servo rightAdjust;

Servo climberLeft;
Servo climberRight;

Servo middle;
DcMotor tapeRight;
DcMotor tapeLeft;
Servo adjustLeft;
Servo adjustRight;
//Servo deposit;
// Servo safe;

Servo deposit;
Servo depositTilt;

Servo allClearLeft;
Servo allClearRight;

boolean reversed = false;
boolean rightExtended = false;
boolean leftExtended = false;
@@ -46,22 +52,30 @@ public void init() {
backRightDrive = hardwareMap.dcMotor.get("backRightDrive");
backLeftDrive = hardwareMap.dcMotor.get("backLeftDrive");
backLeftDrive.setDirection(DcMotor.Direction.REVERSE);

frontRightDrive = hardwareMap.dcMotor.get("frontRightDrive");
frontLeftDrive = hardwareMap.dcMotor.get("frontLeftDrive");
frontLeftDrive.setDirection(DcMotor.Direction.REVERSE);
climberLeft = hardwareMap.servo.get("climberLeft");
climberRight = hardwareMap.servo.get("climberRight");

tapeRight = hardwareMap.dcMotor.get("tapeRight");
tapeLeft = hardwareMap.dcMotor.get("tapeLeft");
tapeLeft.setDirection(DcMotor.Direction.REVERSE);
adjustLeft = hardwareMap.servo.get("adjustLeft");
adjustRight = hardwareMap.servo.get("adjustRight");

//climberLeft = hardwareMap.servo.get("climberLeft");
//climberRight = hardwareMap.servo.get("climberRight");

leftAdjust = hardwareMap.servo.get("leftAdjust");
rightAdjust = hardwareMap.servo.get("rightAdjust");

middle = hardwareMap.servo.get("middle");
// deposit = hardwareMap.servo.get("deposit");
// safe = hardwareMap.servo.get("safe");
allClearLeft = hardwareMap.servo.get("allClearLeft");

deposit = hardwareMap.servo.get("deposit");
depositTilt = hardwareMap.servo.get("depositTilt");

//allClearLeft = hardwareMap.servo.get("allClearLeft");
//allClearLeft.setPosition(Servo.MIN_POSITION);

allClearRight = hardwareMap.servo.get("allClearRight");
allClearLeft.setPosition(Servo.MIN_POSITION);
allClearRight.setPosition(Servo.MIN_POSITION);
}

@@ -86,21 +100,21 @@ public void loop() {

// write the values to the motors

if (right >= .8)
if (right >= .8) //Controls for Center Helper Wheel
middle.setPosition(Servo.MIN_POSITION);
else if (right <= -.8)
middle.setPosition(Servo.MAX_POSITION);
else
middle.setPosition(0.5);

frontRightDrive.setPower(right);
frontRightDrive.setPower(right); //Drive Controls
frontLeftDrive.setPower(left);
backRightDrive.setPower(right);
backLeftDrive.setPower(left);


// update the position of the climberServo.
// if (gamepad1.y) {
// if (gamepad1.y) { //Controls for Climber Servos
// // if the Y button is pushed on gamepad1, increment the position of
// // the climberServo servo.
// climber.setPosition(Servo.MIN_POSITION * .75);
@@ -118,91 +132,124 @@ else if (right <= -.8)
// deposit.setPosition(pos);
// }
//
if (gamepad1.dpad_up) {
allClearLeft.setPosition(Servo.MAX_POSITION);
if (gamepad1.dpad_up) { //All Clear Controls
//allClearLeft.setPosition(Servo.MAX_POSITION);
allClearRight.setPosition(Servo.MAX_POSITION);
}

if (gamepad1.b){
if (rightExtended) {
climberRight.setPosition(Servo.MIN_POSITION);
rightExtended = false;
}
else {
climberRight.setPosition(0.5);
rightExtended = true;
}
else if(gamepad1.dpad_down) {
//allClearLeft.setPosition(Servo.MIN_POSITION);
allClearRight.setPosition(Servo.MIN_POSITION);
}

if (gamepad1.x){
if (leftExtended) {
climberLeft.setPosition(Servo.MIN_POSITION);
leftExtended = false;
// if (gamepad1.b){
// if (rightExtended) {
// climberRight.setPosition(Servo.MIN_POSITION);
// rightExtended = false;
// }
// else {
// climberRight.setPosition(0.5);
// rightExtended = true;
// }
// }
//
// if (gamepad1.x){
// if (leftExtended) {
// climberLeft.setPosition(Servo.MIN_POSITION);
// leftExtended = false;
// }
// else {
// climberLeft.setPosition(0.5);
// leftExtended = true;
// }
// }



if (gamepad2.x) {
if (deposit.getPosition() > 0.05){
deposit.setPosition(deposit.getPosition() - 0.05);
}
else {
climberLeft.setPosition(0.5);
leftExtended = true;
}

if (gamepad2.b) {
if (deposit.getPosition() < 0.95){
deposit.setPosition(deposit.getPosition() + 0.05);
}
}

else if(gamepad1.dpad_down) {
allClearLeft.setPosition(Servo.MIN_POSITION);
allClearRight.setPosition(Servo.MIN_POSITION);
if (gamepad2.y) {
depositTilt.setPosition(Servo.MIN_POSITION);
}
//
// if (gamepad1.x) {
// pos = deposit.getPosition() - .1;
// if (pos < 0)
// pos = 0.01;
// deposit.setPosition(pos);
// }
if (gamepad2.right_bumper) {

if (gamepad2.a) {
depositTilt.setPosition(Servo.MAX_POSITION);
}






if (gamepad1.right_trigger > 0.3) { //Controls for Right Tape Measure
tapeRight.setPower(.5);
}

else if (gamepad2.left_bumper) {
else if (gamepad1.left_trigger > 0.3) {
tapeRight.setPower(-.95);
}

else {
tapeRight.setPower(0);
}

if (gamepad1.right_bumper) {
tapeLeft.setPower(0.5);
}

else if (gamepad1.left_bumper) {
tapeLeft.setPower(-0.95);
}
else {
tapeLeft.setPower(0);
}

if(gamepad1.a){
if (adjustRight.getPosition() < 0.99) {
adjustRight.setPosition(adjustRight.getPosition() + 0.01);
if(gamepad1.right_bumper){
if (rightAdjust.getPosition() < 0.99) {
rightAdjust.setPosition(rightAdjust.getPosition() + 0.01);
}
if (leftAdjust.getPosition() > 0.01) {
leftAdjust.setPosition(leftAdjust.getPosition() - 0.01);
}

}

if ((gamepad1.y)){
if (adjustRight.getPosition() > 0.01) {
adjustRight.setPosition(adjustRight.getPosition() - 0.01);
if ((gamepad1.left_bumper)){
if (rightAdjust.getPosition() > 0.01) {
rightAdjust.setPosition(rightAdjust.getPosition() - 0.01);
}
if (leftAdjust.getPosition() < 0.99) {
leftAdjust.setPosition(leftAdjust.getPosition() + 0.01);
}

}

if(gamepad2.a){
if (adjustLeft.getPosition() < 0.99) {
adjustLeft.setPosition(adjustLeft.getPosition() + 0.01);
}



if (gamepad2.right_trigger > 0.3) { //Controls for Left Tape Measure
tapeLeft.setPower(0.5);
}

if ((gamepad2.y)){
if (adjustLeft.getPosition() > 0.01) {
adjustLeft.setPosition(adjustLeft.getPosition() - 0.01);
}
else if (gamepad2.left_trigger > 0.3) {
tapeLeft.setPower(-0.95);
}
else {
tapeLeft.setPower(0);
}

// if(gamepad2.right_bumper){
// if (leftAdjust.getPosition() < 0.99) {
// leftAdjust.setPosition(leftAdjust.getPosition() + 0.01);
// }
// }
//
// if ((gamepad2.left_bumper)){
// if (leftAdjust.getPosition() > 0.01) {
// leftAdjust.setPosition(leftAdjust.getPosition() - 0.01);
// }
// }


// if (gamepad1.guide) {
//
@@ -218,24 +265,6 @@ else if (gamepad1.left_bumper) {
// tape.setDirection(DcMotor.Direction.REVERSE);
// }
// }

// if (gamepad2.a) {
// allClear.setPosition(Servo.MAX_POSITION);
// }
//
// if (gamepad2.y) {
// allClear.setPosition(Servo.MIN_POSITION);
// }


// update the position of the claw
// clip the position values so that they never exceed their allowed range.
//armPosition = Range.clip(armPosition, ARM_MIN_RANGE,›› ARM_MAX_RANGE);
//clawPosition = Range.clip(clawPosition, CLAW_MIN_RANGE, CLAW_MAX_RANGE);

// write position values to the wrist and claw servo
//climberServo.setPosition(armPosition);
//claw.setPosition(clawPosition);
}

/*