@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()); }