From cec119c15e41e0a2104ad0925a0d31c4196af922 Mon Sep 17 00:00:00 2001 From: OviedoRobotics <54600937+OviedoRobotics@users.noreply.github.com> Date: Thu, 7 Nov 2024 21:46:48 -0500 Subject: [PATCH] 2024-11-07 odometry drive --- .../ftc/teamcode/AutonomousBase.java | 36 +- .../ftc/teamcode/AutonomousLeftRed.java | 20 +- .../ftc/teamcode/Hardware2025Bot.java | 32 +- .../firstinspires/ftc/teamcode/Teleop.java | 38 +- .../ftc/teamcode/TeleopLift.java | 680 ------------------ 5 files changed, 67 insertions(+), 739 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopLift.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousBase.java index 0a957705450d..053ab89edfef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousBase.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousBase.java @@ -14,6 +14,9 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl; import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; import org.firstinspires.ftc.robotcore.internal.system.AppUtil; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; @@ -28,8 +31,7 @@ public abstract class AutonomousBase extends LinearOpMode { /* Declare OpMode members. */ -// HardwarePixelbot robot = new HardwarePixelbot(telemetry); - HardwareMinibot robot = new HardwareMinibot(); + Hardware2025Bot robot = new Hardware2025Bot(); static final int DRIVE_TO = 1; // ACCURACY: tighter tolerances, and slows then stops at final position static final int DRIVE_THRU = 2; // SPEED: looser tolerances, and leave motors running (ready for next command) @@ -62,20 +64,20 @@ public abstract class AutonomousBase extends LinearOpMode { static final double STRAFE_MULTIPLIER = 1.5; static final double MIN_SPIN_RATE = 0.05; // Minimum power to turn the robot static final double MIN_DRIVE_POW = 0.05; // Minimum speed to move the robot -// static final double MIN_DRIVE_MAGNITUDE = Math.sqrt(MIN_DRIVE_POW*MIN_DRIVE_POW+MIN_DRIVE_POW*MIN_DRIVE_POW); + static final double MIN_DRIVE_MAGNITUDE = Math.sqrt(MIN_DRIVE_POW*MIN_DRIVE_POW+MIN_DRIVE_POW*MIN_DRIVE_POW); // NOTE: Initializing the odometry global X-Y and ANGLE to 0-0 and 0deg means the frame of reference for all movements is // the starting positiong/orientation of the robot. An alternative is to make the bottom-left corner of the field the 0-0 // point, with 0deg pointing forward. That allows all absolute driveToPosition() commands to be an absolute x-y location // on the field. - double robotGlobalXCoordinatePosition = 0.0; // in odometer counts - double robotGlobalYCoordinatePosition = 0.0; - double robotOrientationRadians = 0.0; // 0deg (straight forward) + double robotGlobalXCoordinatePosition = 0.0; // inches + double robotGlobalYCoordinatePosition = 0.0; // inches + double robotOrientationRadians = 0.0; // radians 0deg (straight forward) // Odometry values corrected by external source, IE AprilTags // FieldCoordinate robotGlobalCoordinateCorrectedPosition = new FieldCoordinate(0.0, 0.0, 0.0); - double autoXpos = 0.0; // Keeps track of our Autonomous X-Y position and Angle commands. + double autoXpos = 0.0; // Keeps track of our Autonomous X-Y position and Angle commands. double autoYpos = 0.0; // (useful when a given value remains UNCHANGED from one double autoAngle = 0.0; // movement to the next, or INCREMENTAL change from current location). @@ -336,7 +338,11 @@ else if( gamepad1_cross_now && !gamepad1_cross_last ) { /*---------------------------------------------------------------------------------*/ public void performEveryLoop() { robot.readBulkData(); -// globalCoordinatePositionUpdate(); + robot.odom.update(); + Pose2D pos = robot.odom.getPosition(); // x,y pos in inch; heading in degrees + robotGlobalXCoordinatePosition = pos.getY(DistanceUnit.INCH); // opposite x/y from goBilda pinpoint + robotGlobalYCoordinatePosition = pos.getX(DistanceUnit.INCH); + robotOrientationRadians = -pos.getHeading(AngleUnit.RADIANS); // 0deg (straight forward) } // performEveryLoop // Create a time stamped folder in @@ -966,8 +972,8 @@ protected boolean driveToXY(double yTarget, double xTarget, double angleTarget, int driveType) { boolean reachedDestination = false; // Not sure why, but the x and y are backwards - double xWorld = robotGlobalYCoordinatePosition / robot.COUNTS_PER_INCH2; // inches (backward! see notes) - double yWorld = robotGlobalXCoordinatePosition / robot.COUNTS_PER_INCH2; // inches + double xWorld = robotGlobalYCoordinatePosition; // inches (backward! see notes) + double yWorld = robotGlobalXCoordinatePosition; // inches double xMovement, yMovement, turnMovement; // Not sure why, but the x and y are backwards double deltaX = yTarget - xWorld; @@ -1029,10 +1035,10 @@ protected boolean driveToXY(double yTarget, double xTarget, double angleTarget, // Convert from cm to inches double errorMultiplier = 0.033; -// double speedMin = MIN_DRIVE_MAGNITUDE; + double speedMin = MIN_DRIVE_MAGNITUDE; double allowedError = (driveType == DRIVE_THRU) ? 2.75 : 0.5; - return (driveToXY(yTarget, xTarget, angleTarget, 0.0 /*speedMin*/, speedMax, errorMultiplier, + return (driveToXY(yTarget, xTarget, angleTarget, speedMin, speedMax, errorMultiplier, allowedError, driveType)); } @@ -1125,9 +1131,9 @@ public void driveToPosition( double yTarget, double xTarget, double angleTarget, public boolean moveToPosition( double xTarget, double yTarget, double angleTarget, double speedMax, double turnMax, int driveType ) { // Convert current robot X,Y position from encoder-counts to inches - double x_world = robotGlobalYCoordinatePosition / robot.COUNTS_PER_INCH2; // inches (X/Y backward! see notes) - double y_world = robotGlobalXCoordinatePosition / robot.COUNTS_PER_INCH2; // inches - double angle_world = robotOrientationRadians; // radians + double x_world = robotGlobalYCoordinatePosition; // inches (X/Y backward! see notes) + double y_world = robotGlobalXCoordinatePosition; // inches + double angle_world = robotOrientationRadians; // radians // Compute distance and angle-offset to the target point double distanceToPoint = Math.sqrt( Math.pow((xTarget - x_world),2.0) + Math.pow((yTarget - y_world),2.0) ); double distToPointAbs = Math.abs( distanceToPoint ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftRed.java index 647f24ea7f79..3e496342f655 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftRed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftRed.java @@ -2,12 +2,17 @@ */ package org.firstinspires.ftc.teamcode; +import static java.lang.Math.toDegrees; + import android.util.Size; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + /** * This program implements robot movement based on Gyro heading and encoder counts. * It uses the Mecanumbot hardware class to define the drive on the robot. @@ -82,13 +87,13 @@ public void runOpMode() throws InterruptedException { // UNIT TEST: The following methods verify our basic robot actions. // Comment them out when not being tested. // testGyroDrive(); -// unitTestOdometryDrive(); + unitTestOdometryDrive(); //--------------------------------------------------------------------------------- //--------------------------------------------------------------------------------- // AUTONOMOUS ROUTINE: The following method is our main autonomous. // Comment it out if running one of the unit tests above. - mainAutonomous(); +// mainAutonomous(); //--------------------------------------------------------------------------------- telemetry.addData("Program", "Complete"); @@ -115,11 +120,16 @@ private void testGyroDrive() { // TEST CODE: Verify odometry-based motion functions against a tape measure private void unitTestOdometryDrive() { // Drive forward 12" - driveToPosition( 12.0, 0.0, 0.0, DRIVE_SPEED_50, TURN_SPEED_40, DRIVE_THRU ); + driveToPosition( 12.0, 0.0, 0.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_THRU ); // Strafe right 12" - driveToPosition( 12.0, 12.0, 0.0, DRIVE_SPEED_50, TURN_SPEED_40, DRIVE_THRU ); + driveToPosition( 12.0, 12.0, 0.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_THRU ); // Turn 180 deg - driveToPosition( 12.0, 12.0, 179.9, DRIVE_SPEED_50, TURN_SPEED_40, DRIVE_TO ); + driveToPosition( 12.0, 12.0, 90.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO ); + // Report the final odometry position/orientation + telemetry.addData("Final", "x=%.1f, y=%.1f, %.1f deg", + robotGlobalXCoordinatePosition, robotGlobalYCoordinatePosition, toDegrees(robotOrientationRadians) ); + telemetry.update(); + sleep( 7000 ); } // unitTestOdometryDrive /*--------------------------------------------------------------------------------------------*/ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware2025Bot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware2025Bot.java index bb2cdb66fdb5..df98d993627b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware2025Bot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware2025Bot.java @@ -93,21 +93,12 @@ public class Hardware2025Bot public double wormTiltMotorSetPwr = 0.0; // requested power setting public double wormTiltMotorPwr = 0.0; // current power setting - public double PAN_ANGLE_HW_MAX = 400.0; // absolute encoder angles at maximum rotation RIGHT - public double PAN_ANGLE_CYCLE_R = 133.5; - public double PAN_ANGLE_AUTO_M_R = 137.0; // scoring the autonomous pre-load cone (RIGHT MED) - public double PAN_ANGLE_AUTO_R = 117.5; // scoring the autonomous pre-load cone (RIGHT HIGH) - public double PAN_ANGLE_5STACK_L = 118.0; - public double PAN_ANGLE_COLLECT_R = 21.9; - public double PAN_ANGLE_CENTER = 0.0; // turret centered - public double PAN_ANGLE_COLLECT_L = -21.9; - public double PAN_ANGLE_5STACK_R = -103.0; - public double PAN_ANGLE_AUTO_L = -124.5; // scoring the autonomous pre-load cone (LEFT HIGH) - public double PAN_ANGLE_CYCLE_L = -133.5; - public double PAN_ANGLE_AUTO_M_L = -141.0; // scoring the autonomous pre-load cone (LEFT MED) - public double PAN_ANGLE_HW_MIN = -400.0; // absolute encoder angles at maximum rotation LEFT - - public double TILT_ANGLE_HW_MAX = 4000.0; // encoder at maximum rotation UP/BACK + public double PAN_ANGLE_HW_MAX = 200.0; // encoder angles at maximum rotation RIGHT + public double PAN_ANGLE_HW_BASKET = 0.0; // encoder for rotation back to the basket for scoring + public double PAN_ANGLE_HW_MIN = -575.0; // encoder angles at maximum rotation LEFT + + public double TILT_ANGLE_HW_MAX = 3675.0; // encoder at maximum rotation UP/BACK (horizontal = -200) + public double TILT_ANGLE_BASKET = 3675.0; // encoder at rotation back to the basket for scoring public double TILT_ANGLE_HW_MIN = -2000.0; // encoder at maximum rotation DOWN/FWD //====== Viper slide MOTOR (RUN_USING_ENCODER) ===== @@ -129,10 +120,11 @@ public class Hardware2025Bot // Encoder counts for 312 RPM lift motors theoretical max ??? rev * 537.7 ticks/rev = ?? counts public int VIPER_EXTEND_ZERO = 0; // fully retracted (may need to be adjustable??) public int VIPER_EXTEND_AUTO = 482; // extend for collecting during auto - public int VIPER_EXTEND_GRAB = 1230; // extend for collection from submersible + public int VIPER_EXTEND_GRAB = 1000; // extend for collection from submersible public int VIPER_EXTEND_HOOK = 1038; // raised to where the specimen hook is above the high bar - public int VIPER_EXTEND_BASKET= 1482; // raised to basket-scoring height - public int VIPER_EXTEND_FULL = 3000; // fully extended (never exceed this count!) + public int VIPER_EXTEND_BASKET= 3000; // raised to basket-scoring height + public int VIPER_EXTEND_FULL1 = 2250; // extended 36" forward (max for 20"x42" limit) 2310 with overshoot + public int VIPER_EXTEND_FULL2 = 3010; // hardware fully extended (never exceed this count!) PIDControllerLift liftPidController; // PID parameters for the lift motors public double liftMotorPID_p = -0.100; // Raise p = proportional public double liftMotorPID_i = 0.000; // Raise i = integral @@ -532,8 +524,8 @@ public enum LiftStoreActivity { public void startViperSlideExtension(int targetEncoderCount ) { // Range-check the target - if( targetEncoderCount < VIPER_EXTEND_ZERO ) targetEncoderCount = VIPER_EXTEND_ZERO; - if( targetEncoderCount > VIPER_EXTEND_FULL ) targetEncoderCount = VIPER_EXTEND_FULL; + if( targetEncoderCount < VIPER_EXTEND_ZERO ) targetEncoderCount = VIPER_EXTEND_ZERO; + if( targetEncoderCount > VIPER_EXTEND_FULL2 ) targetEncoderCount = VIPER_EXTEND_FULL2; // Configure target encoder count viperMotor.setTargetPosition( targetEncoderCount ); // Enable RUN_TO_POSITION mode diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java index a97c87f6cde9..e3f822ff8de3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java @@ -85,7 +85,6 @@ public abstract class Teleop extends LinearOpMode { Gamepad.RumbleEffect rumblePixelBinSingle; Gamepad.RumbleEffect rumblePixelBinDouble; - // sets unique behavior based on alliance public abstract void setAllianceSpecificBehavior(); @@ -135,7 +134,7 @@ public void runOpMode() throws InterruptedException { robot.odom.update(); Pose2D pos = robot.odom.getPosition(); // x,y pos in inch; heading in degrees String posStr = String.format(Locale.US, "{X,Y: %.1f, %.1f in H: %.1f deg}", - pos.getX(DistanceUnit.INCH), pos.getY(DistanceUnit.INCH), pos.getHeading(AngleUnit.DEGREES)); + pos.getX(DistanceUnit.INCH), pos.getY(DistanceUnit.INCH), -pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Position", posStr); Pose2D vel = robot.odom.getVelocity(); // x,y velocities in inch/sec; heading in deg/sec String velStr = String.format(Locale.US,"{X,Y: %.1f, %.1f in/sec, HVel: %.2f deg/sec}", @@ -196,6 +195,7 @@ public void runOpMode() throws InterruptedException { processPanControls(); processTiltControls(); ProcessViperLiftControls(); + robot.processViperSlideExtension(); processCollectorControls(); // Compute current cycle time @@ -563,7 +563,7 @@ void processTiltControls() { boolean safeToManuallyLower = (robot.wormTiltMotorPos > robot.TILT_ANGLE_HW_MIN); boolean safeToManuallyRaise = (robot.wormTiltMotorPos < robot.TILT_ANGLE_HW_MAX); double gamepad2_right_stick = gamepad2.right_stick_y; - boolean manual_tilt_control = ( Math.abs(gamepad2_right_stick) > 0.10 ); + boolean manual_tilt_control = ( Math.abs(gamepad2_right_stick) > 0.08 ); //=================================================================== // Check for an OFF-to-ON toggle of the gamepad1 CROSS button @@ -580,15 +580,15 @@ else if( gamepad1_l_bumper_now && !gamepad1_l_bumper_last ) //=================================================================== else if( manual_tilt_control || tiltAngleTweaked) { // Does user want to rotate turret DOWN (negative joystick input) - if( safeToManuallyLower && (gamepad2_right_stick < -0.10) ) { - double motorPower = 0.30 * gamepad2_right_stick; // NEGATIVE - robot.wormTiltMotor.setPower( motorPower ); // -3% to -30% + if( safeToManuallyLower && (gamepad2_right_stick < -0.08) ) { + double motorPower = 0.95 * gamepad2_right_stick; // NEGATIVE + robot.wormTiltMotor.setPower( motorPower ); // -8% to -95% tiltAngleTweaked = true; } // Does user want to rotate turret UP (positive joystick input) - else if( safeToManuallyRaise && (gamepad2_right_stick > 0.10) ) { - double motorPower = 0.60 * gamepad2_right_stick; // POSITIVE - robot.wormTiltMotor.setPower( motorPower ); // +6% to +60% + else if( safeToManuallyRaise && (gamepad2_right_stick > 0.08) ) { + double motorPower = 0.95 * gamepad2_right_stick; // POSITIVE + robot.wormTiltMotor.setPower( motorPower ); // +8% to +95% tiltAngleTweaked = true; } // No more input? Time to stop turret movement! @@ -602,8 +602,8 @@ else if(tiltAngleTweaked) { /*---------------------------------------------------------------------------------*/ void ProcessViperLiftControls() { - boolean safeToManuallyLower = (robot.viperMotorPos > robot.VIPER_EXTEND_ZERO); - boolean safeToManuallyRaise = (robot.viperMotorPos < robot.VIPER_EXTEND_FULL); + boolean safeToManuallyRetract = (robot.viperMotorPos > robot.VIPER_EXTEND_ZERO); + boolean safeToManuallyExtend = (robot.viperMotorPos < robot.VIPER_EXTEND_FULL1); // Capture user inputs ONCE, in case they change during processing of this code // or we want to scale them down double gamepad2_left_trigger = gamepad2.left_trigger * 1.00; @@ -614,22 +614,22 @@ void ProcessViperLiftControls() { // Check for an OFF-to-ON toggle of the gamepad2 DPAD UP if( gamepad2_dpad_up_now && !gamepad2_dpad_up_last) { // Move lift to HIGH-SCORING position -// robot.startLiftMove( robot.VIPER_EXTEND_BASKET ); + robot.startLiftMove( robot.VIPER_EXTEND_BASKET ); } // Check for an OFF-to-ON toggle of the gamepad2 DPAD RIGHT else if( gamepad2_dpad_right_now && !gamepad2_dpad_right_last) { // Extend lift to the specimen-scoring hook-above-the-bar height // robot.startLiftMove( robot.VIPER_EXTEND_HOOK ); } - // Check for an OFF-to-ON toggle of the gamepad2 DPAD LEFT - else if( gamepad2_dpad_left_now && !gamepad2_dpad_left_last) + // Check for an OFF-to-ON toggle of the gamepad2 DPAD DOWN + else if( gamepad2_dpad_down_now && !gamepad2_dpad_down_last) { // Move lift to STORED position -// robot.startLiftStore(); + robot.startLiftMove( robot.VIPER_EXTEND_GRAB ); } //=================================================================== else if( manual_lift_control || liftTweaked ) { // Does user want to manually RAISE the lift? - if( safeToManuallyRaise && (gamepad2_right_trigger > 0.25) ) { + if( safeToManuallyExtend && (gamepad2_right_trigger > 0.25) ) { // Do we need to terminate an auto movement? robot.abortViperSlideExtension(); viperPower = gamepad2_right_trigger; @@ -637,7 +637,7 @@ else if( manual_lift_control || liftTweaked ) { liftTweaked = true; } // Does user want to manually LOWER the lift? - else if( safeToManuallyLower && (gamepad2_left_trigger > 0.25) ) { + else if( safeToManuallyRetract && (gamepad2_left_trigger > 0.25) ) { // Do we need to terminate an auto movement? robot.abortViperSlideExtension(); viperPower = robot.VIPER_LOWER_POWER; @@ -701,8 +701,8 @@ else if( gamepad2_r_bumper_now && !gamepad2_r_bumper_last) geckoServoEjecting = false; // (we can't be doing this) } // r_bumper - // Check for an OFF-to-ON toggle of the gamepad2 DPAD DOWN - else if( gamepad2_dpad_down_now && !gamepad2_dpad_down_last) + // Check for an OFF-to-ON toggle of the gamepad2 DPAD LEFT + else if( gamepad2_dpad_left_now && !gamepad2_dpad_left_last) { // Position for scoring a specimen on the submersible bar robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR); robot.wristServo.setPosition(robot.WRIST_SERVO_BAR); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopLift.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopLift.java deleted file mode 100644 index d158a0901b92..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopLift.java +++ /dev/null @@ -1,680 +0,0 @@ -/* FTC Team 7572 - Version 1.0 (10/01/2022) -*/ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; - -import java.util.Locale; - -/** - * TeleOp DriveTrain Only (with test modes). - */ -@TeleOp(name="Teleop-Lift", group="7592") -@Disabled -public class TeleopLift extends LinearOpMode { - boolean gamepad1_triangle_last, gamepad1_triangle_now = false; // Single Wheel Control - boolean gamepad1_circle_last, gamepad1_circle_now = false; // Backwards Drive mode (also turns off driver-centric mode) - boolean gamepad1_cross_last, gamepad1_cross_now = false; // UNUSED - boolean gamepad1_square_last, gamepad1_square_now = false; // Enables/calibrates driver-centric mode - boolean gamepad1_dpad_up_last, gamepad1_dpad_up_now = false; - boolean gamepad1_dpad_down_last, gamepad1_dpad_down_now = false; - boolean gamepad1_dpad_left_last, gamepad1_dpad_left_now = false; - boolean gamepad1_dpad_right_last, gamepad1_dpad_right_now = false; - boolean gamepad1_l_bumper_last, gamepad1_l_bumper_now = false; // UNUSED - boolean gamepad1_r_bumper_last, gamepad1_r_bumper_now = false; // UNUSED - - boolean gamepad2_triangle_last, gamepad2_triangle_now = false; // - boolean gamepad2_circle_last, gamepad2_circle_now = false; // - boolean gamepad2_cross_last, gamepad2_cross_now = false; // - boolean gamepad2_square_last, gamepad2_square_now = false; // - boolean gamepad2_dpad_up_last, gamepad2_dpad_up_now = false; // - boolean gamepad2_dpad_down_last, gamepad2_dpad_down_now = false; // - boolean gamepad2_dpad_left_last, gamepad2_dpad_left_now = false; // - boolean gamepad2_dpad_right_last, gamepad2_dpad_right_now = false; // - boolean gamepad2_l_bumper_last, gamepad2_l_bumper_now = false; // - boolean gamepad2_r_bumper_last, gamepad2_r_bumper_now = false; // - boolean gamepad2_touchpad_last, gamepad2_touchpad_now = false; // - boolean gamepad2_share_last, gamepad2_share_now = false; // - - double yTranslation, xTranslation, rotation; /* Driver control inputs */ - double rearLeft, rearRight, frontLeft, frontRight, maxPower; /* Motor power levels */ - boolean backwardDriveControl = false; // drive controls backward (other end of robot becomes "FRONT") - boolean controlMultSegLinear = true; - - final int DRIVER_MODE_SINGLE_WHEEL = 1; - final int DRIVER_MODE_STANDARD = 2; - final int DRIVER_MODE_DRV_CENTRIC = 3; - int driverMode = DRIVER_MODE_STANDARD; - double driverAngle = 0.0; /* for DRIVER_MODE_DRV_CENTRIC */ - - double viperPower = 0.0; - long nanoTimeCurr=0, nanoTimePrev=0; - double elapsedTime, elapsedHz; - - boolean geckoServoCollecting = false; // Is the collector servo currently intaking (true) or OFF (false); - boolean geckoServoEjecting = false; // Is the collector servo currently ejecting (true) or OFF (false); - boolean panAngleTweaked = false; // Reminder to zero power when PAN input stops - boolean tiltAngleTweaked = false; // Reminder to zero power when TILT input stops - boolean liftTweaked = false; // Reminder to zero power when LIFT input stops - boolean enableOdometry = true; // Process/report odometry updates? - - int geckoWheelState = 0; - - /* Declare OpMode members. */ - Hardware2025Bot robot = new Hardware2025Bot(); - @Override - public void runOpMode() throws InterruptedException { - - telemetry.addData("State", "Initializing (please wait)"); - telemetry.update(); - - // Initialize robot hardware - robot.init(hardwareMap,false); - - // Send telemetry message to signify robot waiting; - telemetry.addData("State", "Ready"); - telemetry.update(); - - // Wait for the game to start (driver presses PLAY) - waitForStart(); - - // run until the end of the match (driver presses STOP) - while (opModeIsActive()) - { - // Refresh gamepad button status - captureGamepad1Buttons(); - captureGamepad2Buttons(); - - // Bulk-refresh the Hub1/Hub2 device status (motor status, digital I/O) -- FASTER! - robot.readBulkData(); - - // Request an update from the Pinpoint odometry computer (single I2C read) - if( enableOdometry ) { - robot.odom.update(); - Pose2D pos = robot.odom.getPosition(); // x,y pos in inch; heading in degrees - String posStr = String.format(Locale.US, "{X,Y: %.1f, %.1f in H: %.1f deg}", - pos.getX(DistanceUnit.INCH), pos.getY(DistanceUnit.INCH), pos.getHeading(AngleUnit.DEGREES)); - telemetry.addData("Position", posStr); - Pose2D vel = robot.odom.getVelocity(); // x,y velocities in inch/sec; heading in deg/sec - String velStr = String.format(Locale.US,"{X,Y: %.1f, %.1f in/sec, HVel: %.2f deg/sec}", - vel.getX(DistanceUnit.INCH), vel.getY(DistanceUnit.INCH), vel.getHeading(AngleUnit.DEGREES)); - telemetry.addData("Velocity", velStr); - telemetry.addData("Status", robot.odom.getDeviceStatus()); - } - - // Check for an OFF-to-ON toggle of the gamepad1 SQUARE button (toggles DRIVER-CENTRIC drive control) - if( gamepad1_square_now && !gamepad1_square_last) - { - driverMode = DRIVER_MODE_DRV_CENTRIC; - } - - // Check for an OFF-to-ON toggle of the gamepad1 CIRCLE button (toggles STANDARD/BACKWARD drive control) - if( gamepad1_circle_now && !gamepad1_circle_last) - { - // If currently in DRIVER-CENTRIC mode, switch to STANDARD (robot-centric) mode - if( driverMode != DRIVER_MODE_STANDARD ) { - driverMode = DRIVER_MODE_STANDARD; - backwardDriveControl = true; // start with phone-end as front of robot - } - // Already in STANDARD mode; Just toggle forward/backward mode - else { - backwardDriveControl = !backwardDriveControl; // reverses which end of robot is "FRONT" - } - } - - telemetry.addData("circle","Robot-centric (fwd/back modes)"); - telemetry.addData("square","Driver-centric (set joystick!)"); - telemetry.addData("d-pad","Fine control (30%)"); - telemetry.addData(" "," "); - - if( processDpadDriveMode() == false ) { - // Control based on joystick; report the sensed values -// telemetry.addData("Joystick1", "x=%.3f, y=%.3f spin=%.3f", -// gamepad1.left_stick_x, -gamepad1.left_stick_y, gamepad1.right_stick_x ); -// telemetry.addData("Joystick2", "pan=%.3f, tilt=%.3f extend=%.3f", -// gamepad2.left_stick_x, -gamepad2.left_stick_y, gamepad2.right_stick_y ); - switch( driverMode ) { - case DRIVER_MODE_STANDARD : - telemetry.addData("Driver Mode", "STD-%s (cir)", - (backwardDriveControl)? "BACKWARD":"FORWARD" ); - processStandardDriveMode(); - break; - case DRIVER_MODE_DRV_CENTRIC : - telemetry.addData("Driver Mode", "DRIVER-CENTRIC (sq)" ); - processDriverCentricDriveMode(); - break; - default : - // should never happen; reset to standard drive mode - driverMode = DRIVER_MODE_STANDARD; - break; - } // switch() - } // processDpadDriveMode - -// processPanAndTilt(); - processPanControls(); - processTiltControls(); - ProcessViperLiftControls(); - processCollectorControls(); - - // Compute current cycle time - nanoTimePrev = nanoTimeCurr; - nanoTimeCurr = System.nanoTime(); - elapsedTime = (nanoTimeCurr - nanoTimePrev)/ 1000000.0; // msec - elapsedHz = 1000.0 / elapsedTime; - - // Update telemetry data -// telemetry.addData("Front", "%.2f (%.0f cts/sec) %.2f (%.0f cts/sec)", -// frontLeft, robot.frontLeftMotorVel, frontRight, robot.frontRightMotorVel ); -// telemetry.addData("Rear ", "%.2f (%.0f cts/sec) %.2f (%.0f cts/sec)", -// rearLeft, robot.rearLeftMotorVel, rearRight, robot.rearRightMotorVel ); -// telemetry.addData("Front", "%d %d counts", robot.frontLeftMotorPos, robot.frontRightMotorPos ); -// telemetry.addData("Back ", "%d %d counts", robot.rearLeftMotorPos, robot.rearRightMotorPos ); - telemetry.addData("Pan", "%d counts", robot.wormPanMotorPos ); - telemetry.addData("Tilt", "%d counts", robot.wormTiltMotorPos ); - telemetry.addData("Viper", "%d counts", robot.viperMotorPos ); - telemetry.addData("Elbow", "%.1f (%.1f deg)", robot.getElbowServoPos(), robot.getElbowServoAngle() ); - telemetry.addData("Wrist", "%.1f (%.1f deg)", robot.getWristServoPos(), robot.getElbowServoAngle() ); -// telemetry.addData("Gyro Angle", "%.1f degrees", robot.headingIMU() ); - telemetry.addData("CycleTime", "%.1f msec (%.1f Hz)", elapsedTime, elapsedHz ); - telemetry.update(); - - // Pause for metronome tick. 40 mS each cycle = update 25 times a second. -// robot.waitForTick(40); - } // opModeIsActive - - } // runOpMode - - /*---------------------------------------------------------------------------------*/ - void captureGamepad1Buttons() { - gamepad1_triangle_last = gamepad1_triangle_now; gamepad1_triangle_now = gamepad1.triangle; - gamepad1_circle_last = gamepad1_circle_now; gamepad1_circle_now = gamepad1.circle; - gamepad1_cross_last = gamepad1_cross_now; gamepad1_cross_now = gamepad1.cross; - gamepad1_square_last = gamepad1_square_now; gamepad1_square_now = gamepad1.square; - gamepad1_dpad_up_last = gamepad1_dpad_up_now; gamepad1_dpad_up_now = gamepad1.dpad_up; - gamepad1_dpad_down_last = gamepad1_dpad_down_now; gamepad1_dpad_down_now = gamepad1.dpad_down; - gamepad1_dpad_left_last = gamepad1_dpad_left_now; gamepad1_dpad_left_now = gamepad1.dpad_left; - gamepad1_dpad_right_last = gamepad1_dpad_right_now; gamepad1_dpad_right_now = gamepad1.dpad_right; - gamepad1_l_bumper_last = gamepad1_l_bumper_now; gamepad1_l_bumper_now = gamepad1.left_bumper; - gamepad1_r_bumper_last = gamepad1_r_bumper_now; gamepad1_r_bumper_now = gamepad1.right_bumper; -// gamepad1_touchpad_last = gamepad1_touchpad_now; gamepad1_touchpad_now = gamepad1.touchpad; - } // captureGamepad1Buttons - - /*---------------------------------------------------------------------------------*/ - void captureGamepad2Buttons() { - gamepad2_triangle_last = gamepad2_triangle_now; gamepad2_triangle_now = gamepad2.triangle; - gamepad2_circle_last = gamepad2_circle_now; gamepad2_circle_now = gamepad2.circle; - gamepad2_cross_last = gamepad2_cross_now; gamepad2_cross_now = gamepad2.cross; - gamepad2_square_last = gamepad2_square_now; gamepad2_square_now = gamepad2.square; - gamepad2_dpad_up_last = gamepad2_dpad_up_now; gamepad2_dpad_up_now = gamepad2.dpad_up; - gamepad2_dpad_down_last = gamepad2_dpad_down_now; gamepad2_dpad_down_now = gamepad2.dpad_down; - gamepad2_dpad_left_last = gamepad2_dpad_left_now; gamepad2_dpad_left_now = gamepad2.dpad_left; - gamepad2_dpad_right_last = gamepad2_dpad_right_now; gamepad2_dpad_right_now = gamepad2.dpad_right; - gamepad2_l_bumper_last = gamepad2_l_bumper_now; gamepad2_l_bumper_now = gamepad2.left_bumper; - gamepad2_r_bumper_last = gamepad2_r_bumper_now; gamepad2_r_bumper_now = gamepad2.right_bumper; -// gamepad2_touchpad_last = gamepad2_touchpad_now; gamepad2_touchpad_now = gamepad2.touchpad; -// gamepad2_share_last = gamepad2_share_now; gamepad2_share_now = gamepad2.share; - } // captureGamepad2Buttons - - /* TELE-OP: Mecanum-wheel drive control using Dpad (slow/fine-adjustment mode) */ - /*---------------------------------------------------------------------------------*/ - boolean processDpadDriveMode() { - double fineControlSpeed = 0.30; - boolean dPadMode = true; - // Only process 1 Dpad button at a time - if( gamepad1.dpad_up ) { - telemetry.addData("Dpad","FORWARD"); - frontLeft = fineControlSpeed; - frontRight = fineControlSpeed; - rearLeft = fineControlSpeed; - rearRight = fineControlSpeed; - } - else if( gamepad1.dpad_down ) { - telemetry.addData("Dpad","BACKWARD"); - frontLeft = -fineControlSpeed; - frontRight = -fineControlSpeed; - rearLeft = -fineControlSpeed; - rearRight = -fineControlSpeed; - } - else if( gamepad1.dpad_left ) { - telemetry.addData("Dpad","LEFT"); - frontLeft = -fineControlSpeed; - frontRight = fineControlSpeed; - rearLeft = fineControlSpeed; - rearRight = -fineControlSpeed; - } - else if( gamepad1.dpad_right ) { - telemetry.addData("Dpad","RIGHT"); - frontLeft = fineControlSpeed; - frontRight = -fineControlSpeed; - rearLeft = -fineControlSpeed; - rearRight = fineControlSpeed; - } - else { - dPadMode = false; - } - if( dPadMode ) { - robot.driveTrainMotors( frontLeft, frontRight, rearLeft, rearRight); - } - return dPadMode; - } // processDpadDriveMode - - private double minThreshold( double valueIn ) { - double valueOut; - - //========= NO/MINIMAL JOYSTICK INPUT ========= - if( Math.abs( valueIn) < 0.02 ) { - valueOut = 0.0; - } - else { - valueOut = valueIn; - } - return valueOut; - } // minThreshold - - private double multSegLinearRot( double valueIn ) { - double valueOut; - - //========= NO JOYSTICK INPUT ========= - if( Math.abs( valueIn) < 0.05 ) { - valueOut = 0.0; - } - //========= POSITIVE JOYSTICK INPUTS ========= - else if( valueIn > 0.0 ) { - if( valueIn < 0.33 ) { // NOTE: approx 0.06 required to **initiate** rotation - valueOut = (0.25 * valueIn) + 0.0650; // 0.02=0.070 0.33=0.1475 - } - else if( valueIn < 0.60 ) { - valueOut = (0.50 * valueIn) - 0.0175; // 0.33=0.1475 0.60=0.2825 - } - else if( valueIn < 0.90 ) { - valueOut = (0.75 * valueIn) - 0.1675; // 0.60=0.2825 0.90=0.5075 - } - else - valueOut = (6.00 * valueIn) - 4.8925; // 0.90=0.5075 1.00=1.1075 (clipped!) - } - //========= NEGATIVE JOYSTICK INPUTS ========= - else { // valueIn < 0.0 - if( valueIn > -0.33 ) { - valueOut = (0.25 * valueIn) - 0.0650; - } - else if( valueIn > -0.60 ) { - valueOut = (0.50 * valueIn) + 0.0175; - } - else if( valueIn > -0.90 ) { - valueOut = (0.75 * valueIn) + 0.1675; - } - else - valueOut = (6.00 * valueIn) + 4.8925; - } - - return valueOut/2.0; - } // multSegLinearRot - - private double multSegLinearXY( double valueIn ) { - double valueOut; - - //========= NO JOYSTICK INPUT ========= - if( Math.abs( valueIn) < 0.05 ) { - valueOut = 0.0; - } - //========= POSITIVE JOYSTICK INPUTS ========= - else if( valueIn > 0.0 ) { - if( valueIn < 0.50 ) { // NOTE: approx 0.06 required to **initiate** rotation - valueOut = (0.25 * valueIn) + 0.040; // 0.01=0.0425 0.50=0.1650 - } - else if( valueIn < 0.90 ) { - valueOut = (0.75 * valueIn) - 0.210; // 0.50=0.1650 0.90=0.4650 - } - else - valueOut = (8.0 * valueIn) - 6.735; // 0.90=0.4650 1.00=1.265 (clipped) - } - //========= NEGATIVE JOYSTICK INPUTS ========= - else { // valueIn < 0.0 - if( valueIn > -0.50 ) { - valueOut = (0.25 * valueIn) - 0.040; - } - else if( valueIn > -0.90 ) { - valueOut = (0.75 * valueIn) + 0.210; - } - else - valueOut = (8.0 * valueIn) + 6.735; - } - - return valueOut; - } // multSegLinearXY - - /*---------------------------------------------------------------------------------*/ - /* TELE-OP: Standard Mecanum-wheel drive control (no dependence on gyro!) */ - /*---------------------------------------------------------------------------------*/ - void processStandardDriveMode() { - // Retrieve X/Y and ROTATION joystick input - if( controlMultSegLinear ) { - yTranslation = multSegLinearXY( -gamepad1.left_stick_y ); - xTranslation = multSegLinearXY( gamepad1.left_stick_x ); - rotation = multSegLinearRot( -gamepad1.right_stick_x ); - } - else { - yTranslation = -gamepad1.left_stick_y * 1.00; - xTranslation = gamepad1.left_stick_x * 1.25; - rotation = -gamepad1.right_stick_x * 0.50; - } - // If BACKWARD drive control, reverse the operator inputs - if( backwardDriveControl ) { - yTranslation = -yTranslation; - xTranslation = -xTranslation; - //rotation = -rotation; // clockwise/counterclockwise doesn't change - } // backwardDriveControl - // Normal teleop drive control: - // - left joystick is TRANSLATE fwd/back/left/right - // - right joystick is ROTATE clockwise/counterclockwise - // NOTE: assumes the right motors are defined FORWARD and the - // left motors are defined REVERSE so positive power is FORWARD. - frontRight = yTranslation - xTranslation + rotation; - frontLeft = yTranslation + xTranslation - rotation; - rearRight = yTranslation + xTranslation + rotation; - rearLeft = yTranslation - xTranslation - rotation; - // Normalize the values so none exceed +/- 1.0 - maxPower = Math.max( Math.max( Math.abs(rearLeft), Math.abs(rearRight) ), - Math.max( Math.abs(frontLeft), Math.abs(frontRight) ) ); - if (maxPower > 1.0) - { - rearLeft /= maxPower; - rearRight /= maxPower; - frontLeft /= maxPower; - frontRight /= maxPower; - } - // Update motor power settings: - robot.driveTrainMotors( frontLeft, frontRight, rearLeft, rearRight ); - } // processStandardDriveMode - - /*---------------------------------------------------------------------------------*/ - /* TELE-OP: Driver-centric Mecanum-wheel drive control (depends on gyro!) */ - /*---------------------------------------------------------------------------------*/ - void processDriverCentricDriveMode() { - double leftFrontAngle, rightFrontAngle, leftRearAngle, rightRearAngle; - double gyroAngle; - - // Retrieve X/Y and ROTATION joystick input - if( controlMultSegLinear ) { - yTranslation = multSegLinearXY( -gamepad1.left_stick_y ); - xTranslation = multSegLinearXY( gamepad1.left_stick_x ); - rotation = multSegLinearRot( -gamepad1.right_stick_x ); - } - else { - yTranslation = -gamepad1.left_stick_y; - xTranslation = gamepad1.left_stick_x; - rotation = -gamepad1.right_stick_x; - } - gyroAngle = -robot.headingIMU(); - - if (gamepad1.square) { - // The driver presses SQUARE, then uses the left joystick to say what angle the robot - // is aiming. This will calculate the values as long as SQUARE is pressed, and will - // not drive the robot using the left stick. Once SQUARE is released, it will use the - // final calculated angle and drive with the left stick. Button should be released - // before stick. The default behavior of atan2 is 0 to -180 on Y Axis CCW, and 0 to - // 180 CW. This code normalizes that to 0 to 360 CCW from the Y Axis - driverAngle = -Math.toDegrees( Math.atan2( -gamepad1.left_stick_x, gamepad1.left_stick_y) ); - if (driverAngle < 0) { - driverAngle += 360.0; - } - driverAngle -= gyroAngle; - xTranslation = 0.0; - yTranslation = 0.0; - rotation = 0.0; - } - - // Adjust new gyro angle for the driver reference angle - gyroAngle += driverAngle; - - // Compute motor angles relative to current orientation - rightFrontAngle = Math.toRadians( gyroAngle + 315 ); // / pulls at 315deg (135+180) - leftFrontAngle = Math.toRadians( gyroAngle + 45 ); // \ pulls at 45deg - rightRearAngle = Math.toRadians( gyroAngle + 225 ); // \ pulls at 225deg (45+180) - leftRearAngle = Math.toRadians( gyroAngle + 135 ); // / pulls at 135 - - frontRight = (yTranslation * Math.sin(rightFrontAngle) + xTranslation * Math.cos(rightFrontAngle))/Math.sqrt(2) + rotation; - frontLeft = (yTranslation * Math.sin(leftFrontAngle) + xTranslation * Math.cos(leftFrontAngle))/Math.sqrt(2) + rotation; - rearRight = (yTranslation * Math.sin(rightRearAngle) + xTranslation * Math.cos(rightRearAngle))/Math.sqrt(2) + rotation; - rearLeft = (yTranslation * Math.sin(leftRearAngle) + xTranslation * Math.cos(leftRearAngle))/Math.sqrt(2) + rotation; - - // Normalize the values so none exceed +/- 1.0 - maxPower = Math.max( Math.max( Math.abs(rearLeft), Math.abs(rearRight) ), - Math.max( Math.abs(frontLeft), Math.abs(frontRight) ) ); - if (maxPower > 1.0) - { - rearLeft /= maxPower; - rearRight /= maxPower; - frontLeft /= maxPower; - frontRight /= maxPower; - } - - // Update motor power settings (left motors are defined as REVERSE mode) - robot.driveTrainMotors( -frontLeft, frontRight, -rearLeft, rearRight ); - - } // processDriverCentricDriveMode - - /*---------------------------------------------------------------------------------*/ - void processPanAndTilt() { - - // Pan movement - if(Math.abs(gamepad2.left_stick_x) > 0.20) { - double panPower = (gamepad2.left_stick_x > 0.0)? 0.10 : -0.10; - robot.wormPanMotor.setPower(panPower); - } - else { - robot.wormPanMotor.setPower(0.0); - } - - // Tilt movement - if(Math.abs(gamepad2.left_stick_y) > 0.05) { - double tiltPower = (gamepad2.left_stick_y > 0.0)? 0.30 : -0.30; - robot.wormTiltMotor.setPower(tiltPower); - } - else { - robot.wormTiltMotor.setPower(0.0); - } - } // processPanAndTilt - - /*---------------------------------------------------------------------------------*/ - void processPanControls() { - boolean safeToManuallyLeft = (robot.wormPanMotorPos > robot.PAN_ANGLE_HW_MIN); - boolean safeToManuallyRight = (robot.wormPanMotorPos < robot.PAN_ANGLE_HW_MAX); - double gamepad2_left_stick = gamepad2.left_stick_x; - boolean manual_pan_control = ( Math.abs(gamepad2_left_stick) > 0.15 ); - - //=================================================================== - // Check for an OFF-to-ON toggle of the gamepad1 LEFT BUMPER - if( gamepad1_l_bumper_now && !gamepad1_l_bumper_last ) - { - } - // Check for an OFF-to-ON toggle of the gamepad1 RIGHT BUMPER - else if( gamepad1_r_bumper_now && !gamepad1_r_bumper_last ) - { - } - - //=================================================================== - else if( manual_pan_control || panAngleTweaked) { - // Does user want to rotate turret LEFT (negative joystick input) - if( safeToManuallyLeft && (gamepad2_left_stick < -0.15) ) { - double motorPower = 0.20 * gamepad2_left_stick; // NEGATIVE - robot.wormPanMotor.setPower( motorPower ); // -3% to -20% - panAngleTweaked = true; - } - // Does user want to rotate turret RIGHT (positive joystick input) - else if( safeToManuallyRight && (gamepad2_left_stick > 0.15) ) { - double motorPower = 0.20 * gamepad2_left_stick; // POSITIVE - robot.wormPanMotor.setPower( motorPower ); // +3% to +20% - panAngleTweaked = true; - } - // No more input? Time to stop turret movement! - else if(panAngleTweaked) { - robot.wormPanMotor.setPower( 0.0 ); - panAngleTweaked = false; - } - } // manual_pan_control - - } // processPanControls - - /*---------------------------------------------------------------------------------*/ - void processTiltControls() { - boolean safeToManuallyLower = (robot.wormTiltMotorPos > robot.TILT_ANGLE_HW_MIN); - boolean safeToManuallyRaise = (robot.wormTiltMotorPos < robot.TILT_ANGLE_HW_MAX); - double gamepad2_right_stick = gamepad2.right_stick_y; - boolean manual_tilt_control = ( Math.abs(gamepad2_right_stick) > 0.10 ); - - //=================================================================== - // Check for an OFF-to-ON toggle of the gamepad1 CROSS button - if( gamepad1_cross_now && !gamepad1_cross_last) - { - // robot.turretPIDPosInit( robot.PAN_ANGLE_CENTER ); - } - //=================================================================== - // Check for an OFF-to-ON toggle of the gamepad1 LEFT BUMPER - else if( gamepad1_l_bumper_now && !gamepad1_l_bumper_last ) - { - } - - //=================================================================== - else if( manual_tilt_control || tiltAngleTweaked) { - // Does user want to rotate turret DOWN (negative joystick input) - if( safeToManuallyLower && (gamepad2_right_stick < -0.10) ) { - double motorPower = 0.30 * gamepad2_right_stick; // NEGATIVE - robot.wormTiltMotor.setPower( motorPower ); // -3% to -30% - tiltAngleTweaked = true; - } - // Does user want to rotate turret UP (positive joystick input) - else if( safeToManuallyRaise && (gamepad2_right_stick > 0.10) ) { - double motorPower = 0.60 * gamepad2_right_stick; // POSITIVE - robot.wormTiltMotor.setPower( motorPower ); // +6% to +60% - tiltAngleTweaked = true; - } - // No more input? Time to stop turret movement! - else if(tiltAngleTweaked) { - robot.wormTiltMotor.setPower( 0.0 ); - tiltAngleTweaked = false; - } - } // manual_tilt_control - - } // processTiltControls - - /*---------------------------------------------------------------------------------*/ - void ProcessViperLiftControls() { - boolean safeToManuallyLower = (robot.viperMotorPos > robot.VIPER_EXTEND_ZERO); - boolean safeToManuallyRaise = (robot.viperMotorPos < robot.VIPER_EXTEND_FULL); - // Capture user inputs ONCE, in case they change during processing of this code - // or we want to scale them down - double gamepad2_left_trigger = gamepad2.left_trigger * 1.00; - double gamepad2_right_trigger = gamepad2.right_trigger * 1.00; - boolean manual_lift_control = ( (gamepad2_left_trigger > 0.25) || (gamepad2_right_trigger > 0.25) ); - - //=================================================================== - // Check for an OFF-to-ON toggle of the gamepad2 DPAD UP - if( gamepad2_dpad_up_now && !gamepad2_dpad_up_last) - { // Move lift to HIGH-SCORING position -// robot.startLiftMove( robot.VIPER_EXTEND_BASKET ); - } - // Check for an OFF-to-ON toggle of the gamepad2 DPAD RIGHT - else if( gamepad2_dpad_right_now && !gamepad2_dpad_right_last) - { // Extend lift to the specimen-scoring hook-above-the-bar height -// robot.startLiftMove( robot.VIPER_EXTEND_HOOK ); - } - // Check for an OFF-to-ON toggle of the gamepad2 DPAD LEFT - else if( gamepad2_dpad_left_now && !gamepad2_dpad_left_last) - { // Move lift to STORED position -// robot.startLiftStore(); - } - //=================================================================== - else if( manual_lift_control || liftTweaked ) { - // Does user want to manually RAISE the lift? - if( safeToManuallyRaise && (gamepad2_right_trigger > 0.25) ) { - // Do we need to terminate an auto movement? - robot.abortViperSlideExtension(); - viperPower = gamepad2_right_trigger; - robot.viperMotor.setPower( viperPower ); // fixed power? (robot.VIPER_RAISE_POWER) - liftTweaked = true; - } - // Does user want to manually LOWER the lift? - else if( safeToManuallyLower && (gamepad2_left_trigger > 0.25) ) { - // Do we need to terminate an auto movement? - robot.abortViperSlideExtension(); - viperPower = robot.VIPER_LOWER_POWER; - robot.viperMotor.setPower( viperPower ); - liftTweaked = true; - } - // No more input? Time to stop lift movement! - else if( liftTweaked ) { - // if the lift is near the bottom, truly go to zero power - // but if in a raised position, only drop to minimal holding power - boolean closeToZero = (Math.abs(robot.viperMotorPos - robot.VIPER_EXTEND_ZERO) < 20); - viperPower = closeToZero? 0.0 : robot.VIPER_HOLD_POWER; - robot.viperMotor.setPower( viperPower ); - liftTweaked = false; - } - } // manual_lift_control - - } // ProcessLiftControls - - /*---------------------------------------------------------------------------------*/ - void processCollectorControls() { - // Check for an OFF-to-ON toggle of the gamepad2 CIRCLE button - // - rotates the wrist/elbow to the floor collection orientation - // TODO: check tilt motor for safe height above floor! - if( gamepad2_circle_now && !gamepad2_circle_last) - { - robot.elbowServo.setPosition(robot.ELBOW_SERVO_GRAB); - robot.wristServo.setPosition(robot.WRIST_SERVO_GRAB); - } - // Check for an OFF-to-ON toggle of the gamepad2 CROSS button - // - rotates the wrist/elbow to the horizontal transport position - // TODO: check tilt motor for safe height above floor! - if( gamepad2_cross_now && !gamepad2_cross_last) - { - robot.elbowServo.setPosition(robot.ELBOW_SERVO_SAFE); - robot.wristServo.setPosition(robot.WRIST_SERVO_SAFE); - } - // Check for an OFF-to-ON toggle of the gamepad2 left or right bumpers - // - right enables the collector intake servo in FORWARD/collect mode - // - left enables the collector intake servo in REVERSE/eject/score mode - if( gamepad2_l_bumper_now && !gamepad2_l_bumper_last) - { - if( geckoServoEjecting ) { - robot.geckoServo.setPower( 0.0 ); // toggle eject OFF - geckoServoEjecting = false; - } else /* not ejecting */ { - robot.geckoServo.setPower( 1.0 ); // toggle eject ON - geckoServoEjecting = true; - } - geckoServoCollecting = false; // (we can't be doing this) - } // l_bumper - else if( gamepad2_r_bumper_now && !gamepad2_r_bumper_last) - { - if( geckoServoCollecting ) { - robot.geckoServo.setPower( 0.0 ); // toggle collect OFF - geckoServoCollecting = false; - } else /* not collecting */ { - robot.geckoServo.setPower( -1.0 ); // toggle collect ON - geckoServoCollecting = true; - } - geckoServoEjecting = false; // (we can't be doing this) - } // r_bumper - - // Check for an OFF-to-ON toggle of the gamepad2 DPAD DOWN - else if( gamepad2_dpad_down_now && !gamepad2_dpad_down_last) - { // Position for scoring a specimen on the submersible bar - robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR); - robot.wristServo.setPosition(robot.WRIST_SERVO_BAR); - } - - } // processCollectorControls - - /*---------------------------------------------------------------------------------*/ - - -} // TeleopLift