@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 double[] run() {
final double K_FAST = 1 / (slowDownStart * TICKS_PER_REVOLUTION);
final double K_SLOW = (1 / slowDownStart - POWER_MIN / fineTuneStart) / TICKS_PER_REVOLUTION;
final double TICK_OFFSET =
POWER_MIN
* slowDownStart
* fineTuneStart
* TICKS_PER_REVOLUTION
/ (fineTuneStart - POWER_MIN * slowDownStart);
double[] currVal = new double[sides];
double[] error = new double[sides];
double[] power = new double[sides];
if (!hasReachedDestination()) {
for (int i = 0; i < sides; i++) {
currVal[i] = getCurrentPosition()[i];
error[i] = targets[i] - currVal[i];
if (Math.abs(error[i]) < threshold) {
power[i] = 0.0d;
} else {
// try using - fineTuneStart on K_SLOW, maybe need /2
power[i] =
(Math.abs(error[i]) > fineTuneStart * TICKS_PER_REVOLUTION)
? K_FAST * Math.abs(error[i])
: K_SLOW * (TICK_OFFSET + Math.abs(error[i]));
}
if (error[i] < 0.0d) {
power[i] *= -1;
}
power[i] = Range.clip(power[i], -powerMax, powerMax);
}
}
return power;
}
double scale_motor_power(double p_power) {
//
// Assume no scaling.
//
double l_scale = 0.0f;
//
// Ensure the values are legal.
//
double l_power = Range.clip(p_power, -1, 1);
double[] l_array = {
0.00, 0.05, 0.09, 0.10, 0.12, 0.15, 0.18, 0.24, 0.30, 0.36, 0.43, 0.50, 0.60, 0.72, 0.85,
1.00, 1.00
};
//
// Get the corresponding index for the specified argument/parameter.
//
int l_index = (int) (l_power * 16.0);
if (l_index < 0) {
l_index = -l_index;
} else if (l_index > 16) {
l_index = 16;
}
if (l_power < 0) {
l_scale = -l_array[l_index];
} else {
l_scale = l_array[l_index];
}
return l_scale;
}
/*
* 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));
}
@Override
public void loop(RobotContext ctx, LinkedList<Object> out) throws Exception {
PolarCoordinates joystick = gamepad1().rightJoystick().polar().invert();
double r = joystick.getR();
r = Range.clip(2.2842 * Math.pow(Math.tanh(r), 3d), -1, 1);
joystick = new PolarCoordinates(r, joystick.getTheta());
// TODO: finish this
}
@Override
public void loop() {
float lf_power;
float lb_power;
float rf_power;
float rb_power;
if (gamepad1.left_stick_x > gamepad1.left_stick_y
|| -gamepad1.left_stick_x > -gamepad1.left_stick_y) {
float left_power = gamepad1.left_stick_x;
float right_power = gamepad1.left_stick_x;
left_power = Range.clip(left_power, -1, 1);
right_power = Range.clip(right_power, -1, 1);
lf_power = -left_power;
lb_power = left_power;
rf_power = right_power;
rb_power = -right_power;
} else {
float left_power = -gamepad1.left_stick_y;
float right_power = gamepad1.right_stick_y;
left_power = Range.clip(left_power, -1, 1);
right_power = Range.clip(right_power, -1, 1);
lf_power = left_power;
lb_power = left_power;
rf_power = right_power;
rb_power = right_power;
}
float turn = (float) scaleInput(gamepad1.right_stick_x);
lf_power += turn;
lb_power += turn;
rf_power += turn;
rb_power += turn;
left_front.setPower((float) scaleInput(lf_power));
left_back.setPower((float) scaleInput(lb_power));
right_front.setPower((float) scaleInput(rf_power));
right_back.setPower((float) scaleInput(rb_power));
}
/**
* 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;
}
}
public void moveSwiv() {
if (gamepad1.left_bumper) {
if (futureSwiv < time) {
futureSwiv = time + .1;
leftSwivPos += 0.1;
leftSwivPos = Range.clip(leftSwivPos, 0, 1);
rightSwivPos = 1 - leftSwivPos;
}
}
if (gamepad1.right_bumper) {
if (futureSwiv < time) {
futureSwiv = time + .1;
leftSwivPos -= 0.1;
leftSwivPos = Range.clip(leftSwivPos, 0, 1);
rightSwivPos = 1 - leftSwivPos;
}
}
}
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));
}
@Override
public void loop() {
float throttle = -gamepad1.left_stick_y;
float dirrection = gamepad1.left_stick_x;
float right = throttle - dirrection;
float left = throttle + dirrection;
right = (float) scaleInput(right);
left = (float) scaleInput(left);
right = Range.clip(right, -1, 1);
left = Range.clip(left, -1, 1);
motorLeft1.setPower(left);
motorLeft2.setPower(left);
motorLeft3.setPower(left);
telemetry.addData("Text", "*** Robot Data***");
telemetry.addData("left tgt pwr", "left pwr: " + String.format("%.2f", left));
telemetry.addData("right tgt pwr", "right pwr: " + String.format("%.2f", right));
}
@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 loop() {
Range.clip(motorPower, -1.0d, 1.0d);
if (gamepad1.right_bumper) {
motorPower = -gamepad1.right_trigger;
} else {
motorPower = gamepad1.right_trigger;
}
practiceMotor.setPower(motorPower);
}
float ProcessToMotorFromJoy(
float input) { // This is used in any case where joystick input is to be converted to a motor
float output = 0.0f;
// clip the power values so that the values never exceed +/- 1
output = Range.clip(input, -1, 1);
// scale the joystick value to make it easier to control
// the robot more precisely at slower speeds.
output = (float) scaleInput(output);
return output;
}
public void rotatePB(double pow, int deg) throws InterruptedException {
double power = pow;
double angleTo = deg;
double error;
double inte = 0;
double inteNoE = 0;
double der;
double currentAngle = sensor.getGyroYaw();
double previousError = angleTo - currentAngle;
opMode.telemetry.addData("Current Angle", currentAngle + "");
opMode.telemetry.addData("Angle To", angleTo + "");
opMode.telemetry.update();
opMode.resetStartTime();
currentAngle = 0;
while (Math.abs(currentAngle) < Math.abs(angleTo) - 2) {
currentAngle = sensor.getGyroYaw();
error = Math.abs(angleTo) - Math.abs(currentAngle);
opMode.telemetry.addData("error", error);
power = (pow * (error) * .005) + .1; // update p values
inte = ((opMode.getRuntime()) * error * .0025); // update inte value
inteNoE = ((opMode.getRuntime()) * .05);
der = (error - previousError) / opMode.getRuntime() * 0; // update der value
power = power + inteNoE + der;
if (angleTo > 0) power *= -1;
Range.clip(power, -1, 1);
startMotors(-power, power);
opMode.telemetry.addData("PID", power);
// opMode.telemetry.addData("integral", inte);
opMode.telemetry.addData("integral without error", inteNoE);
opMode.telemetry.addData("angle", currentAngle);
opMode.telemetry.update();
previousError = error;
opMode.idle();
}
stopMotors();
opMode.telemetry.addData("finished", "done");
opMode.telemetry.update();
}
@Override
public void loop() {
// get the values from the gamepads
// note: pushing the stick all the way up returns -1,
// so we need to reverse the values
float leftY = -gamepad1.left_stick_y;
float rightY = -gamepad1.right_stick_y;
leftY = Range.clip(leftY, -1, 1);
rightY = Range.clip(rightY, -1, 1);
// motorLF.setPower(leftY);
motorLR.setPower(leftY);
// motorRF.setPower(rightY);
motorRR.setPower(rightY);
// telemetry.addData("left tgt pwr", "left pwr: " + String.format("%.2f", left));
// telemetry.addData("right tgt pwr", "right pwr: " + String.format("%.2f", right));
if (leftY < 1 && rightY > 0) {
DbgLog.msg("leftY, RightY " + leftY + " " + rightY);
}
}
public void rotatePZeroRev(double pow) throws InterruptedException {
double power = pow;
double angleTo = 0;
double error;
double inte = 0;
double inteNoE = 0;
double der;
double currentAngle = sensor.getGyroYaw();
double previousError = angleTo - currentAngle;
opMode.telemetry.addData("Current Angle", currentAngle + "");
opMode.telemetry.addData("Angle To", angleTo + "");
opMode.telemetry.update();
opMode.resetStartTime();
while (Math.abs(currentAngle) > 2) {
currentAngle = sensor.getGyroYaw();
error = Math.abs(Math.abs(angleTo) - Math.abs(currentAngle));
opMode.telemetry.addData("error", error);
power = (pow * (error) * .02) + .125; // update p values
inte = ((opMode.getRuntime()) * error * .0020); // update inte value
inteNoE = ((opMode.getRuntime()) * .075);
der = (error - previousError) / opMode.getRuntime() * 0; // update der value
power = power + inteNoE - der;
power *= 1; // -1 is right
Range.clip(power, -1, 1);
startMotors(-power, power);
opMode.telemetry.addData("PID", power);
// opMode.telemetry.addData("integral", inte);
opMode.telemetry.addData("integral without error", inteNoE);
opMode.telemetry.addData("angle", currentAngle);
opMode.telemetry.update();
previousError = error;
opMode.idle();
}
opMode.telemetry.update();
stopMotors();
}
/** Mutate the hand position. */
void m_hand_position(double p_position) {
//
// Ensure the specific value is legal.
//
double l_position = Range.clip(p_position, Servo.MIN_POSITION, Servo.MAX_POSITION);
//
// Set the value. The right hand value must be opposite of the left
// value.
//
if (v_servo_left_hand != null) {
v_servo_left_hand.setPosition(l_position);
}
if (v_servo_right_hand != null) {
v_servo_right_hand.setPosition(1.0 - l_position);
}
} // m_hand_position
/** Scale the joystick input using a nonlinear algorithm. */
float scale_motor_power(float p_power) {
//
// Assume no scaling.
//
float l_scale;
//
// Ensure the values are legal.
//
float l_power = Range.clip(p_power, -1, 1);
float[] l_array = {
0.00f, 0.05f, 0.09f, 0.10f, 0.12f,
0.15f, 0.18f, 0.24f, 0.30f, 0.36f,
0.43f, 0.50f, 0.60f, 0.72f, 0.85f,
1.00f, 1.00f
};
//
// Get the corresponding index for the specified argument/parameter.
//
int l_index = (int) (l_power * 16.0);
if (l_index < 0) {
l_index = -l_index;
} else if (l_index > 16) {
l_index = 16;
}
if (l_power < 0) {
l_scale = -l_array[l_index];
} else {
l_scale = l_array[l_index];
}
return l_scale;
} // scale_motor_power
/*
* This method will be called repeatedly in a loop
* @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
*/
@Override
public void loop() {
// The op mode should only use "write" methods (setPower, setMode, etc) while in
// WRITE_ONLY mode or SWITCHING_TO_WRITE_MODE
if (allowedToWrite()) {
/*
* Gamepad 1
*
* Gamepad 1 controls the motors via the left stick, and it controls the wrist/claw via the a,b,
* x, y buttons
*/
if (gamepad1.dpad_left) {
// Nxt devices start up in "write" mode by default, so no need to switch modes here.
motorLeft.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
motorRight.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
}
if (gamepad1.dpad_right) {
// Nxt devices start up in "write" mode by default, so no need to switch modes here.
motorLeft.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motorRight.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
}
// 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 throttle = -gamepad1.left_stick_y;
float direction = gamepad1.left_stick_x;
float right = throttle - direction;
float left = throttle + direction;
// clip the right/left values so that the values never exceed +/- 1
right = Range.clip(right, -1, 1);
left = Range.clip(left, -1, 1);
// write the values to the motors
motorRight.setPower(right);
motorLeft.setPower(left);
// update the position of the wrist
if (gamepad1.a) {
wristPosition -= wristDelta;
}
if (gamepad1.y) {
wristPosition += wristDelta;
}
// update the position of the claw
if (gamepad1.x) {
clawPosition -= clawDelta;
}
if (gamepad1.b) {
clawPosition += clawDelta;
}
// clip the position values so that they never exceed 0..1
wristPosition = Range.clip(wristPosition, 0, 1);
clawPosition = Range.clip(clawPosition, 0, 1);
// write position values to the wrist and claw servo
wrist.setPosition(wristPosition);
claw.setPosition(clawPosition);
/*
* Gamepad 2
*
* Gamepad controls the motors via the right trigger as a throttle, left trigger as reverse, and
* the left stick for direction. This type of control is sometimes referred to as race car mode.
*/
// we only want to process gamepad2 if someone is using one of it's analog inputs. If you
// always
// want to process gamepad2, remove this check
if (gamepad2.atRest() == false) {
// throttle is taken directly from the right trigger, the right trigger ranges in values
// from
// 0 to 1
throttle = gamepad2.right_trigger;
// if the left trigger is pressed, go in reverse
if (gamepad2.left_trigger != 0.0) {
throttle = -gamepad2.left_trigger;
}
// assign throttle to the left and right motors
right = throttle;
left = throttle;
// now we need to apply steering (direction). The left stick ranges from -1 to 1. If it is
// negative we want to slow down the left motor. If it is positive we want to slow down the
// right motor.
if (gamepad2.left_stick_x < 0) {
// negative value, stick is pulled to the left
left = left * (1 + gamepad2.left_stick_x);
}
if (gamepad2.left_stick_x > 0) {
// positive value, stick is pulled to the right
right = right * (1 - gamepad2.left_stick_x);
}
// write the values to the motor. This will over write any values placed while processing
// gamepad1
motorRight.setPower(right);
motorLeft.setPower(left);
}
}
// To read any values from the NXT controllers, we need to switch into READ_ONLY mode.
// It takes time for the hardware to switch, so you can't switch modes within one loop of the
// op mode. Every 17th loop, this op mode switches to READ_ONLY mode, and gets the current
// power.
if (numOpLoops % 17 == 0) {
// Note: If you are using the NxtDcMotorController, you need to switch into "read" mode
// before doing a read, and into "write" mode before doing a write. This is because
// the NxtDcMotorController is on the I2C interface, and can only do one at a time. If you are
// using the USBDcMotorController, there is no need to switch, because USB can handle reads
// and writes without changing modes. The NxtDcMotorControllers start up in "write" mode.
// This method does nothing on USB devices, but is needed on Nxt devices.
wheelController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.READ_ONLY);
}
// Every 17 loops, switch to read mode so we can read data from the NXT device.
// Only necessary on NXT devices.
if (wheelController.getMotorControllerDeviceMode() == DcMotorController.DeviceMode.READ_ONLY) {
// Update the reads after some loops, when the command has successfully propagated through.
telemetry.addData("Text", "free flow text");
telemetry.addData("left motor", motorLeft.getPower());
telemetry.addData("right motor", motorRight.getPower());
telemetry.addData("RunMode: ", motorLeft.getMode().toString());
// Only needed on Nxt devices, but not on USB devices
wheelController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.WRITE_ONLY);
// Reset the loop
numOpLoops = 0;
}
// Update the current devMode
devMode = wheelController.getMotorControllerDeviceMode();
numOpLoops++;
}
/*
* 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 throttle = -gamepad1.left_stick_y;
float direction = gamepad1.left_stick_x;
float right = throttle - direction;
float left = throttle + direction;
// clip the right/left values so that the values never exceed +/- 1
right = Range.clip(right, -1, 1);
left = Range.clip(left, -1, 1);
// scale the joystick value to make it easier to control
// the robot more precisely at slower speeds.
right = (float) scaleInput(right);
left = (float) scaleInput(left);
// write the values to the motors
motorRight.setPower(right);
motorLeft.setPower(left);
motorRight2.setPower(right / 3); // front wheels
motorLeft2.setPower(left / 3); // /3 because of gear ratio
/*
/
/
/ GAMEPAD 2
/
/
/
*/
float armThrottle = gamepad2.left_stick_y;
armThrottle = Range.clip(armThrottle, -1, 1);
armThrottle = (float) scaleInput(armThrottle);
// down = (float)scaleInput(down);
arm.setPower(armThrottle);
// arm.setPower(down);
// update the position of the arm.
if (gamepad2.a) {
// if the A button is pushed on gamepad1, increment the position of
// the arm servo.
claw1Position += armDelta;
}
if (gamepad2.b) {
// if the Y button is pushed on gamepad1, decrease the position of
// the arm servo.
claw1Position -= armDelta;
}
// update the position of the claw
if (gamepad2.a) {
claw2Position -= clawDelta;
}
if (gamepad2.b) {
claw2Position += clawDelta;
}
// clip the position values so that they never exceed their allowed range.
claw1Position = Range.clip(claw1Position, ARM_MIN_RANGE, ARM_MAX_RANGE);
claw2Position = Range.clip(claw2Position, CLAW_MIN_RANGE, CLAW_MAX_RANGE);
// write position values to the wrist and claw servo
claw1.setPosition(claw1Position);
claw2.setPosition(claw2Position);
/*
* 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("claw1", "claw1: " + String.format("%.2f", claw1Position));
telemetry.addData("claw2", "claw 2: " + String.format("%.2f", claw2Position));
telemetry.addData("left tgt pwr", "left pwr: " + String.format("%.2f", left));
telemetry.addData("right tgt pwr", "right pwr: " + String.format("%.2f", right));
}
@Override
public void loop() {
// PullUp!!!
/*if (gamepad2.left_bumper)
{
PullUp_Motors_SetPower(1.0);
}
if (gamepad2.left_trigger > 0.2)
{
PullUp_Motors_SetPower(-1.0);
}*/
// Climber control variable!
if (!gamepad2.a
&& !gamepad2.b
&& !gamepad2.y
&& gamepad2.right_stick_y > -.05
&& gamepad2.right_stick_y < .05) {
climber_bypass = false;
}
// Pull Up
if (gamepad2.right_bumper) {
PullUp_Motors_SetPower(1.0);
}
if (gamepad2.right_trigger > 0.2) {
PullUp_Motors_SetPower(-1.0);
}
if (gamepad2.left_stick_y > 0.1) {
PullUp_Motor_Tape.setPower(-gamepad2.left_stick_y);
}
if (gamepad2.left_stick_y < 0.1 && !gamepad2.right_bumper && gamepad2.right_trigger <= 0.2) {
PullUp_Motor_Tape.setPower(0); // The tape stop.
}
if (gamepad2.left_stick_y < -0.1) {
// PullUp_Motor_String.setPower(-gamepad2.left_stick_y);
}
if (gamepad2.left_stick_y > -0.1 && !gamepad2.right_bumper && gamepad2.right_trigger <= 0.2) {
PullUp_Motor_String.setPower(0); // The string stop.
}
if (gamepad2.left_stick_y > -0.1
&& gamepad2.left_stick_y < 0.1
&& !gamepad2.right_bumper
&& gamepad2.right_trigger <= 0.2) {
PullUp_Motors_SetPower(0); // Both stop.
}
// STUFF FOR DRIVE TRAIN!!!
float left = gamepad1.left_stick_y;
float right = gamepad1.right_stick_y;
// clip the right/left values so that the values never exceed +/- 1 (changed range to -.8 to .8
// to prevent motor overdrive.)
right = Range.clip(right, -.80F, .80F);
left = Range.clip(left, -.80F, .80F);
// write the values to the motors
setPowerLeftMotor(left);
setPowerRightMotor(right);
// CLIMBER!!!
if (!climber_bypass) {
Climber_dump(gamepad1.b);
}
// FLIPPERS!!!
if (the_stop_button_left_flipper) {
if (!gamepad2.dpad_right) {
the_stop_button_left_flipper = false;
}
}
if (!the_stop_button_left_flipper) {
if (gamepad2.dpad_right) {
was_pressed_left_flipper = true;
}
}
if (was_pressed_left_flipper) {
Flipper_Servo_Left.setPosition(0);
if (!gamepad2.dpad_right) {
bypass_left_flipper = true;
}
if (bypass_left_flipper) {
if (gamepad2.dpad_right) {
was_pressed_left_flipper = false;
bypass_left_flipper = false;
the_stop_button_left_flipper = true;
}
}
}
if (!was_pressed_left_flipper) {
Flipper_Servo_Left.setPosition(.6); // Was .59
}
if (the_stop_button_right_flipper) {
if (!gamepad2.dpad_left) {
the_stop_button_right_flipper = false;
}
}
if (!the_stop_button_right_flipper) {
if (gamepad2.dpad_left) {
was_pressed_right_flipper = true;
}
}
if (was_pressed_right_flipper) {
Flipper_Servo_Right.setPosition(1);
if (!gamepad2.dpad_left) {
bypass_right_flipper = true;
}
if (bypass_right_flipper) {
if (gamepad2.dpad_left) {
was_pressed_right_flipper = false;
bypass_right_flipper = false;
the_stop_button_right_flipper = true;
}
}
}
if (!was_pressed_right_flipper) {
Flipper_Servo_Right.setPosition(.4);
}
/*
if (gamepad2.dpad_left)
{
Flipper_Servo_Right.setPosition(1);
}
if (!gamepad2.dpad_left)
{
Flipper_Servo_Right.setPosition(.57);
}
if (gamepad2.dpad_right)
{
Flipper_Servo_Left.setPosition(0);
}
if (!gamepad2.dpad_right)
{
Flipper_Servo_Left.setPosition(.40);
}
*/
/*
if (gamepad2.left_trigger > 0)
{
Flipper_Servo_Right.setPosition(1);
}
if (gamepad2.left_trigger == 0)
{
Flipper_Servo_Right.setPosition(.57);
}
if (gamepad2.right_trigger > 0)
{
Flipper_Servo_Left.setPosition(0);
}
if (gamepad2.right_trigger == 0)
{
Flipper_Servo_Left.setPosition(.40);
}
*/
// THE CONVEYOR BELT!
boolean conveyor_left = gamepad1.left_bumper;
boolean conveyor_right = gamepad1.right_bumper;
Conveyor_Belt_Control(conveyor_left, conveyor_right);
// SCOOP!!!
if (gamepad2.a) {
Scoop_Motor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
Scoop_Motor.setTargetPosition(5); // Preset for Collecting Debris, previously was 670
// (assumes scoop is ALL THE WAY TO THE FLOOR AT INIT)
Scoop_Motor.setPower(.1); // You set this as the max power the motor can have...
// (foresee issues depending on which side you are on)
MoveClimberDepositor(0);
}
if (gamepad2.b) {
Scoop_Motor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
Scoop_Motor.setTargetPosition(
-355); // Preset for Dumping Debris into Conveyor, previously was -375
Scoop_Motor.setPower(.11);
MoveClimberDepositor(0);
}
if (gamepad2.y) {
Scoop_Motor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
Scoop_Motor.setTargetPosition(-640); // Preset for Storage, previously was 30
Scoop_Motor.setPower(.1);
Climber_servo.setPosition(climber_safe_from_smashing_position);
climber_bypass = true;
}
if (gamepad2.x) {
int hover_position = Scoop_Motor.getCurrentPosition();
Scoop_Motor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
Scoop_Motor.setTargetPosition(hover_position);
Scoop_Motor.setPower(.1);
}
double Scoop_Motor_value = Range.clip(gamepad2.right_stick_y, -.1, .1);
double Scoop_Motor_Power =
Scoop_Motor_value; // Equivalent to x^3? Based on
// https://docs.oracle.com/javase/tutorial/java/data/beyondmath.htm
// if joystick 2, right joystick
// Stop Motor
if (gamepad2.right_stick_y < 0.05
&& gamepad2.right_stick_y > -0.05
&& !gamepad2.a
&& !gamepad2.b
&& !gamepad2.y
&& !gamepad2.x) {
Scoop_Motor.setMode((DcMotorController.RunMode.RUN_USING_ENCODERS));
Scoop_Motor.setPower(0);
}
if (gamepad2.right_stick_y > 0.05 || gamepad2.right_stick_y < -.05) {
Scoop_Motor.setMode((DcMotorController.RunMode.RUN_USING_ENCODERS));
Scoop_Motor.setPower(Scoop_Motor_Power);
MoveClimberDepositor(225);
}
// END Scoop Fine-Tuning
// Sweeper!!!
boolean locked_controls = false;
if (gamepad1.a && gamepad1.y) // Pressing A and Y buttons at same time will reset Sweeper.
{
locked_controls = true;
Sweep_servo.setPosition(.5);
}
if (the_stop_button_a) {
if (!gamepad1.a) {
the_stop_button_a = false;
}
}
if (!the_stop_button_a) {
if (gamepad1.a) {
was_pressed_a = true;
}
}
if (was_pressed_a) {
if (!locked_controls) {
Sweep_servo.setPosition(.1); // FORWARD SWEEPER!!!
}
if (was_pressed_y) {
was_pressed_y = false;
bypass_y = false;
}
if (!gamepad1.a) {
bypass_a = true;
}
if (bypass_a) {
if (gamepad1.a) {
was_pressed_a = false;
bypass_a = false;
the_stop_button_a = true;
}
}
}
if (!was_pressed_a && !was_pressed_y) {
Sweep_servo.setPosition(.5);
}
if (gamepad1.a && gamepad1.y) {
locked_controls = true;
Sweep_servo.setPosition(.5);
}
if (the_stop_button_y) {
if (!gamepad1.y) {
the_stop_button_y = false;
}
}
if (!the_stop_button_y) {
if (gamepad1.y) {
was_pressed_y = true;
}
}
if (was_pressed_y) {
if (!locked_controls) {
Sweep_servo.setPosition(.9); // REVERSE SWEEPER!!!
}
if (was_pressed_a) {
was_pressed_a = false;
bypass_a = false;
}
if (!gamepad1.y) {
bypass_y = true;
}
if (bypass_y) {
if (gamepad1.y) {
was_pressed_y = false;
bypass_y = false;
the_stop_button_y = true;
}
}
}
if (locked_controls) {
was_pressed_y = false;
was_pressed_a = false;
bypass_y = false;
bypass_a = false;
the_stop_button_y = false;
the_stop_button_a = false;
}
if (!was_pressed_a && !was_pressed_y) {
Sweep_servo.setPosition(.5);
}
// telemetry.addData("Text", "*** Robot Data***");
telemetry.addData("Scoop Encoder value", Scoop_Motor.getCurrentPosition());
telemetry.addData("Scoop Motor Power", Scoop_Motor.getPower());
telemetry.addData("PullUp Tape Motor Power", PullUp_Motor_Tape.getPower());
telemetry.addData("PullUp String Motor Power", PullUp_Motor_String.getPower());
telemetry.addData("Climber Position", Climber_servo.getPosition());
telemetry.addData("Left Motor Power:" + motorLeft1.getPower(), motorLeft2.getPower());
telemetry.addData("Right Motor Power:" + motorRight1.getPower(), motorRight2.getPower());
}
/** Loop repeatedly called during TeleOp while the robot is running. */
@Override
public void loop() {
// Scale values from analog sticks into range readable by motors
float leftY = (float) scaleInput(Range.clip(gamepad1.left_stick_y, -1, 1));
float rightY = (float) scaleInput(Range.clip(gamepad1.right_stick_y, -1, 1));
float hangMove = -(float) scaleInput(Range.clip(gamepad2.right_stick_y, -1, 1));
float hangPincher = -(float) scaleInput(Range.clip(gamepad2.left_stick_y, -.5, .5));
// hangingPower.setPower(hangMove);
// Control motors for hanging the robot
if (hangMove > 0.1) hangingPower.setPower(hangMove);
else if (hangMove < -0.1) hangingPower.setPower(hangMove);
else hangingPower.setPower(0);
if (gamepad1.right_bumper) servoPlease.setPosition(.5);
else if (gamepad1.left_bumper) servoPlease.setPosition(0);
hangingPincer.setPower(hangPincher);
if (gamepad2.right_bumper) {
if (!hangPivotPressed) {
hangPivotPressed = true;
if (hangPivot == .75f) hangPivot = 1;
else if (hangPivot == 1) hangPivot = .75f;
}
} else if (hangPivotPressed) hangPivotPressed = false;
// Control motors for pivoting the hanger
// Power is based on how far the joystick is moved
if (gamepad2.dpad_up) {
hangingPivot.setPower(-hangPivot);
hangingPivot2.setPower(-hangPivot);
} else if (gamepad2.dpad_down) {
hangingPivot.setPower(hangPivot);
hangingPivot2.setPower(hangPivot);
} else {
// Stop the pivot motors
hangingPivot.setPower(0);
hangingPivot2.setPower(0);
}
// Control servos for controlling the wings
// *** X Y for left, A B for right
if (gamepad2.x) LWing.setPosition(0.0);
else if (gamepad2.y) LWing.setPosition(1.0);
else LWing.setPosition(0.5);
if (gamepad2.a) RWing.setPosition(1.0);
else if (gamepad2.b) RWing.setPosition(0.0);
else RWing.setPosition(0.5);
// Reverse the tank drive direction
if (gamepad1.start) {
if (!speedReversePressed) {
speedMultiplier *= -1;
speedReversePressed = true;
}
} else if (speedReversePressed) speedReversePressed = false;
SetDrivePower(speedMultiplier * rightY, speedMultiplier * leftY);
telemetry.addData("Drive Right", speedMultiplier * rightY);
telemetry.addData("Drive Left", speedMultiplier * leftY);
}
@Override
public void loop() {
// When dpad is pushed up increase one mode
// When dpad is pushed down decrease by one mode
if (gamepad1.dpad_up) {
if (!iSawDpadUpAlready) {
iSawDpadUpAlready = true;
mode = mode + 0.25;
}
} else {
iSawDpadUpAlready = false;
}
if (gamepad1.dpad_down) {
if (!iSawDpadDownAlready) {
iSawDpadDownAlready = true;
mode = mode - 0.25;
}
} else {
iSawDpadDownAlready = false;
}
mode = Range.clip(mode, 0.25, 1);
// when leftstick is pushed up move forward
// when rightstick is pushed down move backwards
double left = -gamepad1.left_stick_y;
double right = -gamepad1.right_stick_y;
right = (double) scaleInput(right);
left = (double) scaleInput(left);
right = Range.clip(right, -mode, mode);
left = Range.clip(left, -mode, mode);
leftFront.setPower(left);
leftBack.setPower(left);
rightFront.setPower(right);
rightBack.setPower(right);
if (gamepad1.y) {
vortex.setPower(50);
} else {
vortex.setPower(0);
}
if (gamepad1.a) {
vortex.setPower(-50);
} else {
vortex.setPower(0);
}
if (gamepad2.x) {
pusherposition = 1;
pusherposition = Range.clip(pusherposition, -1, 1);
pusherLeft.setPower(1);
} else {
pusherposition = 0;
pusherposition = Range.clip(pusherposition, -1, 1);
pusherLeft.setPower(0);
}
if (gamepad2.b) {
pusherposition = -1;
pusherposition = Range.clip(pusherposition, -1, 1);
pusherLeft.setPower(-1);
} else {
pusherposition = 0;
pusherposition = Range.clip(pusherposition, -1, 1);
pusherLeft.setPower(0);
}
if (gamepad2.y) {
pusherposition = 1;
pusherposition = Range.clip(pusherposition, -1, 1);
pusherRight.setPower(1);
} else {
pusherposition = 0;
pusherposition = Range.clip(pusherposition, -1, 1);
pusherRight.setPower(0);
}
if (gamepad2.a) {
pusherposition = -1;
pusherposition = Range.clip(pusherposition, -1, 1);
pusherRight.setPower(-1);
} else {
pusherposition = 0;
pusherposition = Range.clip(pusherposition, -1, 1);
pusherRight.setPower(0);
}
}
//
// This method will be called repeatedly in a loop
// @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
//
@Override
public void loop() {
String selectedOption = "";
long startTime = System.currentTimeMillis();
M_driveRightPower = gamepad1.right_stick_y;
M_driveLeftPower = gamepad1.left_stick_y;
if (gamepad1.a && A_upDownServoPos > Servo.MIN_POSITION) A_upDownServoPos -= 0.01d;
else if (gamepad1.b && A_upDownServoPos < Servo.MAX_POSITION) A_upDownServoPos += 0.01d;
if (gamepad1.x && A_sideSideServoPos < Servo.MAX_POSITION) A_sideSideServoPos += 0.01d;
else if (gamepad1.y && A_sideSideServoPos > Servo.MIN_POSITION) A_sideSideServoPos -= 0.01d;
// setting hardware
M_driveRight.setPower(M_driveRightPower);
M_driveLeft.setPower(M_driveLeftPower);
S_sideSide.setPosition(Range.clip(A_sideSideServoPos, 0.0, 1.0));
S_upDown.setPosition(Range.clip(A_upDownServoPos, 0.0, 1.0));
// Employ the enum option set in init_loop with if and switch statements.
if (choiceValue == Options.Option_1) selectedOption = "Option 1";
switch (choiceValue) {
case Option_2:
selectedOption = "Option 2";
break;
case Option_3:
selectedOption = "Option 3";
break;
}
// display telemetry on DS. line labels are sorted.
Util.telemetry(
"1-LeftGP",
"LY=%.2f LP=%.2f RY=%.2f RP=%.2f",
gamepad1.left_stick_y,
M_driveLeftPower,
gamepad1.right_stick_y,
M_driveRightPower);
Util.telemetry(
"Buttons",
"t=%b x=%b a=%b b=%b y=%b",
I_touch.isPressed(),
gamepad1.x,
gamepad1.a,
gamepad1.b,
gamepad1.y);
Util.telemetry("UpDwn Servo", "Sup=%.2f real=%.2f", A_upDownServoPos, S_upDown.getPosition());
Util.telemetry(
"Sidew Servo", "Sup=%.2f real=%.2f", A_sideSideServoPos, S_sideSide.getPosition());
// Util.telemetry("Options", "String=%s Enum=%s SelEnum=%s", option,
// enumOption.toString(), selectedEnumOption);
Util.telemetry("Menu selection", "%d=%s (%s)", choice, choiceValue.name(), selectedOption);
Util.telemetry("Outside Loop Time", Long.toString(startTime - lastLoopTime));
long endTime = System.currentTimeMillis();
Util.telemetry("Loop Time", Long.toString(endTime - startTime));
lastLoopTime = endTime;
}
@Override
public void runOpMode() throws InterruptedException {
// initMediaPlayer();
leftMotor = hardwareMap.dcMotor.get("leftdrive");
rightMotor = hardwareMap.dcMotor.get("rightdrive");
armmotor1 = hardwareMap.dcMotor.get("armmotor1");
armmotor2 = hardwareMap.dcMotor.get("armmotor2");
spinner = hardwareMap.dcMotor.get("spinner");
turner = hardwareMap.dcMotor.get("turner");
zipLine1 = hardwareMap.servo.get("servo1");
zipLine2 = hardwareMap.servo.get("servo2");
leftMotor.setDirection(DcMotor.Direction.REVERSE);
armOn = false;
waitForStart();
// assign the starting position of the servos
zipPosition1 = 1.0;
zipPosition2 = 0;
while (opModeIsActive()) {
// Change Direction
if (gamepad1.right_bumper) {
if (reverseOn) {
reverseOn = false;
} else if (!reverseOn) {
reverseOn = true;
}
}
float throttle_r = -gamepad1.right_stick_y;
float throttle_l = -gamepad1.left_stick_y;
float right = throttle_r;
float left = throttle_l;
// This is for sniper mode
if (gamepad1.a) {
if (sniperOn) {
sniperOn = false;
} else {
sniperOn = true;
}
}
if (!sniperOn) {
// clip the right/left values so that the values never exceed +/- 1
right = Range.clip(right, -1, 1);
left = Range.clip(left, -1, 1);
// write the values to the motors
rightMotor.setPower(right);
leftMotor.setPower(left);
} else if (sniperOn) {
// clip the right/left values so that the values never exceed +/- .5
right = Range.clip(right, -.5f, .5f);
left = Range.clip(left, -.5f, .5f);
// write the values to the motors
rightMotor.setPower(right);
leftMotor.setPower(left);
}
if (gamepad2.dpad_down) {
// move the servos to hit the ziplines
zipPosition1 = 0.2D;
zipPosition2 = 0.7D;
}
if (gamepad2.dpad_up) {
zipPosition1 = 1.0;
zipPosition2 = 0;
}
if (gamepad2.y) {
spinner.setPower(-0.9);
} else if (gamepad2.x) {
spinner.setPower(0.9);
} else {
if (!gamepad2.y && !gamepad2.x) {
spinner.setPower(0);
}
}
if (gamepad2.right_bumper) {
turner.setPower(-0.5);
} else if (gamepad2.left_bumper) {
turner.setPower(0.5);
} else {
if (!gamepad2.right_bumper && !gamepad2.left_bumper) {
turner.setPower(0);
}
}
if (gamepad2.a) {
armmotor1.setPower(-0.2);
armmotor2.setPower(0.2);
armOn = true;
} else if (gamepad2.b) {
armmotor1.setPower(0.2);
armmotor2.setPower(-0.2);
armOn = true;
} else {
if (armOn) {
armmotor1.setPower(0);
armmotor2.setPower(0);
armOn = false;
}
}
// if (gamepad2.a){
// spinnerMotor.setPower(1);
// }
// if (gamepad2.x){
// spinnerMotor.setPower(-1);
// }
// if (!gamepad2.a && !gamepad2.x){
// spinnerMotor.setPowerFloat();
// }
// write position values to the wrist and claw servo
zipLine1.setPosition(zipPosition1);
zipLine2.setPosition(zipPosition2);
// if (gamepad1.left_bumper) {
// toggleMediaPlayer();
// }
/*
* Send telemetry data back to driver station.
*/
telemetry.addData("Text", "*** TestBot Data***");
telemetry.addData("left tgt pwr", "left pwr: " + String.format("%.2f", left));
telemetry.addData("tgt pwr", "right pwr: " + String.format("%.2f", right));
waitOneFullHardwareCycle();
// stopMediaPlayer();
}
}
/*
* This method will be called repeatedly in a loop
*
* @see com.qualcomm.robotcore.eventloop.opmode.OpMode#run()
*/
@Override
public void loop() {
double motor_1_stick;
double motor_2_stick;
// ON CONTROLLER:
// - no light next to mode button
// - switch set to x on back
// IN DRIVER APP
// - controller set as Logitech F310 Gamepad
//
// y value: -1 = up, +1 = down
// x value: -1 = left, +1 = right
//
// Joystick y inverted, so then the motor speed is
// positive when pushing stick up
double motor_speed = -gamepad1.left_stick_y;
// Just get the plain stick x and use as the turn speed
double turn_speed = gamepad1.right_stick_x;
// Joystick is pushed forward or backward
if (motor_speed != 0) {
// If the turn speed is maxed at either 1 or -1,
// this variable will cancel out the motor speed (by subtraction).
// This is useful in turning (while moving) by preventing
// motor from going the opposite direction and making things go crazy.
double turn_speed_to_halt = abs_d(turn_speed) * motor_speed;
// If joystick is push to the left (turn_speed < 0),
// then motor 2 will slow down.
motor_2_stick = motor_speed - ((turn_speed < 0) ? turn_speed_to_halt : 0);
// If joystick is push to the right (turn_speed > 0),
// then motor 1 will slow down.
motor_1_stick = motor_speed - ((turn_speed > 0) ? turn_speed_to_halt : 0);
}
// Basic 'standing' turn
else {
motor_1_stick = -turn_speed;
motor_2_stick = turn_speed;
}
// clip values so to be in [-1,1]
motor_1_stick = Range.clip(motor_1_stick, -1, 1);
motor_2_stick = Range.clip(motor_2_stick, -1, 1);
// call motor response function
motor_1_stick = (float) response_function(motor_1_stick);
motor_2_stick = (float) response_function(motor_2_stick);
// set motor values (inverted because of motor placement)
motor_1.setPower(-motor_1_stick);
motor_2.setPower(-motor_2_stick);
if (gamepad1.a && !touch_sensor.isPressed()) {
motor_3.setPower(motor_3_delta);
} else if (gamepad1.y) {
motor_3.setPower(-motor_3_delta);
} else {
motor_3.setPower(0);
}
// Left dpad, move servo, then move back after 3 sec
if (gamepad1.dpad_left) {
servo1Timer.setWaitPeriod(3000);
// servo_1_position = SERVO_MAX;
servo_1.setPosition(SERVO_MAX);
}
if (servo1Timer.isDone()) {
servo_1.setPosition(SERVO_MIN);
}
// Up dpad, move servo, then move back after 3 sec
if (gamepad1.dpad_up) {
servo2Timer.setWaitPeriod(3000);
servo_2.setPosition(SERVO_MAX);
}
if (servo2Timer.isDone()) {
servo_2.setPosition(SERVO_MIN);
}
// Right dpad, move servo, then move back after 3 sec
if (gamepad1.dpad_right) {
servo3Timer.setWaitPeriod(3000);
servo_3.setPosition(SERVO_MAX);
}
if (servo3Timer.isDone()) {
servo_3.setPosition(SERVO_MIN);
}
// use a to ratchet down servo 2, x to ratchet down servo 1, y to ratchet up servo 2 and b to
// ratchet up servo 1
/*if(gamepad1.left_bumper){
servo_1_position-=servo_1_delta;
}
if(gamepad1.left_trigger>0){
servo_1_position+=servo_1_delta;
}*/
// clip servos to prespectified range
servo_1_position = Range.clip(servo_1_position, SERVO_MIN, SERVO_MAX);
// set servo positions
servo_1.setPosition(servo_1_position);
/*if(touch_sensor.isPressed()) {
motor_1.setPower(0.00);
motor_2.setPower(0.00);
motor_1_stick = 0.00;
motor_2_stick = 0.00;
}*/
/*double ir_angle = 0.0;
double ir_strength = 0.0;
if (ir_seeker.signalDetected()) {
/*
* Get angle and strength of the signal.
* Note an angle of zero implies straight ahead.
* A negative angle implies that the source is to the left.
* A positive angle implies that the source is to the right.
/
ir_angle = ir_seeker.getAngle();
ir_strength = ir_seeker.getStrength();
}*/
// write some telemetry to the driver station
// telemetry.addData("Text","*** Robot Data ***");
// telemetry.addData("servo_1","servo_1: "+String.format("%.2f",servo_1_position));
// telemetry.addData("servo_2","servo_2: "+String.format("%.2f",servo_2_position));
telemetry.addData("motor_1", "motor 1: " + String.format("%.2f", motor_1_stick));
telemetry.addData("motor_2", "motor 2: " + String.format("%.2f", motor_2_stick));
telemetry.addData("turn_speed", "turn_speed: " + String.format("%.2f", turn_speed));
telemetry.addData("motor_speed", "motor_speed: " + String.format("%.2f", motor_speed));
// telemetry.addData("ir detected","ir detected:
// "+String.format("%d",ir_seeker.signalDetected()));
// telemetry.addData("ir angle","ir angle: "+String.format("%.2f",ir_angle));
// telemetry.addData("ir strength","ir strength: "+String.format("%.2f",ir_strength));
// telemetry.addData("ods value","ods value: "+String.format("%.2f",light_detected));
}
void handle_drivetrain() {
if (arcademode) { // code for arcade mode here
telemetry.addData("Drive: ", drive_mode(arcademode));
double xValue = -gamepad1.left_stick_x;
double yValue = -gamepad1.left_stick_y;
xValue = xValue * maxXJoystick;
double xSquaredVal = xValue * xValue;
double ySquaredVal = yValue * yValue;
double xPower;
double yPower;
if (xValue < 0) {
xPower = -xSquaredVal;
} else {
xPower = xSquaredVal;
}
if (yValue < 0) {
yPower = -ySquaredVal;
} else {
yPower = ySquaredVal;
}
double leftPower = yPower + xPower;
double rightPower = yPower - xPower;
leftPower = Range.clip(leftPower, -1, 1);
rightPower = Range.clip(rightPower, -1, 1);
leftMotor.setPower(leftPower * arcadeMaxPower);
rightMotor.setPower(rightPower * arcadeMaxPower);
} else { // code for tank mode here
telemetry.addData("Drive: ", drive_mode(arcademode));
double leftY = -gamepad1.left_stick_y;
double rightY = -gamepad1.right_stick_y;
double leftSquaredVal = leftY * leftY;
double rightSquaredVal = rightY * rightY;
double leftPower;
double rightPower;
if (leftY < 0) {
leftPower = -leftSquaredVal;
} else {
leftPower = leftSquaredVal;
}
if (rightY < 0) {
rightPower = -rightSquaredVal;
} else {
rightPower = rightSquaredVal;
}
leftMotor.setPower(leftPower * tankMaxPower);
rightMotor.setPower(rightPower * tankMaxPower);
}
}
void gripperPosition(double p_position) {
double l_position = Range.clip(p_position, Servo.MIN_POSITION + 0.1, Servo.MAX_POSITION - 0.1);
gripper.setPosition(l_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);
}
public void moveBackward(double pow, int encoderVal, int timeout) throws InterruptedException {
// sensor.resetGyro();
double angle;
double startAngle = Math.abs(sensor.getGyroYaw());
opMode.telemetry.update();
double error;
double power;
motorBL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
opMode.idle();
motorBR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
opMode.idle();
motorFR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
opMode.idle();
motorFL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
opMode.idle();
motorBL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
opMode.idle();
motorBR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
opMode.idle();
motorFR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
opMode.idle();
motorFL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
opMode.idle();
setNullValue();
nullValue = 0;
ElapsedTime time = new ElapsedTime();
time.startTime();
int currentEncoder = getEncoderAvg() - nullValue;
while (encoderVal > currentEncoder && time.milliseconds() < timeout) {
opMode.telemetry.update();
angle = Math.abs(sensor.getGyroYaw());
currentEncoder = getEncoderAvg() - nullValue;
error = (double) (encoderVal - currentEncoder) / encoderVal;
power = (pow * error) - .25;
Range.clip(power, -1, 1);
opMode.telemetry.addData("Power", power);
opMode.telemetry.addData("LeftPower", motorBL.getPower());
opMode.telemetry.addData("RightPower", motorBR.getPower());
opMode.telemetry.update();
if (angle < startAngle - 2) {
startMotors((power), (power * .75));
} else if (angle > startAngle + 2) {
startMotors((power * .75), (power));
} else {
startMotors(power, power);
}
startMotors(power, power);
opMode.idle();
}
stopMotors();
opMode.telemetry.update();
angleError = sensor.getGyroYaw();
}