@Override
public void loop() {
if (this.time < 6.5d) {
goForward(-1.0);
} else if (this.time < 8.0d) {
turnRight(1.0);
} else if (this.time < 11d) {
goForward(-1.0);
} else if (this.time < 14d) {
turnRight(1.0);
armUp = true;
motorArm.setPower(0.5);
if (this.time > 11.2d) {
motorArm.setPower(0.1);
}
} else if (this.time < 16) {
goForward(1.0);
motorSlide.setPower(1.0);
} else if (this.time < 18) {
stopMotors();
motorSlide.setPower(0.0);
forwardBack.setPosition(0.75);
} else if (this.time < 22) {
motorSlide.setPower(-1.0);
forwardBack.setPosition(0.5);
}
telemetry.addData("Text", "*** Robot Data***");
telemetry.addData("..", "time: " + String.format("%.2f", this.time));
}
@Override
public void init() {
/* Assigns DcMotor objects to physical motors
using Hardware Mapping */
lbMotor = hardwareMap.dcMotor.get("lbMotor");
lfMotor = hardwareMap.dcMotor.get("lfMotor");
rfMotor = hardwareMap.dcMotor.get("rfMotor");
rbMotor = hardwareMap.dcMotor.get("rbMotor");
hangRotateMotor = hardwareMap.dcMotor.get("hangRotateMotor");
hangSlideMotor = hardwareMap.dcMotor.get("hangSlideMotor");
armMotor = hardwareMap.dcMotor.get("armMotor");
/* Assigns Servo objects to physical servos
using Hardware Mapping */
climber = hardwareMap.servo.get("climber");
rSwitch = hardwareMap.servo.get("rSwitch");
lSwitch = hardwareMap.servo.get("lSwitch");
/* resets Servo positions */
climber.setPosition(CLIMBER_DOWN_POSITION);
rSwitch.setPosition(RSWITCH_CLOSED_POSITION);
lSwitch.setPosition(LSWITCH_CLOSED_POSITION);
/* runs backward motors in opposite direction */
lfMotor.setDirection(DcMotor.Direction.REVERSE);
lbMotor.setDirection(DcMotor.Direction.REVERSE);
telemetry.addData("init complete", "");
}
@Override
public void init() {
// Treads.
rightTread = hardwareMap.dcMotor.get("rightTread");
leftTread = hardwareMap.dcMotor.get("leftTread");
leftTread.setDirection(DcMotor.Direction.REVERSE);
rightTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);
leftTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);
// Servos.
rightPusher = hardwareMap.servo.get("rightPusher");
leftPusher = hardwareMap.servo.get("leftPusher");
rightBumper = hardwareMap.servo.get("rightBumper");
leftBumper = hardwareMap.servo.get("leftBumper");
climber = hardwareMap.servo.get("climber");
rightPusher.setPosition(NeverlandServoConstants.RIGHT_PUSHER_STOWED);
leftPusher.setPosition(NeverlandServoConstants.LEFT_PUSHER_STOWED);
rightBumper.setPosition(NeverlandServoConstants.RIGHT_BUMPER_DOWN);
leftBumper.setPosition(NeverlandServoConstants.LEFT_BUMPER_DOWN);
climber.setPosition(NeverlandServoConstants.CLIMBER_STORE);
// Beacon arms.
BeaconArms beacon = new BeaconArms(rightPusher, leftPusher, true);
}
public void smoothDump(ElapsedTime timer) {
moveStraight(8.5, false, .3);
double pos = Keys.CLIMBER_INITIAL_STATE;
// .85 to .31 so you want to decrement
timer.reset();
telemetry.addData("place", "before while");
while (pos > Keys.CLIMBER_DUMP) {
timer.reset();
while (timer.time() * 1000 < 200) {
// do nothing
try {
sleep(2);
} catch (InterruptedException e) {
e.printStackTrace();
}
telemetry.addData("waiting", timer.time() * 1000);
}
pos -= .05;
climber.setPosition(pos);
/*telemetry.addData("place","during while");
telemetry.addData("timer",timer.time());
telemetry.addData("timer math",((int)(timer.time()*1000)));
telemetry.addData("timer math2",((int)(timer.time()*1000))%200);
telemetry.addData("climber",climber.getPosition());
telemetry.addData("constant","INIT"+Keys.CLIMBER_INITIAL_STATE+" DUMP"+Keys.CLIMBER_DUMP);*/
}
// once it is here, it finished dumping.
// retract - the sudden should be ok cuz hopefully by that time it will have already dumped
climber.setPosition(Keys.CLIMBER_INITIAL_STATE);
moveStraight(8.5, true, .3);
telemetry.addData("place", "after while");
}
@Override
public void loop() {
// Gamepad 1 - Drive Controller
float rightDriveMotorSpeed = -gamepad1.right_stick_y;
float leftDriveMotorSpeed = -gamepad1.left_stick_y;
rightDriveMotorSpeed = Range.clip(rightDriveMotorSpeed, -1, 1);
leftDriveMotorSpeed = Range.clip(leftDriveMotorSpeed, -1, 1);
rightDriveMotorSpeed = (float) scaleInput(rightDriveMotorSpeed);
leftDriveMotorSpeed = (float) scaleInput(leftDriveMotorSpeed);
System.out.println(rightDriveMotorSpeed);
driveRight.setPower(rightDriveMotorSpeed);
driveLeft.setPower(leftDriveMotorSpeed);
if (gamepad1.a & hookServoClick == 0) {
hookServoPosition -= hookServoDelta;
System.out.println(hookServoPosition);
hookServoClick = 1;
hookServo.setPosition(hookServoPosition);
}
if (gamepad1.a & hookServoClick == 1) {
hookServoPosition += hookServoDelta;
System.out.println(hookServoPosition);
hookServoClick = 0;
hookServo.setPosition(hookServoPosition);
}
if (gamepad1.x & triggerServoClick == 0) {
triggerServoPosition -= triggerServoDelta;
System.out.println(triggerServoPosition);
triggerServoClick = 1;
triggerServo.setPosition(triggerServoPosition);
}
if (gamepad1.x & triggerServoClick == 1) {
triggerServoPosition += triggerServoDelta;
System.out.println(triggerServoPosition);
triggerServoClick = 0;
triggerServo.setPosition(triggerServoPosition);
}
// Gamepad 2
float armStage1MotorSpeed = -gamepad2.right_stick_y;
float armStage2MotorSpeed = -gamepad2.left_stick_y;
armStage1MotorSpeed = Range.clip(armStage1MotorSpeed, -1, 1);
armStage2MotorSpeed = Range.clip(armStage2MotorSpeed, -1, 1);
armStage1MotorSpeed = (float) scaleInput(armStage1MotorSpeed);
armStage2MotorSpeed = (float) scaleInput(armStage2MotorSpeed);
armStage1.setPower(armStage1MotorSpeed);
armStage2.setPower(armStage2MotorSpeed);
}
public void init(HardwareMap hardwareMap) {
leftClimberServo = hardwareMap.servo.get("leftClimberServo");
leftClimberServo.setPosition(leftInitialPosition);
rightClimberServo = hardwareMap.servo.get("rightClimberServo");
rightClimberServo.setPosition(rightInitialPosition);
}
private void armDrive(double armPosition, long sleepAmount) throws InterruptedException {
climbers.setPosition(armPosition); // set the arm power according to the double armPower
sleep(sleepAmount); // sleep for a certain amount of milliseconds
climbers.setPosition(1.0); // stop the motors
}
@Override
public void init() {
// Drive Motors
driveRight = hardwareMap.dcMotor.get("driveRight");
driveLeft = hardwareMap.dcMotor.get("driveLeft");
// Arm Motors
armStage1 = hardwareMap.dcMotor.get("armStage1");
armStage2 = hardwareMap.dcMotor.get("armStage2");
// Servos
hookServo = hardwareMap.servo.get("hookServo");
triggerServo = hardwareMap.servo.get("triggerServo");
// Controllers
driveController = hardwareMap.dcMotorController.get("driveController");
armController = hardwareMap.dcMotorController.get("armController");
servoController = hardwareMap.servoController.get("servoController");
// sets servo Pos
hookServoPosition = 0.2;
triggerServoPosition = 0.2;
hookServo.setPosition(hookServoPosition);
triggerServo.setPosition(triggerServoPosition);
}
@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);
}
/* Initialize standard Hardware interfaces */
public void init(HardwareMap ahwMap) {
// Save reference to Hardware map
hwMap = ahwMap;
// Define and Initialize Motors
leftMotor = hwMap.dcMotor.get("left_drive");
rightMotor = hwMap.dcMotor.get("right_drive");
armMotor = hwMap.dcMotor.get("left_arm");
leftMotor.setDirection(DcMotor.Direction.FORWARD); // Set to REVERSE if using AndyMark motors
rightMotor.setDirection(DcMotor.Direction.REVERSE); // Set to FORWARD if using AndyMark motors
// Set all motors to zero power
leftMotor.setPower(0);
rightMotor.setPower(0);
armMotor.setPower(0);
// Set all motors to run without encoders.
// May want to use RUN_USING_ENCODERS if encoders are installed.
leftMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
rightMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
armMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// Define and initialize ALL installed servos.
leftClaw = hwMap.servo.get("left_hand");
rightClaw = hwMap.servo.get("right_hand");
leftClaw.setPosition(MID_SERVO);
rightClaw.setPosition(MID_SERVO);
}
/*
* This method will be called repeatedly in a loop
*
* @see com.qualcomm.robotcore.eventloop.opmode.OpMode#run()
*/
@Override
public void loop() {
double reflection = 0.0;
double left, right = 0.0;
// keep manipulator out of the way.
arm.setPosition(armPosition);
claw.setPosition(clawPosition);
/*
* Use the 'time' variable of this op mode to determine
* how to adjust the motor power.
*/
if (this.time <= 1) {
// from 0 to 1 seconds, run the motors for five seconds.
left = 0.15;
right = 0.15;
} else if (this.time > 5 && this.time <= 8.5) {
// between 5 and 8.5 seconds, point turn right.
left = 0.15;
right = -0.15;
} else if (this.time > 8.5 && this.time <= 15) {
// between 8 and 15 seconds, idle.
left = 0.0;
right = 0.0;
} else if (this.time > 15d && this.time <= 20.75d) {
// between 15 and 20.75 seconds, point turn left.
left = -0.15;
right = 0.15;
} else {
// after 20.75 seconds, stop.
left = 0.0;
right = 0.0;
}
/*
* set the motor power
*/
motorRight.setPower(left);
motorLeft.setPower(right);
/*
* read the light sensor.
*/
// reflection = reflectedLight.getLightLevel();
/*
* Send telemetry data back to driver station. Note that if we are using
* a legacy NXT-compatible motor controller, then the getPower() method
* will return a null value. The legacy NXT-compatible motor controllers
* are currently write only.
*/
telemetry.addData("Text", "*** Robot Data***");
telemetry.addData("time", "elapsed time: " + Double.toString(this.time));
telemetry.addData("reflection", "reflection: " + Double.toString(reflection));
telemetry.addData("left tgt pwr", "left pwr: " + Double.toString(left));
telemetry.addData("right tgt pwr", "right pwr: " + Double.toString(right));
}
public void operate(Servo ser, double position, long wait) {
ser.setPosition(position);
try {
Thread.sleep(wait);
} catch (Exception e) {
System.err.println(e);
System.exit(1);
}
ser.setPosition(0);
}
// initialize servos
public void init_servos() {
//
redNoodleServo = hardwareMap.servo.get("redNoodle");
blueNoodleServo = hardwareMap.servo.get("blueNoodle");
shoulderRatchet = hardwareMap.servo.get("shoulderRatchet");
tapeRatchet = hardwareMap.servo.get("tapeRatchet");
redNoodleServo.setPosition(redNoodleDown);
blueNoodleServo.setPosition(blueNoodleDown);
tapeRatchet.setPosition(tapeRatchetOpen);
shoulderRatchet.setPosition(shoulderRatchetOpen);
}
public void loop() {
resetServos();
moveSwiv();
swivLeft.setPosition(leftSwivPos);
swivRight.setPosition((rightSwivPos));
updateGamepadTelemetry();
float direction = gamepad1.right_stick_y;
direction = Range.clip(direction, -1, 1);
arm.setPosition(scaleContinuous(-gamepad1.right_stick_y));
}
/*
* Function to automatically move the hanging arm.
* Should be called in a loop.
* @return returns true when the arm is in the process of moving.
*/
public boolean hangingAutomation() {
double hang1Position = Math.max(srvoHang_1.getPosition() - HangServoDelta, HANG1_MIN_RANGE);
srvoHang_1.setPosition(hang1Position);
// when big servo is midway through it's travel from min to max...
if (srvoHang_1.getPosition() < (HANG1_MAX_RANGE + HANG1_MIN_RANGE) / 2) {
// tell the big servo to go to it's max range (as fast as possible)
srvoHang_1.setPosition(HANG1_MAX_RANGE);
// start the small servo to create a lunging effect
srvoHang_2.setPosition(HANG2_MAX_RANGE);
return false;
}
return true;
}
/**
* Moves right climber servo based on {@code enum rightClimberDirection} value set in {@link
* DriverStation#getNextClimberCmd()}
*
* @param drvrcmd {@link DriverCommand} object with values.
*/
public void applyDSCmd(DriverCommand drvrcmd) {
switch (drvrcmd.rightClimberCmd.rightClimberDirection) {
case DOWN:
rightClimber.setPosition(0.5);
break;
case UP:
rightClimber.setPosition(0);
break;
case NONE:
break;
default:
break;
}
}
public void setPosition(Direction direction) {
if (verticalPostiiton == Direction.MID) {
switch (direction) {
case UP:
servoLeft.setPosition(leftUP);
servoRight.setPosition(rightUP);
break;
case MID:
servoLeft.setPosition(leftMID);
servoRight.setPosition(rightMID);
break;
case DOWN:
servoLeft.setPosition(leftDOWN);
servoRight.setPosition(rightDOWN);
break;
default:
throw new IllegalArgumentException("Direction must be UP, DOWN, MID!");
}
horizontalPosition = direction;
} else {
servoLeft.setPosition(leftMID);
servoRight.setPosition(rightMID);
horizontalPosition = Direction.MID;
}
}
/*
* handleServos
*
* Oscillate the servos.
*/
protected void handleServos() {
if ((firstServos) || (servoOscTimer.time() > SERVO_OSC_FREQ)) {
if (servoPosition == 0.0) {
servoPosition = 1.0;
} else {
servoPosition = 0.0;
}
servo1.setPosition(servoPosition);
servo2.setPosition(servoPosition);
servo3.setPosition(servoPosition);
servo4.setPosition(servoPosition);
servoOscTimer.reset();
firstServos = false;
}
}
@Override
public void init() {
motorHangL = hardwareMap.dcMotor.get("motorHangL");
motorHangR = hardwareMap.dcMotor.get("motorHangR");
motorSpinner = hardwareMap.dcMotor.get("manipulator");
ramp = hardwareMap.servo.get("ramp");
drop = hardwareMap.servo.get("drop");
claw = hardwareMap.servo.get("claw");
motorHangL.setPower(0);
motorHangR.setPower(0);
motorSpinner.setPower(0);
boxBelt.setPosition(.5);
ramp.setPosition(0);
drop.setPosition(0);
}
public Climber_Savers(LinearOpMode opMode) throws InterruptedException {
this.opMode = opMode;
opMode.telemetry.addData("ClimberSavers", "constructor");
// get hardware mappings
climberSaverServo = opMode.hardwareMap.servo.get("ClimberSaverServo");
climberSaverServo.setPosition(STARTPOSITION);
}
/*
* This method will be called repeatedly in a loop
*
* @see com.qualcomm.robotcore.eventloop.opmode.OpMode#run()
*/
@Override
public void loop() {
/*
* Gamepad 1
*
* Gamepad 1 controls the motors via the left stick, and it controls the
* wrist/claw via the a,b, x, y buttons
*/
// throttle: left_stick_y ranges from -1 to 1, where -1 is full up, and
// 1 is full down
// direction: left_stick_x ranges from -1 to 1, where -1 is full left
// and 1 is full right
float left_throttle = -gamepad1.left_stick_y;
float right_throttle = -gamepad1.right_stick_y;
// scale the joystick value to make it easier to control
// the robot more precisely at slower speeds.
left_throttle = (float) scaleInput(left_throttle);
right_throttle = (float) scaleInput(right_throttle);
// write the values to the motors
front_right.setPower(right_throttle);
back_right.setPower(right_throttle);
front_left.setPower(left_throttle);
back_left.setPower(left_throttle);
// update the position of the arm.
if (gamepad1.y) {
// if the A button is pushed on gamepad1, increment the position of
// the arm servo.
spinnerPosition += spinnerDelta;
}
if (gamepad1.a) {
// if the Y button is pushed on gamepad1, decrease the position of
// the arm servo.
spinnerPosition -= spinnerDelta;
}
// clip the position values so that they never exceed their allowed range.
spinnerPosition = Range.clip(spinnerPosition, 0.0, 1.0);
// write position values to the wrist and claw servo
spinner.setPosition(spinnerPosition);
/*
* Send telemetry data back to driver station. Note that if we are using
* a legacy NXT-compatible motor controller, then the getPower() method
* will return a null value. The legacy NXT-compatible motor controllers
* are currently write only.
*/
telemetry.addData("Text", "*** Robot Data***");
telemetry.addData("arm", "arm: " + String.format("%.2f", armPosition));
telemetry.addData("claw", "claw: " + String.format("%.2f", clawPosition));
telemetry.addData("left tgt pwr", "left pwr: " + String.format("%.2f", left));
telemetry.addData("right tgt pwr", "right pwr: " + String.format("%.2f", right));
}
public void moveVertical(Direction direction) {
switch (direction) {
case UP:
setPosition(Direction.MID);
servoVertical.setPosition(verticalUP);
break;
case MID:
servoVertical.setPosition(verticalMID);
break;
case DOWN:
setPosition(Direction.MID);
servoVertical.setPosition(verticalDOWN);
break;
default:
throw new IllegalArgumentException("Direction must be UP, DOWN, MID!");
}
verticalPostiiton = direction;
}
@Override
public void runOpMode() throws InterruptedException {
initMotors();
waitForStart();
// Autonomous starts here
// Wait for motors to calibrate
Thread.sleep(1000);
// Drive to floorgoal, backwards
MotorRunner.run(
this,
new DcMotor[] {motorLeft, motorRight},
-Power.FULL_SPEED,
new TimeUnit(Values.DRIVE_FLOORGOAL));
// Turn to align with floor goal
MotorRunner.run(this, motorRight, Power.FULL_SPEED, new TimeUnit(Values.TURN_FLUSH));
// Attempt to Deliver Climbers
dump.setPosition(Values.DUMP_DOWN);
Thread.sleep(1000);
dump.setPosition(Values.DUMP_UP);
// Turn to align with mountain
motorLeft.setPower(-Power.FULL_SPEED);
MotorRunner.run(this, motorRight, Power.FULL_SPEED, new TimeUnit(Values.TURN_MOUNTAIN));
motorRight.setPower(0);
// Drive up mountain
MotorRunner.run(
this,
new DcMotor[] {motorLeft, motorRight},
Power.FULL_SPEED,
new TimeUnit(Values.DRIVE_MOUNTAIN));
// Try to grab a churro
MotorRunner.run(this, tape1, Power.SLOW_SPEED, new TimeUnit(Values.SEND_TAPE));
motorLeft.setPower(Power.FULL_SPEED);
motorRight.setPower(Power.FULL_SPEED);
for (int i = 0; i < Values.TAPE_TIMES; i++) {
MotorRunner.run(this, tape1, -Power.NORMAL_SPEED, new TimeUnit(Values.RETRACT_TAPE));
Thread.sleep(250);
}
stopMotors();
}
/**
* Moves right climber servo based on {@code enum rightClimberDirection} value set in {@link
* DriverStation#getNextClimberCmd()}
*
* @param drvrcmd {@link DriverCommand} object with values.
*/
public void applyDSCmd(DriverCommand drvrcmd) {
if (!rightClimberAvailable) {
return;
}
double rightClimberPosition = rightClimber.getPosition();
DbgLog.msg(String.format("RightClimber Position = %f", rightClimberPosition));
switch (drvrcmd.rightClimberCmd.rightClimberStatus) {
case -1:
rightClimber.setPosition(Range.clip(0, 0, 1));
break;
case 1:
rightClimber.setPosition(Range.clip(1, 0, 1));
break;
case 0:
break;
default:
break;
}
}
/*
* Code to run when the op mode is first enabled goes here
*
* @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
*/
@Override
public void init() {
// Setup motor and reverse direction of left motor
motorRight = hardwareMap.dcMotor.get("motor_3");
motorLeft = hardwareMap.dcMotor.get("motor_4");
motorLeft.setDirection(DcMotor.Direction.REVERSE);
motorArm = hardwareMap.dcMotor.get("motor_1");
motorSlide = hardwareMap.dcMotor.get("motor_2");
rightLeft = hardwareMap.servo.get("servo_3");
forwardBack = hardwareMap.servo.get("servo_4");
clawGrab = hardwareMap.servo.get("servo_5");
grabPos = clawGrab.getPosition();
while (grabPos < 0.9) {
grabPos += 0.1;
clawGrab.setPosition(grabPos);
}
rightLeft.setPosition(0.2);
forwardBack.setPosition(0.20);
new ArmStopper().start();
}
@Override
public void loop() {
telemetry.addData("!WARNING! ", "!DANGEROUS SERVO TESTING! - " + mode);
changeMode();
theDumper.setPosition(Range.clip(lDrivePower, 0.0, 1.0));
if (!mode) {
lDrivePower -= 0.01;
}
if (mode) {
lDrivePower += 0.01;
}
}
@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();
}
@Override
public void init() {
// Ints hardware
motorRight = hardwareMap.dcMotor.get("motorR");
motorLeft = hardwareMap.dcMotor.get("motorL");
trackLifter = hardwareMap.dcMotor.get("trkLftr");
armExtend = hardwareMap.dcMotor.get("armExtend");
// servos
// servoBeaconPusher = hardwareMap.servo.get("beacon_pusher");
servoClimberDumper = hardwareMap.servo.get("climber_dumper");
// climberDoor = hardwareMap.servo.get("climberDoor");
servoDist = hardwareMap.servo.get("servoDist");
climberTriggerLeft = hardwareMap.servo.get("trigLeft");
climberTriggerRight = hardwareMap.servo.get("trigRight");
cowCatcher = hardwareMap.servo.get("cowCatcher");
armLock = hardwareMap.servo.get("armLock");
trackLock = hardwareMap.servo.get("trackLock");
// zipTieSweeper = hardwareMap.servo.get("zipTieSweeper");
ColorSense = hardwareMap.colorSensor.get("color");
nearMountainSwitch = hardwareMap.digitalChannel.get("nearMtnSw");
redBlueSwitch = hardwareMap.digitalChannel.get("rbSw");
ultraSense = hardwareMap.ultrasonicSensor.get("ultraSense");
gyroSense = hardwareMap.gyroSensor.get("gyro");
liftCheck = hardwareMap.touchSensor.get("liftCheck");
servoClimberDumper.setPosition(0.0);
// WHATEVER WE ARE GOING TO CALL THIS THING =
// hardwareMap.opticalDistanceSensor.get("STUFFANDTHINGS");
nearMountainFlag = nearMountainSwitch.getState();
// set up motors
motorLeft.setDirection(DcMotor.Direction.FORWARD);
motorRight.setDirection(DcMotor.Direction.REVERSE);
trackLifter.setDirection(DcMotor.Direction.REVERSE);
armExtend.setDirection(DcMotor.Direction.REVERSE);
motorRight.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
motorLeft.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
trackLifter.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
armExtend.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
cowCatcher.setPosition(0.1);
climberTriggerLeft.setPosition(0.8);
climberTriggerRight.setPosition(0.0);
armLock.setPosition(0.5);
servoDist.setPosition(0.5);
trackLock.setPosition(0.45);
}
/**
* Initializes hardware devices and ties them to the hardware map. Also sets starting directions
* and initialization positions for servos and motors. Initializes a new Simulacrum object.
*
* @see com.qualcomm.robotcore.eventloop.opmode.OpMode
*/
@SuppressLint("SimpleDateFormat")
public void init() {
leftWheels = hardwareMap.dcMotor.get("motor_2");
rightWheels = hardwareMap.dcMotor.get("motor_1");
armExtender2 = hardwareMap.dcMotor.get("motor_6");
hangMotor = hardwareMap.dcMotor.get("motor_4");
liftMotor = hardwareMap.dcMotor.get("motor_3");
armExtender = hardwareMap.dcMotor.get("motor_5");
leftBucket = hardwareMap.servo.get("servo_1");
rightBucket = hardwareMap.servo.get("servo_2");
collectorDrive = hardwareMap.servo.get("servo_3");
collectorDrive2 = hardwareMap.servo.get("servo_4");
climberCarrierServo = hardwareMap.servo.get("servo_5");
leftClimberServo = hardwareMap.servo.get("servo_6");
rightClimberServo = hardwareMap.servo.get("servo_7");
leftWheels.setDirection(DcMotor.Direction.REVERSE);
armExtender.setDirection(DcMotor.Direction.REVERSE);
leftBucket.setDirection(Servo.Direction.REVERSE);
rightBucket.setDirection(Servo.Direction.FORWARD);
leftClimberServo.setDirection(Servo.Direction.REVERSE);
rightClimberServo.setDirection(Servo.Direction.FORWARD);
leftBucket.setPosition(0.5);
rightBucket.setPosition(0.5);
collectorDrive.setPosition(0.5);
collectorDrive2.setPosition(0.5);
rightClimberServo.setPosition(0.05);
leftClimberServo.setPosition(0.05);
climberCarrierServo.setPosition(1.0);
try {
readWriteObj.writeInit();
} catch (IOException e) {
e.printStackTrace();
telemetry.addData("Fatal Error", "IOException @ Init");
}
}