public void moveState(double power, double inches, int state, int change) {
int stateMove = 1;
telemetry.addData("Move state: ", stateMove);
switch (stateMove) {
case 1:
leftTread.setPower(power);
rightTread.setPower(power);
if (Math.abs(leftTread.getCurrentPosition()) >= (inches * TICKS_PER_INCH)) {
stateMove = 2;
}
break;
case 2:
leftTread.setPower(0.0);
rightTread.setPower(0.0);
stateMove = 3;
break;
case 3:
leftTread.setMode(DcMotor.RunMode.RESET_ENCODERS);
rightTread.setMode(DcMotor.RunMode.RESET_ENCODERS);
stateMove = 4;
break;
case 4:
if ((leftTread.getCurrentPosition() == 0) && (rightTread.getCurrentPosition() == 0)) {
leftTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);
rightTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);
}
state = change;
break;
default:
// End of switch (or bug).
break;
}
}
public int getEncoderAvg() {
return ((Math.abs(motorBR.getCurrentPosition()))
+ (Math.abs(motorBL.getCurrentPosition()))
+ (Math.abs(motorFR.getCurrentPosition()))
+ (Math.abs(motorFL.getCurrentPosition())))
/ 4;
}
@Override
public void loop() {
telemetry.addData("rightEncoder = ", motorRight.getCurrentPosition());
if (motorRight.getCurrentPosition() <= -5000) {
if (finishedForward) {
finishedForward = false;
finishedTurning = true;
telemetry.addData("I did it!", motorRight.getCurrentPosition());
// wait(waitValue);
newValue = motorRight.getCurrentPosition() - 250;
motorRight.setTargetPosition(-newValue);
motorRight.setPower(.50);
motorLeft.setPower(-.50);
}
}
if (motorRight.getCurrentPosition() <= -newValue) {
if (finishedTurning) {
finishedTurning = false;
motorRight.setPower(0);
motorLeft.setPower(0);
}
}
}
public void turnState(double power, double degrees, int state, int change) {
int stateTurn = 1;
telemetry.addData("Turn state: ", stateTurn);
switch (stateTurn) {
case 1:
leftTread.setPower(power);
rightTread.setPower(-power);
if (Math.abs(leftTread.getCurrentPosition()) >= (degrees * TICKS_PER_DEGREE)) {
stateTurn = 2;
}
break;
case 2:
leftTread.setPower(0.0);
rightTread.setPower(0.0);
stateTurn = 3;
break;
case 3:
leftTread.setMode(DcMotor.RunMode.RESET_ENCODERS);
rightTread.setMode(DcMotor.RunMode.RESET_ENCODERS);
stateTurn = 4;
break;
case 4:
if ((leftTread.getCurrentPosition() == 0) && (rightTread.getCurrentPosition() == 0)) {
leftTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);
rightTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);
}
state = change;
break;
default:
// End of switch (or bug).
break;
}
}
@Override
public void loop() {
telemetry.addData("encoders", motor1.getCurrentPosition());
telemetry.addData("x", x);
switch (x) {
case 0:
motor1.setPower(0.2);
motor1.setTargetPosition(1120);
motor1.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
if (motor1.getCurrentPosition() < 1125 && motor1.getCurrentPosition() > 1115) {
motor1.setPower(0.0);
x = 1;
motor1.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
}
break;
case 1:
motor1.setPower(0.1);
motor1.setTargetPosition(2400);
motor1.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
if (motor1.getCurrentPosition() < 2405 && motor1.getCurrentPosition() > 2395) {
motor1.setPower(0.0);
x = 2;
motor1.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
}
}
}
@Override
public void loop() {
double rightDrive;
double leftDrive;
rightDrive = -gamepad1.right_stick_y;
leftDrive = -gamepad1.left_stick_y;
if (gamepad1.dpad_up) {
rightDrive = 0.3;
leftDrive = 0.3;
} else if (gamepad1.dpad_down) {
rightDrive = -0.3;
leftDrive = -0.3;
} else if (gamepad1.dpad_right) {
rightDrive = -0.3;
leftDrive = 0.3;
} else if (gamepad1.dpad_left) {
rightDrive = 0.3;
leftDrive = -0.3;
}
motorRight.setPower(rightDrive);
motorLeft.setPower(leftDrive);
if (gamepad1.a) {
servoClimberDumper.setPosition(1.0);
} else {
servoClimberDumper.setPosition(0.0);
}
telemetry.addData("trackLifter", Integer.toString(trackLifter.getCurrentPosition()));
telemetry.addData("liftCheck", liftCheck.isPressed());
telemetry.addData("ENCLeft", Integer.toString(motorLeft.getCurrentPosition()));
telemetry.addData("ENCRight", Integer.toString(motorRight.getCurrentPosition()));
telemetry.addData("trigger", gamepad1.right_trigger);
}
public void driveDistance(double ldist, double rdist, double power) throws InterruptedException {
int enc1;
int enc2;
enc1 = (int) Math.round(ldist * 33.65);
enc2 = (int) Math.round(rdist * 33.65);
leftMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
rightMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
waitOneFullHardwareCycle();
waitOneFullHardwareCycle();
waitOneFullHardwareCycle();
leftMotor2.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
rightMotor2.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
waitOneFullHardwareCycle();
leftMotor2.setTargetPosition(enc1);
rightMotor2.setTargetPosition(enc2);
leftMotor2.setPower(power);
rightMotor2.setPower(power);
waitOneFullHardwareCycle();
waitOneFullHardwareCycle();
waitOneFullHardwareCycle();
waitOneFullHardwareCycle();
waitOneFullHardwareCycle();
leftMotorController.setMotorControllerDeviceMode(
DcMotorController.DeviceMode.READ_ONLY); // Change to read
rightMotorController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.READ_ONLY);
waitOneFullHardwareCycle();
while (counter == 1) {
if (isCloseto(leftMotor2.getCurrentPosition(), enc1)
&& isCloseto(rightMotor2.getCurrentPosition(), enc2)) {
counter = 2;
}
waitOneFullHardwareCycle();
}
waitOneFullHardwareCycle();
leftMotorController.setMotorControllerDeviceMode(
DcMotorController.DeviceMode.WRITE_ONLY); // Change to read
rightMotorController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.WRITE_ONLY);
waitOneFullHardwareCycle();
leftMotor2.setPower(0);
rightMotor2.setPower(0);
leftMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
rightMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
waitOneFullHardwareCycle();
leftMotorController.setMotorControllerDeviceMode(
DcMotorController.DeviceMode.READ_ONLY); // Change to read
rightMotorController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.READ_ONLY);
waitOneFullHardwareCycle();
telemetry.addData("Currentvalue", leftMotor2.getCurrentPosition());
waitOneFullHardwareCycle();
leftMotorController.setMotorControllerDeviceMode(
DcMotorController.DeviceMode.WRITE_ONLY); // Change to read
rightMotorController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.WRITE_ONLY);
waitOneFullHardwareCycle();
leftMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
rightMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
counter = 1;
}
public void getEncoderValues() {
telemetry.addData("motorBL", motorBL.getCurrentPosition());
telemetry.addData("motorFR", motorFR.getCurrentPosition());
telemetry.addData("motorBR", motorBR.getCurrentPosition());
telemetry.addData("motorFL", motorFL.getCurrentPosition());
}
protected void printEncoderValues() throws InterruptedException {
switchToReadOnly();
telemetry.addData("Motor", "Encoder Position is " + bLeft.getCurrentPosition());
telemetry.addData("Motor", "Encoder Position is " + bRight.getCurrentPosition());
telemetry.addData("Motor", "Encoder Position is " + fLeft.getCurrentPosition());
telemetry.addData("Motor", "Encoder Position is " + fRight.getCurrentPosition());
switchToWriteOnly();
}
public void smoothMoveVol2(double inches, boolean backwards) {
// works best for at least 1000 ticks = 11.2 inches approx
double rotations = inches / (Keys.WHEEL_DIAMETER * Math.PI);
double totalTicks = rotations * 1120 * 3 / 2;
int positionBeforeMovement = fl.getCurrentPosition();
double ticksToGo = positionBeforeMovement + totalTicks;
// p;us one because make the first tick 1, not 0, so fxn will never be 0
double savedPower = 0;
double savedTick = 0;
while (fl.getCurrentPosition() < ticksToGo + 1) {
telemetry.addData("front left encoder: ", fl.getCurrentPosition());
telemetry.addData("ticksFor", totalTicks);
collector.setPower(-1 * Keys.COLLECTOR_POWER);
// convert to radians
int currentTick = fl.getCurrentPosition() - positionBeforeMovement + 1;
if (currentTick < ticksToGo / 2) {
// use an inv tan function as acceleration
// power = ((2/pi)*.86) arctan (x/totalticks*.1)
double power =
((2 / Math.PI) * Keys.MAX_SPEED) * Math.atan(currentTick / totalTicks / 2 * 10);
telemetry.addData("power", "accel" + power);
if (power < Keys.MIN_SPEED_SMOOTH_MOVE) {
telemetry.addData("bool", power < Keys.MIN_SPEED_SMOOTH_MOVE);
power = Keys.MIN_SPEED_SMOOTH_MOVE;
telemetry.addData("power", "adjusted" + power);
}
telemetry.addData("power", power);
setMotorPowerUniform(power, backwards);
savedPower = power;
savedTick = currentTick;
} else {
// decelerate using
double newCurrentCount = currentTick + 1 - savedTick;
// current tick changes, savedTick is constant
double horizontalStretch = totalTicks / 2 * .2;
if (newCurrentCount < horizontalStretch) {
// becuase of domain restrictions
setMotorPowerUniform(savedPower, backwards);
} else {
// in the domain
double power =
(2 / Math.PI) * savedPower * Math.asin(horizontalStretch / newCurrentCount);
telemetry.addData("power", "decel" + power);
if (power < Keys.MIN_SPEED_SMOOTH_MOVE) {
power = Keys.MIN_SPEED_SMOOTH_MOVE;
telemetry.addData("power", "adjusted" + power);
}
setMotorPowerUniform(power, backwards);
}
}
}
rest();
}
// code to run the arm
public void handle_arm() {
tapeMotor.setPower(0);
shoulderMotor.setPower(0);
double shoulder = gamepad2.left_stick_y;
// double tape = gamepad2.right_stick_y;
if (shoulder > Positive_Dead_Zone) {
if ((shoulderMotor.getCurrentPosition() + shoulder_increment) < shoulder_top_limit) {
// don't go forward past maximum position
shoulderMotor.setTargetPosition(shoulderMotor.getCurrentPosition() + shoulder_increment);
shoulderMotor.setPower(shoulderMaxPower);
telemetry.addData(
"shoulder : ", "Shoulder up " + System.out.format("%d", shoulderMotorEncodercurrent));
} else {
shoulderMotor.setTargetPosition(shoulder_top_limit);
shoulderMotor.setPower(shoulderMaxPower);
telemetry.addData(
"shoulder : ", "Shoulder up " + System.out.format("%d", shoulderMotorEncodercurrent));
}
} else if (shoulder < Negative_Dead_Zone) {
if ((shoulderMotor.getCurrentPosition() - shoulder_increment) < shoulder_bottom_limit) {
// don't go backwards beyond starting position
shoulderMotor.setTargetPosition(shoulderMotor.getCurrentPosition() - shoulder_increment);
shoulderMotor.setPower(shoulderMaxPower);
telemetry.addData(
"shoulder : ", "Shoulder down " + System.out.format("%d", shoulderMotorEncodercurrent));
} else {
shoulderMotor.setTargetPosition(shoulder_bottom_limit);
shoulderMotor.setPower(shoulderMaxPower);
telemetry.addData(
"shoulder : ", "Shoulder down " + System.out.format("%d", shoulderMotorEncodercurrent));
}
}
// need to change below because it's for tape measure motor, which doesn't have encoders
double tapeValue = -gamepad2.right_stick_y;
double tapePower = tapeValue * tapeValue;
if (gamepad2.right_stick_y > Positive_Dead_Zone) {
// run at scaled power
tapeMotor.setPower(tapePower * tapeMaxPower);
} else if (gamepad2.right_stick_y < Negative_Dead_Zone) {
tapeMotor.setPower(-tapePower * tapeMaxPower);
}
// ***************************************8
}
public void moveStraight(double dist, boolean backwards, double power) {
double rotations = dist / (Keys.WHEEL_DIAMETER * Math.PI);
double totalTicks = rotations * 1120 * 3 / 2;
int positionBeforeMovement = fl.getCurrentPosition();
if (backwards) {
while (fl.getCurrentPosition() > positionBeforeMovement - totalTicks) {
setMotorPowerUniform(power, backwards);
}
} else {
while (fl.getCurrentPosition() < positionBeforeMovement + totalTicks) {
setMotorPowerUniform(power, backwards);
}
}
rest();
}
public void run1() {
motorW();
rnctr++;
leftmotor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
rightmotor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
telemetry.addData("runct", rnctr);
while (togo < inchtorun) {
motorW();
rightmotor.setTargetPosition(rightPos * togo);
leftmotor.setTargetPosition(leftPos * togo);
leftmotor.setPower(.5);
rightmotor.setPower(.5);
while (ctr < 50000000) {
ctr++;
}
motorR();
while (leftmotor.getCurrentPosition() != leftPos) {}
togo++;
}
motorW();
leftmotor.setPower(0);
rightmotor.setPower(0);
// return;
}
@Override
public void loop() {
loopctr++;
motorR();
// tpos=rightmotor.getTargetPosition();
// curpos=rightmotor.getCurrentPosition();
// togo=tpos-curpos;
telemetry.addData("left mode", leftmotor.getMode());
telemetry.addData("lf pos", leftmotor.getCurrentPosition());
telemetry.addData("lf to", leftmotor.getTargetPosition());
/*if (togo<=0)
{
if (runagain==true)
{
resetEncode();
rightPos=800;
leftPos=800;
run1();
runagain=false;
run3rd=true;
}
if (run3rd==true)
{
resetEncode();
rightPos=6625;
leftPos=-6625;
run1();
run3rd=false;
}
}*/
} // end of loop
public void resetEncode() {
motorW();
leftmotor.setMode(DcMotorController.RunMode.RESET_ENCODERS);
rightmotor.setMode(DcMotorController.RunMode.RESET_ENCODERS);
motorR();
while (leftmotor.getCurrentPosition() != 0) {}
return;
}
public void moveForwardToWallEnc(double pow, int timeout) throws InterruptedException {
double angle = Math.abs(sensor.getGyroYaw());
double startAngle = angle;
double power = pow;
int tick = 1;
resetEncoders();
int startEncoder = Math.abs(motorFL.getCurrentPosition());
int endEncoder;
ElapsedTime time = new ElapsedTime();
time.startTime();
while (Math.abs(angle - startAngle) < 10 && time.milliseconds() < timeout) {
angle = Math.abs(sensor.getGyroYaw());
opMode.telemetry.addData("LeftPower", motorBL.getPower());
opMode.telemetry.addData("RightPower", motorBR.getPower());
opMode.telemetry.update();
if (time.milliseconds() > 250 * tick) {
endEncoder = Math.abs(motorFL.getCurrentPosition());
if (startEncoder + 250 < endEncoder) {
break;
}
}
if (angle < startAngle - 1) {
startMotors((power * .75), power);
} else if (angle > startAngle + 1) {
startMotors(power, (power * .75));
} else {
startMotors(power, power);
}
opMode.idle();
}
stopMotors();
angleError = sensor.getGyroYaw();
opMode.telemetry.update();
}
private double getEncoderAvg() {
motorBLE = motorBL.getCurrentPosition();
motorBRE = motorBR.getCurrentPosition();
motorFLE = motorFL.getCurrentPosition();
motorFRE = motorFR.getCurrentPosition();
return (Math.abs(motorBL.getCurrentPosition())
+ Math.abs(motorBR.getCurrentPosition())
+ Math.abs(motorFL.getCurrentPosition())
+ Math.abs(motorFR.getCurrentPosition()))
/ 4;
}
public void moveAlteredSin(double dist, boolean backwards) {
// inches
double rotations = dist / (Keys.WHEEL_DIAMETER * Math.PI);
double totalTicks = rotations * 1120 * 3 / 2;
int positionBeforeMovement = fl.getCurrentPosition();
while (fl.getCurrentPosition() < positionBeforeMovement + totalTicks) {
telemetry.addData("front left encoder: ", "sin" + fl.getCurrentPosition());
telemetry.addData("ticksFor", totalTicks);
collector.setPower(-1 * Keys.COLLECTOR_POWER);
// convert to radians
int currentTick = fl.getCurrentPosition() - positionBeforeMovement;
// accelerate 15% of time
// coast 25% of time
// decelerate 60% of time
int firstSectionTime = (int) Math.round(.05 * totalTicks);
int secondSectionTime = (int) (Math.round((.05 + .05) * totalTicks)); // 35
int thirdSectionTime = (int) (Math.round((.5) * totalTicks)); // 35
// rest will just be 100%
double power;
if (currentTick < firstSectionTime) {
power = .33;
// first quarter (period = 2pi) of sin function is only reaching altitude
} else if (currentTick < secondSectionTime) {
power = .66;
} else if (currentTick < thirdSectionTime) {
power = .86;
} else {
// between [40%,100%]
// decrease time
int ticksLeft = (int) Math.round(currentTick - (totalTicks * .35));
// with these ticks left, set a range within cosine to decrease
power = .4 * Math.cos((ticksLeft) * Math.PI / totalTicks) + .4;
}
telemetry.addData("power", power);
setMotorPowerUniform(power, backwards);
}
rest();
}
// calculates movement updates encoders and looks for buttons pressed
@Override
public void loop() {
telemetry.addData("EncoderAverage", getEncoderAvg());
telemetry.addData("motorBL", motorBR.getCurrentPosition());
telemetry.addData("motorFR", motorFR.getCurrentPosition());
telemetry.addData("motorBR", motorBR.getCurrentPosition());
telemetry.addData("motorFL", motorFL.getCurrentPosition());
// controls motion motors
if (Math.abs(gamepad1.left_stick_y) > .05 || Math.abs(gamepad1.right_stick_y) > .05) {
startMotors(
gamepad1.right_stick_y,
-gamepad1.left_stick_y,
-gamepad1.left_stick_y,
gamepad1.right_stick_y);
} else {
stopMotors();
}
// raises lift
if (Math.abs(gamepad2.right_trigger) > .05) changeLift(gamepad2.right_trigger);
else stopLift();
// lowers lift
if (Math.abs(gamepad2.left_trigger) > .05) changeLift(-gamepad2.left_trigger);
else stopLift();
// resets encoders
if (gamepad1.a) {
resetEncoders();
}
// sets speed to 1/2
if (gamepad1.x) {
setDivider(2);
}
// sets speed to 1/4
if (gamepad1.b) {
setDivider(4);
}
// resets speed to normal
if (gamepad1.y) {
setDivider(1);
}
}
/*
reverseDriveAuto sets the motors to drive in reverse to the target location
@param distance distance to travel in inches
*/
public void reverseDriveAuto(double distance) {
double counts = ENCODER_CPR * (distance / (Math.PI * wheelDiameter)) * gearRatio;
leftMotor.setTargetPosition((int) counts);
rightMotor.setTargetPosition((int) counts);
if (leftMotor.getCurrentPosition() != leftMotor.getTargetPosition()) {
leftMotor.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
rightMotor.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
reverseDrive(leftMotor, rightMotor, POWER);
} else {
leftMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
rightMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
}
}
/*
forwardDriveAuto method that calculates the distance from inches to encoder counts and then
sets the motors to drive to the target location
@param distance distance to travel in inches
*/
public void forwardDriveAuto(double distance) {
double counts = ENCODER_CPR * (distance / (Math.PI * wheelDiameter)) * gearRatio;
leftMotor.setTargetPosition((int) counts);
rightMotor.setTargetPosition((int) counts);
if (leftMotor.getCurrentPosition()
!= leftMotor
.getTargetPosition()) { // Resets the encoders after the position has been reached
leftMotor.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
rightMotor.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
forwardDrive(leftMotor, rightMotor, POWER);
} else {
leftMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
rightMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
}
}
@Override
public void runOpMode() {
elapsedTime = new ElapsedTime();
dim = hardwareMap.deviceInterfaceModule.get("dim");
dim.setDigitalChannelMode(GYROSCOPE_SPOT, DigitalChannelController.Mode.OUTPUT);
gyro = hardwareMap.gyroSensor.get("gyro");
dim.setDigitalChannelState(GYROSCOPE_SPOT, true);
motorBL = hardwareMap.dcMotor.get("motorBL");
motorBR = hardwareMap.dcMotor.get("motorBR");
motorFL = hardwareMap.dcMotor.get("motorFL");
elapsedTime.startTime();
motorFR = hardwareMap.dcMotor.get("motorFR");
// color = hardwareMap.colorSensor.get("color");
int distance1 = 5550;
int distance3 = 9700;
double currentAngle = 0.0;
while (motorBL.getCurrentPosition() < distance1) {
startMotors(-1, 1, 1, -1);
getEncoderValues();
}
while (currentAngle < 90.0) {
startMotors(-1, -1, -1, -1);
getEncoderValues();
currentAngle = gyro.getRotation();
}
while (motorBL.getCurrentPosition() < distance3) {
startMotors(-1, 1, 1, -1);
getEncoderValues();
}
getTime();
elapsedTime.reset();
double currentTime = 0.0;
while (currentTime < 5.0) {
getEncoderValues();
getTime();
currentTime = elapsedTime.time();
}
while (motorBL.getCurrentPosition() > 9000) { // 9034
startMotors(1, -1, -1, 1);
getEncoderValues();
}
while (currentAngle > 45.0) {
startMotors(-1, -1, -1, -1);
getEncoderValues();
currentAngle = gyro.getRotation();
}
elapsedTime.reset();
currentTime = 0.0;
while (currentTime < 5.0) {
startMotors(-1, 1, 1, -1);
getEncoderValues();
getTime();
currentTime = elapsedTime.time();
}
stopMotors();
motorBL.close();
motorFL.close();
motorBR.close();
motorFR.close();
}
protected void move(int targetInches, MoveDirection moveDirection) throws InterruptedException {
switchToReadOnly();
double targetPos = (targetInches / INCHES_PER_ROTATION) * NEVEREST_PPR;
if (moveDirection == MoveDirection.FORWARD) {
while ((Math.abs(fLeft.getCurrentPosition()) < targetPos
&& Math.abs(fRight.getCurrentPosition()) < targetPos)
&& (Math.abs(bLeft.getCurrentPosition()) < targetPos
&& Math.abs(bRight.getCurrentPosition()) < targetPos)) {
System.out.println(
fLeft.getCurrentPosition()
+ " "
+ fRight.getCurrentPosition()
+ " "
+ bLeft.getCurrentPosition()
+ " "
+ bRight.getCurrentPosition());
System.out.println("Moving" + targetInches + " Inches Forward...");
switchToWriteOnly();
fLeft.setPower(SPEED);
fRight.setPower(SPEED);
bLeft.setPower(SPEED);
bRight.setPower(SPEED);
printEncoderValues();
switchToReadOnly();
}
} else if (moveDirection == MoveDirection.BACKWARD) {
while ((Math.abs(fLeft.getCurrentPosition()) < targetPos
&& Math.abs(fRight.getCurrentPosition()) < targetPos)
&& (Math.abs(bLeft.getCurrentPosition()) < targetPos
&& Math.abs(bRight.getCurrentPosition()) < targetPos)) {
System.out.println(
fLeft.getCurrentPosition()
+ " "
+ fRight.getCurrentPosition()
+ " "
+ bLeft.getCurrentPosition()
+ " "
+ bRight.getCurrentPosition());
System.out.println("Moving" + targetInches + " Inches Backward...");
switchToWriteOnly();
fLeft.setPower(-SPEED);
fRight.setPower(-SPEED);
bLeft.setPower(-SPEED);
bRight.setPower(-SPEED);
printEncoderValues();
switchToReadOnly();
}
}
switchToWriteOnly();
stopMotors();
}
public boolean aRange(double min, double max) { // Absolute Range (the actual encoder value)
return (Math.abs((motor.getCurrentPosition() - offset)) < max
&& Math.abs((motor.getCurrentPosition() - offset)) > min);
}
public double getEnc() {
return motor.getCurrentPosition();
}
// resets encoders
public void resetEncs() {
enc = 0;
offset = encOld = motor.getCurrentPosition();
}
@Override
public void getValues() {
enc += motor.getCurrentPosition() - encOld;
}
@Override
public void
calibrate() { // sets the current encoder value as 'old' such that getValues can see the
// difference
encOld = motor.getCurrentPosition();
}
protected void turn(int targetDegrees, TurnDirection turnDirection) throws InterruptedException {
switchToReadOnly();
double targetPos = (targetDegrees / DEGREES_PER_ROTATION) * NEVEREST_PPR;
if (turnDirection == TurnDirection.CLOCKWISE) {
switchToReadOnly();
while ((Math.abs(fLeft.getCurrentPosition()) < targetPos
&& Math.abs(fRight.getCurrentPosition()) < targetPos)
|| (Math.abs(bLeft.getCurrentPosition()) < targetPos
&& Math.abs(bRight.getCurrentPosition()) < targetPos)) {
System.out.println(
fLeft.getCurrentPosition()
+ " "
+ fRight.getCurrentPosition()
+ " "
+ bLeft.getCurrentPosition()
+ " "
+ bRight.getCurrentPosition());
System.out.println("Turning " + targetDegrees + " Degrees Clockwise...");
switchToWriteOnly();
fLeft.setPower(SPEED);
fRight.setPower(-SPEED);
bLeft.setPower(SPEED);
bRight.setPower(-SPEED);
switchToReadOnly();
}
switchToWriteOnly();
} else if (turnDirection == TurnDirection.COUNTER_CLOCKWISE) {
switchToReadOnly();
while ((Math.abs(fLeft.getCurrentPosition()) < targetPos
&& Math.abs(fRight.getCurrentPosition()) < targetPos)
|| (Math.abs(bLeft.getCurrentPosition()) < targetPos
&& Math.abs(bRight.getCurrentPosition()) < targetPos)) {
System.out.println(
fLeft.getCurrentPosition()
+ " "
+ fRight.getCurrentPosition()
+ " "
+ bLeft.getCurrentPosition()
+ " "
+ bRight.getCurrentPosition());
System.out.println("Turning " + targetDegrees + " Degrees Counter-Clockwise...");
switchToWriteOnly();
fLeft.setPower(-SPEED);
fRight.setPower(SPEED);
bLeft.setPower(-SPEED);
bRight.setPower(SPEED);
switchToReadOnly();
}
switchToWriteOnly();
}
switchToWriteOnly();
printEncoderValues();
stopMotors();
}
@Override
public void runOpMode() throws InterruptedException {
sweeper = hardwareMap.dcMotor.get("motor_4");
motorLeft = hardwareMap.dcMotor.get("motor_1");
motorRight = hardwareMap.dcMotor.get("motor_2");
climbers = hardwareMap.servo.get("servo_1");
sensorGyro = hardwareMap.gyroSensor.get("gyro");
// sensorRGB = hardwareMap.colorSensor.get("mr");
motorLeft.setDirection(DcMotor.Direction.REVERSE);
wrist = hardwareMap.servo.get("servo_3");
score = hardwareMap.servo.get("servo_1");
climbers = hardwareMap.servo.get("servo_2");
climberknockleft = hardwareMap.servo.get("servo_5");
climberknockright = hardwareMap.servo.get("servo_6");
extend = hardwareMap.servo.get("servo_4");
// button = hardwareMap.servo.get("servo_");
odsleft = hardwareMap.opticalDistanceSensor.get("odsleft");
odsright = hardwareMap.opticalDistanceSensor.get("odsright");
climberknockleft.setPosition(positionclimberknockleftclosed);
climberknockright.setPosition(positionclimberknockrightclosed);
arm = hardwareMap.dcMotor.get("motor_3");
motorRight.setMode(DcMotorController.RunMode.RESET_ENCODERS);
motorLeft.setMode(DcMotorController.RunMode.RESET_ENCODERS);
int Rotation = 1440;
int Amount = 6;
int[] Distance = {
5000, 0, 0, 12000, 8000, 0, 0, 0
}; // int[] Distance = {2000,2050,5000, 12000, 8000, 0, 0, 0};//int[] Distance =
// {2000,2050,3000, 12000, 8000, 4000, 0};
// 4250//11000//10500//11500
// {2000,2050,5000, 12000, 8000, 0, 0, 0}
int i = 0;
int xVal, yVal, zVal = 0;
int heading = 0;
// set the starting 2q S 3qA3eASZzxa saxz of the wrist and neck
positionwrist = Range.clip(positionwrist, ARM_MIN_RANGE, ARM_MAX_RANGE);
positionclimber = Range.clip(positionclimber, ARM_MIN_RANGE, ARM_MAX_RANGE);
positionscore = Range.clip(positionscore, ARM_MIN_RANGE, ARM_MAX_RANGE);
wrist.setPosition(0.5);
score.setPosition(0.9);
climbers.setPosition(0.85);
Boolean stop = false;
sensorGyro.calibrate();
waitForStart();
while (sensorGyro.isCalibrating()) {
Thread.sleep(50);
}
// write some device information (connection info, name and type)
// to the log file.
// get a reference to our GyroSensor object.
resetStartTime();
motorLeft.setDirection(DcMotor.Direction.REVERSE);
motorRight.setDirection(DcMotor.Direction.FORWARD);
// uncomment this to enable sweeper
sweeper.setPower(0.85);
motorRight.setPower(1);
motorLeft.setPower(0.78);
boolean reset = false;
while (opModeIsActive() && this.time < 30) {
if (i < 7) {
telemetry.addData("odsleft", odsleft.getLightDetected());
telemetry.addData("odsright", odsright.getLightDetected());
telemetry.addData("time: ", this.time);
telemetry.addData("i=", i);
if (reset) {
motorLeft.setMode(DcMotorController.RunMode.RESET_ENCODERS);
sleep(50);
motorRight.setMode(DcMotorController.RunMode.RESET_ENCODERS);
sleep(50);
reset = false;
}
if (i == 0) {
// do nothing because already going right way
}
if (i == 1) {
motorLeft.setDirection(DcMotor.Direction.FORWARD);
motorRight.setDirection(DcMotor.Direction.REVERSE);
// change direction to knock down sweeper
}
/*if(i==2)
{
sweeper.setPower(-0.85);
motorLeft.setDirection(DcMotor.Direction.REVERSE);
motorRight.setDirection(DcMotor.Direction.FORWARD);
}
if(i==3)
{sweeper.setPower(-0.85);
motorLeft.setDirection(DcMotor.Direction.REVERSE);
motorRight.setDirection(DcMotor.Direction.FORWARD);
//go parallel to ramp
}*/
if (i == 4) {
sweeper.setPower(-0.85);
motorLeft.setDirection(DcMotor.Direction.FORWARD);
motorRight.setDirection(DcMotor.Direction.REVERSE);
// go to bucket
}
if (i == 4) {
telemetry.addData("here", "here");
motorLeft.setDirection(DcMotor.Direction.FORWARD);
motorRight.setDirection(DcMotor.Direction.REVERSE);
sweeper.setPower(0);
motorLeft.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
motorRight.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
telemetry.addData("odsleft", odsleft.getLightDetected());
telemetry.addData("odsright", odsright.getLightDetected());
if (first) {
starttime = this.time;
first = false;
telemetry.addData("first", "first");
}
drop = true;
condition =
(odsleft.getLightDetected() < distanceleft
|| odsright.getLightDetected() < distanceright)
&& (this.time - starttime < 4.0)
&& opModeIsActive();
if (condition) { // only get 3 seconds to line up so motors don't keep going
/*
* if (odsleft.getLightDetected() < .5)
* {
*
* */
telemetry.addData("odsleft: ", odsleft.getLightDetected());
telemetry.addData("odsright: ", odsright.getLightDetected());
telemetry.addData("time: ", this.time);
telemetry.addData("time taken: ", (this.time - starttime));
if (odsleft.getLightDetected() < distanceleft && opModeIsActive()) {
left = 0;
motorLeft.setPower(0.2);
} else {
left = 1;
motorLeft.setPower(0);
}
if (odsright.getLightDetected() < distanceright && opModeIsActive()) {
right = 0;
motorRight.setPower(0.2);
} else {
right = 1;
motorRight.setPower(0);
}
} else {
motorLeft.setPower(0);
motorRight.setPower(0);
i++;
}
}
if (i != 4) {
motorRight.setTargetPosition(Distance[i]);
motorLeft.setTargetPosition(Distance[i]);
motorRight.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
motorLeft.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
motorLeft.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
}
if (motorRight.getCurrentPosition() >= Distance[i]
&& motorLeft.getCurrentPosition() >= Distance[i]) {
motorRight.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
motorLeft.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
if (i == 2) {
sweeper.setPower(0);
motorLeft.setDirection(DcMotor.Direction.REVERSE);
motorRight.setDirection(DcMotor.Direction.FORWARD);
while ((heading < 30 || heading > 40) && opModeIsActive()) { // 323,// 327
// turn parallel to ramp
motorLeft.setPower(0.8);
motorRight.setPower(-1);
heading = sensorGyro.getHeading();
telemetry.addData("gyro val: ", heading);
}
}
if (i == 3) {
sweeper.setPower(0);
motorLeft.setDirection(DcMotor.Direction.REVERSE);
motorRight.setDirection(DcMotor.Direction.FORWARD);
while ((heading < 277 || heading > 279)
&& opModeIsActive()) { // 270,272//329,331//323,// 327
// turn to dump climbers
motorLeft.setPower(-0.8);
motorRight.setPower(1.0);
heading = sensorGyro.getHeading();
telemetry.addData("gyro val: ", heading);
}
motorLeft.setDirection(DcMotor.Direction.FORWARD);
motorRight.setDirection(DcMotor.Direction.REVERSE);
}
// don't know if this will work
if (i == 6) {
/*
if((servocount>0.02) && opModeIsActive());
{
telemetry.addData("there", "there");
telemetry.addData("climber", servocount);
climbers.setPosition(servocount);
servocount-=0.01;
}
*/
if (true || odsright.getLightDetected() > 0.2) {
climbers.setPosition(0.01);
}
i++;
}
if (i != 4 && i != 6) {
drop = false;
} else {
drop = true;
}
if (!drop) {
motorRight.setPower(0);
motorLeft.setPower(0);
motorRight.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motorLeft.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motorRight.setMode(DcMotorController.RunMode.RESET_ENCODERS);
sleep(50);
motorLeft.setMode(DcMotorController.RunMode.RESET_ENCODERS);
sleep(50);
reset = true;
motorLeft.setPower(1);
motorRight.setPower(1);
sleep(50);
i++;
}
}
} else {
if (!end) {
programtimetaken = this.time;
end = true;
}
motorLeft.setPower(0);
motorRight.setPower(0);
telemetry.addData("done", programtimetaken);
telemetry.addData("odsleft: ", odsleft.getLightDetected());
telemetry.addData("odsright: ", odsright.getLightDetected());
}
}
motorLeft.setDirection(DcMotor.Direction.FORWARD);
motorRight.setDirection(DcMotor.Direction.FORWARD);
}