@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 runOpMode() throws InterruptedException {
leftDrive = hardwareMap.dcMotor.get("leftDrive");
rightDrive = hardwareMap.dcMotor.get("rightDrive");
leftDriveB = hardwareMap.dcMotor.get("leftDriveB");
rightDriveB = hardwareMap.dcMotor.get("rightDriveB");
climberServo = hardwareMap.servo.get("climberServo");
rightDrive.setDirection(DcMotor.Direction.REVERSE);
rightDriveB.setDirection(DcMotor.Direction.REVERSE);
leftDrive.setChannelMode(DcMotor.RunModeController.RUN_TO_POSITION);
rightDrive.setChannelMode(DcMotor.RunModeController.RUN_TO_POSITION);
leftDriveB.setChannelMode(DcMotor.RunModeController.RUN_TO_POSITION);
rightDriveB.setChannelMode(DcMotor.RunModeController.RUN_TO_POSITION);
waitForStart();
drive(-0.5, -0.5, 2.4);
// drive(0.5, -0.5, 0.7);
// drive(-0.5, -0.5, 3);
turn(RIGHT, 45);
// drive(-0.5, -0.5, 1);
// climberServo.setPosition(0.7);
// Thread.sleep((long)(0.5));
// climberServo.setPosition(0.5);
}
@Override
public void init() {
leftMotor = hardwareMap.dcMotor.get("left_drive");
rightMotor = hardwareMap.dcMotor.get("right_drive");
navx_device =
AHRS.getInstance(
hardwareMap.deviceInterfaceModule.get("dim"),
NAVX_DIM_I2C_PORT,
AHRS.DeviceDataType.kProcessedData,
NAVX_DEVICE_UPDATE_RATE_HZ);
rightMotor.setDirection(DcMotor.Direction.REVERSE);
/* If possible, use encoders when driving, as it results in more */
/* predictable drive system response. */
leftMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
rightMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
/* Create a PID Controller which uses the Yaw Angle as input. */
yawPIDController =
new navXPIDController(navx_device, navXPIDController.navXTimestampedDataSource.YAW);
/* Configure the PID controller */
yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES);
yawPIDController.setContinuous(true);
yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE);
yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES);
yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D);
yawPIDController.enable(true);
first_iteration = true;
}
@Override
public void init_loop() {
// Reverses the Right Wheel
driveRight.setDirection(DcMotor.Direction.REVERSE);
// Sets motors to run without incoders
driveLeft.setChannelMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
driveRight.setChannelMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
}
/*
Constructor method that initializes most of the variables and objects
@param left the left side motors
@param right the right side motors
@param WheelDiameter the diameter of the wheel
@param GearRatio the gear ratio
*/
public MovementAuto(DcMotor left, DcMotor right, int WheelDiameter, double GearRatio) {
leftMotor = left;
rightMotor = right;
wheelDiameter = WheelDiameter;
gearRatio = GearRatio;
rightMotor.setDirection(DcMotor.Direction.REVERSE);
leftMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
rightMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
}
/** if val is true, enables encoders, else disables them */
public void enableEncoders(boolean val) {
if (val) {
leftMotorf.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
rightMotorf.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
encodersEnabled = true;
} else {
leftMotorf.setChannelMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
rightMotorf.setChannelMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
encodersEnabled = false;
}
}
@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);
}
private void a() {
Iterator var1 = this.hardwareMap.servoController.iterator();
while (var1.hasNext()) {
ServoController var2 = (ServoController) var1.next();
var2.pwmDisable();
}
var1 = this.hardwareMap.dcMotorController.iterator();
while (var1.hasNext()) {
DcMotorController var3 = (DcMotorController) var1.next();
var3.setMotorControllerDeviceMode(DeviceMode.WRITE_ONLY);
}
var1 = this.hardwareMap.dcMotor.iterator();
while (var1.hasNext()) {
DcMotor var4 = (DcMotor) var1.next();
var4.setPower(0.0D);
var4.setChannelMode(RunMode.RUN_WITHOUT_ENCODERS);
}
var1 = this.hardwareMap.lightSensor.iterator();
while (var1.hasNext()) {
LightSensor var5 = (LightSensor) var1.next();
var5.enableLed(false);
}
}
public void resetEncoders() {
motor_1.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
motor_2.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
motor_3.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
motor_4.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
motor_1.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motor_2.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motor_3.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motor_4.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
}
@Override
public void init() {
// Get hardware references
leftBackMotor = hardwareMap.dcMotor.get("back_left_drive");
leftFrontMotor = hardwareMap.dcMotor.get("front_left_drive");
rightBackMotor = hardwareMap.dcMotor.get("back_right_motor");
rightFrontMotor = hardwareMap.dcMotor.get("front_right_motor");
leftController = hardwareMap.dcMotorController.get("right_controller");
rightController = hardwareMap.dcMotorController.get("left_controller");
// Reverses right motors
rightBackMotor.setDirection(DcMotor.Direction.REVERSE);
leftFrontMotor.setDirection(DcMotor.Direction.REVERSE);
leftBackMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
leftFrontMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
rightBackMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
rightFrontMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
}
/*
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 init() {
/*
* Use the hardwareMap to get the dc motors and servos by name. Note
* that the names of the devices must match the names used when you
* configured your robot and created the configuration file.
*/
motor_1 = hardwareMap.dcMotor.get("motor_1"); // right side
motor_2 = hardwareMap.dcMotor.get("motor_2"); // right side
motor_3 = hardwareMap.dcMotor.get("motor_3"); // left side
motor_4 = hardwareMap.dcMotor.get("motor_4"); // left side
motor_harvest = hardwareMap.dcMotor.get("motor_harvest");
motor_harvest2 = hardwareMap.dcMotor.get("motor_harvest2");
servo_1 = hardwareMap.servo.get("servo_1");
servo_2 = hardwareMap.servo.get("servo_2");
motor_1.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motor_2.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motor_3.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motor_4.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motor_harvest.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motor_harvest2.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
}
@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 resetEncoders() {
rf.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
rb.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
lf.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
lb.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
}
@Override
public void runOpMode() {
/*
* Use the hardwareMap to get the dc motors and servos by name. Note
* that the names of the devices must match the names used when you
* configured your robot and created the configuration file.
*/
rf = hardwareMap.dcMotor.get("rf"); // right
rb = hardwareMap.dcMotor.get("rb");
lf = hardwareMap.dcMotor.get("lf"); // left
lb = hardwareMap.dcMotor.get("lb");
lift1 = hardwareMap.dcMotor.get("lift"); // RIGHT lift
lift2 = hardwareMap.dcMotor.get("lift2"); // LEFT lift
harvester = hardwareMap.dcMotor.get("harvester");
retractor = hardwareMap.dcMotor.get("retractor");
harvester.setChannelMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
lift2.setDirection(DcMotor.Direction.REVERSE);
basket = hardwareMap.servo.get("basket"); // basket servo
door = hardwareMap.servo.get("door");
// waitForStart();
// Straight 4 tiles in order to line up with the climber box
rb.setPower(1.0);
rf.setPower(1.0);
lf.setPower(-1.0);
lb.setPower(-1.0);
while (rf.getCurrentPosition() < 5040 && lf.getCurrentPosition() > -5040) {}
rf.setPower(0.0);
rb.setPower(0.0);
lf.setPower(0.0);
lb.setPower(0.0);
resetEncoders();
// turn left to face away from basket
rb.setPower(1.0);
rf.setPower(1.0);
lf.setPower(1.0);
lb.setPower(1.0);
while (rf.getCurrentPosition() < 840 && lf.getCurrentPosition() < 840) {}
rf.setPower(0.0);
rb.setPower(0.0);
lf.setPower(0.0);
lb.setPower(0.0);
resetEncoders();
// Straight 3 tiles in order to go to climber box
rf.setPower(-1.0);
rb.setPower(-1.0);
lf.setPower(1.0);
lb.setPower(1.0);
while (rf.getCurrentPosition() < 3360 && lf.getCurrentPosition() > -3360) {}
rf.setPower(0.0);
rb.setPower(0.0);
lf.setPower(0.0);
lb.setPower(0.0);
resetEncoders();
// lift basket towards box
lift1.setPower(0.7);
lift2.setPower(0.7);
retractor.setPower(0.7);
door.setPosition(0.8);
while (lift1.getCurrentPosition() < 2800 && lift2.getCurrentPosition() < 2800) {}
lift1.setPower(0.0);
lift2.setPower(0.0);
retractor.setPower(0.0);
// turn basket to score
basket.setPosition(0.8);
door.setPosition(0.1);
while (basket.getPosition() != 90) {}
try {
wait(3000L);
} catch (InterruptedException e) {
}
basket.setPosition(0);
while (basket.getPosition() != 0) {}
// lower slides
lift1.setPower(-0.5);
lift2.setPower(-0.5);
retractor.setPower(-0.5);
door.setPosition(0.3);
while (lift1.getCurrentPosition() > 0 && lift2.getCurrentPosition() > 0) {}
lift1.setPower(0.0);
lift2.setPower(0.0);
retractor.setPower(0.0);
// head straight 3 to the opposite ramp
rf.setPower(1.0);
rb.setPower(1.0);
lf.setPower(-1.0);
lb.setPower(-1.0);
while (rf.getCurrentPosition() < 2940 && lf.getCurrentPosition() > -2940) {}
rf.setPower(0.0);
rb.setPower(0.0);
lf.setPower(0.0);
lb.setPower(0.0);
resetEncoders();
// turn left to face away from ramp
rf.setPower(1.0);
rb.setPower(1.0);
lf.setPower(1.0);
lb.setPower(1.0);
while (rf.getCurrentPosition() < 1260 && lf.getCurrentPosition() < 1260) {}
rf.setPower(0.0);
rb.setPower(0.0);
lf.setPower(0.0);
lb.setPower(0.0);
resetEncoders();
// GO ONTO THE RAMP
rf.setPower(-1.0);
rb.setPower(-1.0);
lf.setPower(1.0);
lb.setPower(1.0);
while (rf.getCurrentPosition() > -2940 && lb.getCurrentPosition() < 2940) {}
rf.setPower(0.0);
rb.setPower(0.0);
lf.setPower(0.0);
lb.setPower(0.0);
resetEncoders();
try {
wait(30000L);
} catch (InterruptedException e) {
}
}
public void resetEncoders() {
motorBR.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
motorBL.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
motorFR.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
motorFL.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
}
@Override
public void init() {
motor1 = hardwareMap.dcMotor.get("motor_2");
motor1.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
}
