/**
* This method does all of the things required to make each side of the robot move forward by the
* given amounts. Seriously, It changes the channelMode, It calculates the pulse counts
* Everything! If you understand java and can't use this, you don't deserve to!
*/
public void measuredDrive(double Rdist, double Ldist) {
double frrot = Rdist / frontCirc;
double flrot = Ldist / frontCirc;
double rPow = .5, lPow = .5;
leftMotorf.setTargetPosition((int) flrot * pulsePerRot);
rightMotorf.setTargetPosition((int) frrot * pulsePerRot);
if (!encodersEnabled) {
enableEncoders(true);
}
if (Rdist < 0) {
rPow = -rPow;
} else if (Rdist == 0) {
rPow = 0;
}
if (Ldist < 0) {
lPow = -lPow;
} else if (Ldist == 0) {
lPow = 0;
}
leftMotorf.setPower(frontRatio * lPow);
rightMotorf.setPower(frontRatio * rPow);
}
@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);
}
}
}
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;
}
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 driveEncoder(double leftPower, double rightPower, double ticks) {
rightDrive.setTargetPosition(
(int) ticks); // may need to cast inches to int or long idk also may be in other units
rightDriveB.setTargetPosition((int) ticks);
leftDrive.setTargetPosition((int) ticks);
leftDriveB.setTargetPosition((int) ticks);
rightDrive.setPower(rightPower);
rightDriveB.setPower(rightPower);
leftDrive.setPower(leftPower);
leftDriveB.setPower(leftPower);
}
// 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
}
@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 setCliffElevation() {
cliffElevation.setTargetPosition((int) (climberTheta * ticksPerDegree));
if (cliffRelaxDelay < System.nanoTime() && cliffRelaxDelay > 0) {
cliffRelaxDelay = 0;
cliffElevation.setPower(0);
}
}
/*
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);
}
}
private void moveDistance(double leftY, double rightY, int distance) throws InterruptedException {
rightY = -rightY;
leftbackMotor.setTargetPosition(distance);
rightbackMotor.setTargetPosition(distance);
leftbackMotor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
rightbackMotor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
leftfrontMotor.setPower(leftY);
leftbackMotor.setPower(leftY);
rightfrontMotor.setPower(rightY);
rightbackMotor.setPower(rightY);
if (leftbackMotor.getPower() < 0.1) {
leftfrontMotor.setPower(0.0);
}
if (rightbackMotor.getPower() < 0.1) {
rightfrontMotor.setPower(0.0);
}
}
/*
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 start() {
motorRight.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
motorLeft.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
// currentState = 1;
telemetry.addData("Text", "***Starting***");
motorRight.setTargetPosition(5000);
motorRight.setPower(.75);
// motorLeft.setTargetPosition(2500);
motorLeft.setPower(.75);
}
@Override
public void init() {
// currentState = 1;
knockerRight = hardwareMap.servo.get("knockerRight"); // servo_3
knockerLeft = hardwareMap.servo.get("knockerLeft"); // servo_2
backClimber = hardwareMap.servo.get("backClimber"); // servo_4
// sweeper = hardwareMap.dcMotor.get("motor_5");
hook = hardwareMap.dcMotor.get("hookMotor"); // motor_3
winch = hardwareMap.dcMotor.get("winchMotor"); // motor_4
motorLeft = hardwareMap.dcMotor.get("leftMotor"); // motor_2
motorRight = hardwareMap.dcMotor.get("rightMotor"); // motor_1
// physical motors are mounted 180 from each other to both face out to wheels.
// So, to have robot go forward, one motor needs to be reversed.
motorLeft.setDirection(DcMotor.Direction.REVERSE);
// 1/21/16 - adding (motor mode) Reset Encoders before start loop.
motorRight.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
motorLeft.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
telemetry.addData("Text", "***RESETTING***");
// motorRight.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
// motorLeft.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
motorRight.setTargetPosition(0);
motorRight.setPower(0.0);
motorLeft.setTargetPosition(0);
motorLeft.setPower(0.0);
knockerLeft.setPosition(.52);
knockerRight.setPosition(.52);
backClimber.setPosition(.52);
// REMEMBER: 0.0 to .5 is forward, while .5 to 1.0 is backwards.
}
public void loop() {
arm.setPower(1);
arm.setTargetPosition(3500);
if (getRuntime() > 5) {
revMotor.setPower(0 - 1 * (1.3 / 3));
if (getRuntime() > 10) launcher.setPosition(0);
}
if (getRuntime() > 11) {
launcher.setPosition(1);
revMotor.setPower(0);
}
if ((getRuntime() > 12) && (getRuntime() < 14)) {
arm.setTargetPosition(0);
arm.setPower(-1);
motorBackLeft.setPower(-1);
motorFrontLeft.setPower(0.6);
motorBackRight.setPower(1);
motorBackRight.setPower(-0.6);
telemetry.addData("I'm doing it", arm.isBusy());
}
telemetry.addData("amIBusy()", arm.isBusy());
telemetry.update();
}
public void setCliffHangerExtension(double metersOut) {
cliffHanger1.setTargetPosition(calcCliffHangerTarget(metersOut));
cliffHanger2.setTargetPosition(cliffHanger1.getTargetPosition());
}
@Override
public void setTargetPosition(int position) {
dcMotor.setTargetPosition(position);
}
@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);
}
