@Override
public void stop() {
Conveyor_servo.setPosition(.5);
PullUp_Motor_String.setPower(0);
PullUp_Motor_Tape.setPower(0);
setPowerLeftMotor(0);
setPowerRightMotor(0);
// Flipper_Servo_Left.setPosition(.59);
// Flipper_Servo_Right.setPosition(.40);
// Climber_servo.setPosition(default_climber_position);
Scoop_Motor.setPower(0);
Sweep_servo.setPosition(.5);
}
@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());
}
