From 66c45dc5a6810ac9acf515e17f6b6c7d71ba0178 Mon Sep 17 00:00:00 2001 From: OviedoRobotics <54600937+OviedoRobotics@users.noreply.github.com> Date: Thu, 5 Dec 2024 20:56:39 -0500 Subject: [PATCH] Not perfect, but close. --- .../ftc/teamcode/AutonomousBase.java | 4 +- .../ftc/teamcode/AutonomousLeftRed.java | 165 ++++----- .../ftc/teamcode/Hardware2025Bot.java | 106 ++++-- .../firstinspires/ftc/teamcode/Teleop.java | 336 ++++++++++++++++-- 4 files changed, 452 insertions(+), 159 deletions(-) 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 ba2591db02f4..9d82e852d1a3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousBase.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousBase.java @@ -276,7 +276,7 @@ else if( gamepad1_cross_now && !gamepad1_cross_last ) { telemetry.addData("Park Location","%s %s", parkLocationStr[parkLocation], ((initMenuSelected==5)? "<-":" ")); telemetry.addData("spike specimens", "%d %s",spikeSamples,((initMenuSelected==6)? "<-":" ") ); - telemetry.addData(">","version 100" ); + telemetry.addData(">","version 102" ); telemetry.update(); } // processAutonomousInitMenu @@ -677,7 +677,7 @@ public boolean strafeToWall(boolean leftWall, double maxSpeed, int distanceFromW rotatePower = angleError * angleErrorMult; drivePower = distanceError * distanceErrorMult; drivePower = leftWall ? drivePower : -drivePower; - drivePower = Math.copySign(Math.min(Math.max(Math.abs(drivePower), robot.MIN_STRAFE_POW), maxPower), drivePower); + drivePower = Math.copySign(Math.min(Math.max(Math.abs(drivePower), Hardware2025Bot.MIN_STRAFE_POW), maxPower), drivePower); fl = -drivePower + rotatePower; fr = drivePower - rotatePower; bl = drivePower + rotatePower; 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 28ad61fd03ec..54ab14b02239 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftRed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftRed.java @@ -158,11 +158,16 @@ private void mainAutonomous() { } // Score the preloaded specimen -// if( !onlyPark && scorePreloadSpecimen ) { -// scoreSpecimenPreload(); -// } + if( !onlyPark && scorePreloadSpecimen ) { + scoreSpecimenPreload(); + } if( !onlyPark && (spikeSamples > 0) ) { + driveToPosition( 16.0, -19.0, 0.0, DRIVE_SPEED_90, TURN_SPEED_50, DRIVE_THRU ); + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_DRIVE_DEG); + autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_AUTO_READY); + robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_GRAB); + robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_GRAB); // Score starting sample // scoreSample(); int samplesScored = 0; @@ -173,7 +178,6 @@ private void mainAutonomous() { samplesScored++; } } - sleep(2000); // Park for 3pts (level 1 ascent) // level1Ascent(); @@ -189,41 +193,29 @@ private void scoreSpecimenPreload() { telemetry.update(); // Move away from field wall (viper slide motor will hit field wall if we tilt up too soon!) pos_y=3.0; pos_x=0.0; pos_angle=0.0; // start at this absolute location - driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_50, TURN_SPEED_20, DRIVE_THRU ); - autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO1_DEG); - pos_y += 3.0; driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_70, TURN_SPEED_20, DRIVE_THRU ); - robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR1); - robot.wristServo.setPosition(robot.WRIST_SERVO_BAR1); + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO1_DEG); pos_y += 3.0; driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_70, TURN_SPEED_20, DRIVE_THRU ); - robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR2); - robot.wristServo.setPosition(robot.WRIST_SERVO_BAR2); - // small shift right in preparation for 45deg rotation (minimize shift toward alliance partner) + robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_BAR2); + robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_BAR2); + pos_y += 6.0; pos_x += 3.0; + // small shift right in preparation for 45deg rotation (minimize shift toward alliance partner) driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_70, TURN_SPEED_20, DRIVE_TO ); - robot.driveTrainMotorsZero(); - do { - if( !opModeIsActive() ) break; - // wait for lift/tilt to finish... - sleep( 150 ); - // update all our status - performEveryLoop(); - } while( autoTiltMotorMoving() ); + autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_AUTO1); } // opModeIsActive if( opModeIsActive() ) { - pos_y += 23.0; pos_x += 3.5; pos_angle = 45.0; + pos_y += 23.0; pos_x += 5.0; pos_angle = 45.0; driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_70, TURN_SPEED_50, DRIVE_TO ); - robot.driveTrainMotorsZero(); - autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO1); do { if( !opModeIsActive() ) break; // wait for lift/tilt to finish... - sleep( 200 ); + sleep( 50 ); // update all our status performEveryLoop(); - } while( autoViperMotorMoving() ); + } while( autoViperMotorMoving() || autoTiltMotorMoving()); } // opModeIsActive // Rotate lift down to get specimen close to bar @@ -233,7 +225,7 @@ private void scoreSpecimenPreload() { do { if( !opModeIsActive() ) break; // wait for lift/tilt to finish... - sleep( 150 ); + sleep( 50 ); // update all our status performEveryLoop(); } while( autoTiltMotorMoving() ); @@ -241,11 +233,11 @@ private void scoreSpecimenPreload() { // Retract lift to clip the specimen on the bar if( opModeIsActive() ) { - autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO2); + autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_AUTO2); do { if( !opModeIsActive() ) break; // wait for lift/tilt to finish... - sleep( 200 ); + sleep( 50 ); // update all our status performEveryLoop(); } while( autoViperMotorMoving() ); @@ -258,33 +250,63 @@ private void scoreSpecimenPreload() { robot.geckoServo.setPower(0.0); // stop } // opModeIsActive - // Retract the arm for parking + // Setup the arm for scoring samples if( opModeIsActive() ) { - autoViperMotorMoveToTarget( robot.VIPER_EXTEND_ZERO); + autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_SECURE); do { if( !opModeIsActive() ) break; // wait for lift/tilt to finish... - sleep( 200 ); + sleep( 50 ); // update all our status performEveryLoop(); } while( autoViperMotorMoving() ); - robot.elbowServo.setPosition(robot.ELBOW_SERVO_INIT); - robot.wristServo.setPosition(robot.WRIST_SERVO_INIT); + robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_GRAB); + robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_GRAB); } // opModeIsActive + } // scoreSpecimenPreload - // Lower the arm for parking - if( opModeIsActive() ) { - autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ZERO_DEG); - do { - if( !opModeIsActive() ) break; - // wait for lift/tilt to finish... - sleep( 150 ); - // update all our status - performEveryLoop(); - } while( autoTiltMotorMoving() ); - } // opModeIsActive + //************************************ + // Collect sample + //************************************ + private void collectSample(int samplesScored) { + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_DRIVE_DEG); + autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_AUTO_READY); + robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_GRAB); + robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_GRAB); - } // scoreSpecimenPreload + switch(samplesScored) { + case 0: + // Drive forward toward the wall + driveToPosition( 16.0, -37.0, 0.0, DRIVE_SPEED_90, TURN_SPEED_50, DRIVE_TO ); + break; + case 1: + driveToPosition( 17.0, -46.0, 0.0, DRIVE_SPEED_90, TURN_SPEED_50, DRIVE_TO ); + break; + case 2: + driveToPosition( 17.5, -53.0, 7.0, DRIVE_SPEED_90, TURN_SPEED_50, DRIVE_TO ); + break; + default: + } + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_COLLECT_DEG); + do { + if( !opModeIsActive() ) break; + // wait for lift/tilt to finish... + sleep( 50 ); + // update all our status + performEveryLoop(); + } while( autoTiltMotorMoving() ); + robot.geckoServo.setPower( -1.0 ); + autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_AUTO_COLLECT, 0.5); + do { + if( !opModeIsActive() ) break; + // wait for lift/tilt to finish... + sleep( 50 ); + // update all our status + performEveryLoop(); + } while( autoViperMotorMoving() ); + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_DRIVE_DEG); + autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_AUTO_READY); + } // collectSample //************************************ // Score Sample @@ -293,13 +315,13 @@ private void scoreSample() { do { if( !opModeIsActive() ) break; // wait for lift/tilt to finish... - sleep( 200 ); + sleep( 50 ); // update all our status performEveryLoop(); } while( autoTiltMotorMoving() ); autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO_PRE_DEG); driveToPosition( 3.5, -47.5, -32.0, DRIVE_SPEED_90, TURN_SPEED_50, DRIVE_TO ); - robot.startViperSlideExtension( robot.VIPER_EXTEND_BASKET ); + robot.startViperSlideExtension( Hardware2025Bot.VIPER_EXTEND_BASKET ); robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_SAFE); robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_AUTO_SCORE); do { @@ -322,55 +344,10 @@ private void scoreSample() { robot.geckoServo.setPower( 0.0 ); robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_GRAB); robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_GRAB); - autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO_READY); + autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_AUTO_READY); autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_DRIVE_DEG); } // scoreSample - //************************************ - // Collect sample - //************************************ - private void collectSample(int samplesScored) { - autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_DRIVE_DEG); - autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO_READY); - - switch(samplesScored) { - case 0: - // Drive forward toward the wall - driveToPosition( 16.0, -38.0, 0.0, DRIVE_SPEED_90, TURN_SPEED_50, DRIVE_TO ); - break; - case 1: - driveToPosition( 17.0, -46.5, 0.0, DRIVE_SPEED_90, TURN_SPEED_50, DRIVE_TO ); - break; - case 2: - driveToPosition( 17.5, -53.0, 7.0, DRIVE_SPEED_90, TURN_SPEED_50, DRIVE_TO ); - break; - default: - } - robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_GRAB); - robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_GRAB); - autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_COLLECT_DEG); - do { - if( !opModeIsActive() ) break; - // wait for lift/tilt to finish... - sleep( 50 ); - // update all our status - performEveryLoop(); - } while( autoTiltMotorMoving() ); - robot.geckoServo.setPower( -1.0 ); - autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO_COLLECT, 0.5); - do { - if( !opModeIsActive() ) break; - // wait for lift/tilt to finish... - sleep( 50 ); - // update all our status - performEveryLoop(); - } while( autoViperMotorMoving() ); - robot.geckoServo.setPower( -0.30 ); - autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_DRIVE_DEG); - autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO_READY); - } // collectSample - - private void level1Ascent() { if( opModeIsActive() ) { // Back up from submersible @@ -387,7 +364,7 @@ private void level1Ascent() { } // opModeIsActive if( opModeIsActive() ) { - autoViperMotorMoveToTarget( robot.VIPER_EXTEND_GRAB); + autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_GRAB); autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ASCENT1_DEG); timeDriveStraight(-DRIVE_SPEED_20,3000); do { 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 305565cb20b7..5a04a29267b9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware2025Bot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware2025Bot.java @@ -98,6 +98,8 @@ public class Hardware2025Bot public double wormTiltMotorAmpsPk = 0.0; // peak power draw (Amps) 312rpm = 9.2A @ 12V public double wormTiltMotorSetPwr = 0.0; // requested power setting public double wormTiltMotorPwr = 0.0; // current power setting + public double WORM_TILT_VIPER_TO_BASKET_DEG= 45.0; // angle at when the viper motor starts extending when going to basket + public double WORM_TILT_VIPER_FROM_BASKET_DEG = 80; // angle at when the viper motor starts extending from the basket protected AnalogInput armTiltEncoder = null; // US Digital absolute magnetic encoder (MA3) public double armTiltAngle = 0.0; // 0V = 0 degrees; 3.3V = 359.99 degrees @@ -121,19 +123,20 @@ public class Hardware2025Bot // 94.4 deg 5 encoder counts // 94.5 deg 3896 encoder counts range public final static double ENCODER_COUNTS_PER_DEG = 3896.0 / 94.5; - public final static double TILT_ANGLE_HW_MAX_DEG = 95.00; // encoder at maximum rotation UP/BACK (horizontal = -200) - public final static double TILT_ANGLE_BASKET_DEG = 95.00; // encoder at rotation back to the basket for scoring - public final static double TILT_ANGLE_AUTO_PRE_DEG = 92.00; // encoder at rotation back to the basket for scoring - public final static double TILT_ANGLE_ASCENT1_DEG = 93.80; // encoder at rotation back to the low bar for ascent level 1 - public final static double TILT_ANGLE_RAISED_DEG = 54.50; // encoder at rotation back to the basket for scoring - public final static double TILT_ANGLE_HANG1_DEG = 40.10; // encoder when preparing for level 2 ascent - public final static double TILT_ANGLE_HANG2_DEG = 16.40; // encoder at the end of level 2 ascent - public final static double TILT_ANGLE_ZERO_DEG = 7.00; // encoder for parking fullyh reset in auto - public final static double TILT_ANGLE_DRIVE_DEG = 11.80; // encoder for parking in auto or driving around - public final static double TILT_ANGLE_AUTO1_DEG = 54.80; // tilted up for autonomous specimen scoring (above bar) - public final static double TILT_ANGLE_AUTO2_DEG = 49.60; // tilted up for autonomous specimen scoring (clipped) - public final static double TILT_ANGLE_HW_MIN_DEG = 0.00; // encoder at maximum rotation DOWN/FWD - public final static double TILT_ANGLE_COLLECT_DEG = 2.00; + public final static double TILT_ANGLE_HW_MAX_DEG = 95.00; // Arm at maximum rotation UP/BACK (horizontal = -200) + public final static double TILT_ANGLE_BASKET_DEG = 95.00; // Arm at rotation back to the basket for scoring + public final static double TILT_ANGLE_AUTO_PRE_DEG = 92.00; // Arm at rotation back to the basket for scoring + public final static double TILT_ANGLE_ASCENT1_DEG = 93.80; // Arm at rotation back to the low bar for ascent level 1 + public final static double TILE_ANGLE_BASKET_SAFE_DEG = 90.00; // Arm safe to rotate intake from basket + public final static double TILT_ANGLE_RAISED_DEG = 54.50; // Arm at rotation back to the basket for scoring + public final static double TILT_ANGLE_HANG1_DEG = 40.10; // Arm when preparing for level 2 ascent + public final static double TILT_ANGLE_HANG2_DEG = 16.40; // Arm at the end of level 2 ascent + public final static double TILT_ANGLE_ZERO_DEG = 7.00; // Arm for parking fully reset in auto + public final static double TILT_ANGLE_DRIVE_DEG = 11.80; // Arm for parking in auto or driving around + public final static double TILT_ANGLE_AUTO1_DEG = 54.80; // Arm tilted up for autonomous specimen scoring (above bar) + public final static double TILT_ANGLE_AUTO2_DEG = 49.60; // Arm tilted up for autonomous specimen scoring (clipped) + public final static double TILT_ANGLE_HW_MIN_DEG = 0.00; // Arm at maximum rotation DOWN/FWD + public final static double TILT_ANGLE_COLLECT_DEG = 2.00; // Arm to collect at ground level //====== Viper slide MOTOR (RUN_USING_ENCODER) ===== protected DcMotorEx viperMotor = null; @@ -146,25 +149,31 @@ public class Hardware2025Bot public double viperMotorPwr = 0.0; // current power setting public ElapsedTime viperSlideTimer = new ElapsedTime(); + public ElapsedTime wormTiltTimer = new ElapsedTime(); + public boolean viperMotorAutoMove = false; // have we commanded an automatic lift movement? public boolean viperMotorBusy = false; + public boolean wormTiltMotorAutoMove = false; // have we commanded an automatic lift movement? + public boolean wormTiltMotorBusy = false; public double VIPER_RAISE_POWER = 1.000; // Motor power used to EXTEND viper slide public double VIPER_HOLD_POWER = 0.001; // Motor power used to HOLD viper slide at current extension public double VIPER_LOWER_POWER = -0.500; // Motor power used to RETRACT viper slide // Encoder counts for 435 RPM lift motors theoretical max 5.8 rev * 384.54 ticks/rev = 2230.3 counts // 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_READY = 1000; // extend for collecting during auto - public int VIPER_EXTEND_AUTO_COLLECT = 2000; // extend for collecting during auto - public int VIPER_EXTEND_HANG1 = 2050; // extend to this to prepare for level 2 ascent - public int VIPER_EXTEND_HANG2 = 500; // retract to this extension during level 2 ascent - public int VIPER_EXTEND_GRAB = 1000; // extend for collection from submersible - public int VIPER_EXTEND_AUTO1 = 1400; // raised to where the specimen hook is above the high bar - public int VIPER_EXTEND_AUTO2 = 980; // retract to clip the specimen to the bar - public int VIPER_EXTEND_BASKET= 3000; // raised to basket-scoring height - public int VIPER_EXTEND_FULL1 = 2000; // extended 36" forward (max for 20"x42" limit) 2310 with overshoot - public int VIPER_EXTEND_FULL2 = 3010; // hardware fully extended (never exceed this count!) + final public static int VIPER_EXTEND_ZERO = 0; // fully retracted (may need to be adjustable??) + final public static int VIPER_EXTEND_AUTO_READY = 1000; // extend for collecting during auto + final public static int VIPER_EXTEND_AUTO_COLLECT = 2000; // extend for collecting during auto + final public static int VIPER_EXTEND_HANG1 = 2050; // extend to this to prepare for level 2 ascent + final public static int VIPER_EXTEND_HANG2 = 500; // retract to this extension during level 2 ascent + final public static int VIPER_EXTEND_GRAB = 1000; // extend for collection from submersible + final public static int VIPER_EXTEND_SECURE= 350; // Intake is tucked into robot to be safe + final public static int VIPER_EXTEND_SAFE = 750; // Intake is far enough it can safely swing + final public static int VIPER_EXTEND_AUTO1 = 1400; // raised to where the specimen hook is above the high bar + final public static int VIPER_EXTEND_AUTO2 = 850; // retract to clip the specimen to the bar + final public static int VIPER_EXTEND_BASKET= 3000; // raised to basket-scoring height + final public static int VIPER_EXTEND_FULL1 = 2000; // extended 36" forward (max for 20"x42" limit) 2310 with overshoot + final public static 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 @@ -609,6 +618,55 @@ public double getWristServoAngle() { /* startViperSlideExtension() */ /* NOTE: Comments online say the firmware that executes the motor RUN_TO_POSITION logic want */ /* the setup commands in this order: setTargetPosition(), setMode(), setPower(). */ + + public void startWormTilt(double targetArmAngle) + { // Convert angle to encoder counts + int targetEncoderCount = computeEncoderCountsFromAngle(targetArmAngle); + // Range-check the target + if( targetArmAngle < TILT_ANGLE_HW_MIN_DEG) targetEncoderCount = computeEncoderCountsFromAngle(TILT_ANGLE_HW_MIN_DEG); + if( targetArmAngle > TILT_ANGLE_HW_MAX_DEG ) targetEncoderCount = computeEncoderCountsFromAngle(TILT_ANGLE_HW_MAX_DEG); + + wormTiltMotor.setTargetPosition( targetEncoderCount ); + wormTiltMotor.setMode( DcMotor.RunMode.RUN_TO_POSITION ); + + wormTiltMotor.setPower(0.8); + wormTiltTimer.reset(); + wormTiltMotorAutoMove = true; + wormTiltMotorBusy = true; + } // startWormTilt + public void processWormTilt(){ + // Has the automatic movement reached its destination?. + if( wormTiltMotorAutoMove ) { + if (!wormTiltMotor.isBusy()) { + wormTiltMotorBusy = false; + // Timeout reaching destination. + } else if (wormTiltTimer.milliseconds() > 5000) { + wormTiltMotorBusy = false; + //telemetry.addData("processViperSlideExtension", "Movement timed out."); + //telemetry.addData("processViperSlideExtension", "Position: %d", viperMotor.getCurrentPosition()); + //telemetry.update(); + //telemetrySleep(); + } + } + } // processWormTilt + + public void abortWormTilt() + { + // Have we commanded an AUTOMATIC lift movement that we need to terminate so we + // can return to MANUAL control? (NOTE: we don't care here whether the AUTOMATIC + // movement has finished yet; MANUAL control immediately overrides that action. + if( wormTiltMotorAutoMove ) { + // turn off the auto-movement power, but don't go to ZERO POWER or + // the weight of the lift will immediately drop it back down. + wormTiltMotor.setMode( DcMotor.RunMode.RUN_USING_ENCODER ); + wormTiltMotor.setPower( 0.0 ); +// liftMoveState = LiftMoveActivity.IDLE; +// liftStoreState = LiftStoreActivity.IDLE; + wormTiltMotorAutoMove = false; + wormTiltMotorBusy = false; + } + } // abortWormTilt + public void startViperSlideExtension(int targetEncoderCount ) { // Range-check the target 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 34ec45046fde..c85dc77e1843 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; @@ -254,7 +255,12 @@ public void runOpMode() throws InterruptedException { /*---------------------------------------------------------------------------------*/ void performEveryLoopTeleop() { - robot.processViperSlideExtension(); + robot.processViperSlideExtension(); + robot.processWormTilt(); + processHoverArm(); + processSecureArm(); + processScoreArm(); + processScoreArmSpec(); } // performEveryLoopTeleop /*---------------------------------------------------------------------------------*/ @@ -612,6 +618,8 @@ else if( gamepad1_l_bumper_now && !gamepad1_l_bumper_last ) //=================================================================== else if( manual_tilt_control || tiltAngleTweaked) { + terminateAutoArmMovements(); + robot.abortWormTilt(); // Does user want to rotate turret DOWN (negative joystick input) if( safeToManuallyLower && (gamepad2_right_stick < -0.08) ) { double motorPower = 0.95 * gamepad2_right_stick; // NEGATIVE @@ -635,8 +643,8 @@ else if(tiltAngleTweaked) { /*---------------------------------------------------------------------------------*/ void ProcessViperLiftControls() { - boolean safeToManuallyRetract = (robot.viperMotorPos > robot.VIPER_EXTEND_ZERO); - boolean safeToManuallyExtend = (robot.viperMotorPos < robot.VIPER_EXTEND_FULL1); + boolean safeToManuallyRetract = (robot.viperMotorPos > Hardware2025Bot.VIPER_EXTEND_ZERO); + boolean safeToManuallyExtend = (robot.viperMotorPos < Hardware2025Bot.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; @@ -646,39 +654,31 @@ void ProcessViperLiftControls() { //=================================================================== // Check for an OFF-to-ON toggle of the gamepad2 DPAD UP if( gamepad2_dpad_up_now && !gamepad2_dpad_up_last) - { // Extend lift to the basket-scoring position - robot.startViperSlideExtension( robot.VIPER_EXTEND_BASKET ); - robot.elbowServo.setPosition(robot.ELBOW_SERVO_SAFE); -// robot.elbowServo.setPosition(robot.ELBOW_SERVO_DROP); - robot.wristServo.setPosition(robot.WRIST_SERVO_SAFE); -// robot.wristServo.setPosition(robot.WRIST_SERVO_DROP); + { + startScoreArm(); } // 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.startViperSlideExtension( robot.VIPER_EXTEND_AUTO1 ); - // Position for scoring a specimen on the submersible bar - robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR2); - robot.wristServo.setPosition(robot.WRIST_SERVO_BAR2); + { + startScoreArmSpec(); } // Check for an OFF-to-ON toggle of the gamepad2 DPAD DOWN else if( gamepad2_dpad_down_now && !gamepad2_dpad_down_last) - { // Retract lift to the collection position - boolean armRaised = (robot.armTiltAngle > Hardware2025Bot.TILT_ANGLE_RAISED_DEG)? true: false; - // If raised to the basket, do this automatically before lowering - // (don't want to do it if we use this backing out of submersimble - if (armRaised) { - robot.elbowServo.setPosition(robot.ELBOW_SERVO_GRAB); - robot.wristServo.setPosition(robot.WRIST_SERVO_GRAB); - sleep(250); - } - robot.startViperSlideExtension( robot.VIPER_EXTEND_GRAB ); + { + // Retract lift to the collection position + startHoverArm(); + } + // Check for an OFF-to-ON toggle of the gamepad2 DPAD RIGHT + else if( gamepad2_dpad_left_now && !gamepad2_dpad_left_last) + { + startSecureArm(); } //=================================================================== else if( manual_lift_control || liftTweaked ) { // Does user want to manually RAISE the lift? if( safeToManuallyExtend && (gamepad2_right_trigger > 0.25) ) { // Do we need to terminate an auto movement? + terminateAutoArmMovements(); robot.abortViperSlideExtension(); viperPower = gamepad2_right_trigger; robot.viperMotor.setPower( viperPower ); // fixed power? (robot.VIPER_RAISE_POWER) @@ -687,6 +687,7 @@ else if( manual_lift_control || liftTweaked ) { // Does user want to manually LOWER the lift? else if( safeToManuallyRetract && (gamepad2_left_trigger > 0.25) ) { // Do we need to terminate an auto movement? + terminateAutoArmMovements(); robot.abortViperSlideExtension(); viperPower = robot.VIPER_LOWER_POWER; robot.viperMotor.setPower( viperPower ); @@ -696,7 +697,7 @@ else if( safeToManuallyRetract && (gamepad2_left_trigger > 0.25) ) { 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); + boolean closeToZero = (Math.abs(robot.viperMotorPos - Hardware2025Bot.VIPER_EXTEND_ZERO) < 20); viperPower = closeToZero? 0.0 : robot.VIPER_HOLD_POWER; robot.viperMotor.setPower( viperPower ); liftTweaked = false; @@ -712,23 +713,30 @@ void processCollectorControls() { // TO DO: check tilt motor for safe height above floor for wrist rotation! if( gamepad2_circle_now && !gamepad2_circle_last) { - robot.elbowServo.setPosition(robot.ELBOW_SERVO_GRAB); - robot.wristServo.setPosition(robot.WRIST_SERVO_GRAB); + robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_GRAB); + robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_GRAB); } // Check for an OFF-to-ON toggle of the gamepad2 CROSS button // - rotates the wrist/elbow to the horizontal transport position // TO DO: check tilt motor for safe height above floor for wrist rotation! if( gamepad2_cross_now && !gamepad2_cross_last) { - robot.elbowServo.setPosition(robot.ELBOW_SERVO_SAFE); - robot.wristServo.setPosition(robot.WRIST_SERVO_SAFE); + robot.startWormTilt(Hardware2025Bot.TILT_ANGLE_COLLECT_DEG); + } + // Check for an OFF-to-ON toggle of the gamepad2 CROSS button + // - rotates the wrist/elbow to the horizontal transport position + // TO DO: check tilt motor for safe height above floor for wrist rotation! + if( gamepad2_triangle_now && !gamepad2_triangle_last) + { + robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_SAFE); + robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_SAFE); } // Check for an OFF-to-ON toggle of the gamepad2 SQUARE button // - rotates the wrist/elbow to the vertical init position if( gamepad2_square_now && !gamepad2_square_last ) { - robot.elbowServo.setPosition(robot.ELBOW_SERVO_INIT); - robot.wristServo.setPosition(robot.WRIST_SERVO_INIT); + robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_INIT); + robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_INIT); } // Check for an OFF-to-ON toggle of the gamepad2 left or right bumpers // - right enables the collector intake servo in FORWARD/collect mode @@ -778,26 +786,26 @@ void processLevel2Ascent() { // First instance of BOTH gamepad1 left/right bumpers initiates ascent prep if( gamepad1_l_bumper_now && gamepad1_r_bumper_now ) { - robot.wristServo.setPosition(robot.WRIST_SERVO_INIT); - robot.elbowServo.setPosition(robot.ELBOW_SERVO_INIT); + robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_INIT); + robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_INIT); ascent2telem = true; // start monitoring motor powers ascent2state = ASCENT_STATE_SETUP; } break; case ASCENT_STATE_SETUP: // Send TILT motor to hang position - robot.wormTiltMotor.setTargetPosition( robot.computeEncoderCountsFromAngle(Hardware2025Bot.TILT_ANGLE_HANG1_DEG) ); + robot.wormTiltMotor.setTargetPosition( Hardware2025Bot.computeEncoderCountsFromAngle(Hardware2025Bot.TILT_ANGLE_HANG1_DEG) ); robot.wormTiltMotor.setMode( DcMotor.RunMode.RUN_TO_POSITION ); robot.wormTiltMotor.setPower( 0.90 ); // Send LIFT motor to hang position - robot.viperMotor.setTargetPosition( robot.VIPER_EXTEND_HANG1 ); + robot.viperMotor.setTargetPosition( Hardware2025Bot.VIPER_EXTEND_HANG1 ); robot.viperMotor.setMode( DcMotor.RunMode.RUN_TO_POSITION ); robot.viperMotor.setPower( 0.90 ); ascent2state = ASCENT_STATE_MOVING; break; case ASCENT_STATE_MOVING : - boolean tiltReady = (Math.abs(robot.armTiltAngle - robot.TILT_ANGLE_HANG1_DEG) < 0.2)? true:false; - boolean viperReady = (Math.abs(robot.viperMotorPos - robot.VIPER_EXTEND_HANG1) < 20)? true:false; + boolean tiltReady = (Math.abs(robot.armTiltAngle - Hardware2025Bot.TILT_ANGLE_HANG1_DEG) < 0.2)? true:false; + boolean viperReady = (Math.abs(robot.viperMotorPos - Hardware2025Bot.VIPER_EXTEND_HANG1) < 20)? true:false; break; case ASCENT_STATE_READY : break; @@ -816,7 +824,7 @@ void processLevel2Ascent() { telemetry.addData("Pan Motor", "%.1f Amp (%.1f peak)", robot.wormPanMotorAmps, robot.wormPanMotorAmpsPk ); // Do we need to further retract viper slide motor? - boolean viperRetractionIncomplete = (robot.viperMotorPos > robot.VIPER_EXTEND_HANG2)? true : false; + boolean viperRetractionIncomplete = (robot.viperMotorPos > Hardware2025Bot.VIPER_EXTEND_HANG2)? true : false; boolean viperMotorLoadOkay = (robot.viperMotorAmps < 8.5)? true : false; if( viperRetractionIncomplete && viperMotorLoadOkay ) { robot.viperMotor.setPower( -1.0 ); @@ -835,4 +843,254 @@ void processLevel2Ascent() { } // processLevel2Ascent -} // TeleopLift + //************************************************************************************ + // Activity functions + //************************************************************************************ + public void terminateAutoArmMovements() { + abortSecureArm(); + abortHoverArm(); + abortScoreArm(); + abortScoreArmSpec(); + } + //************************** + // Hover Arm - This should have the robot ready to collect, except above the level of + // the low bar so we can go into the pit. + //************************** + public enum Hover_Arm_Steps { + IDLE, + ROTATING_ARM, + EXTENDING_ARM, + POSITION_INTAKE; + }; + public Hover_Arm_Steps hoverArmState = Hover_Arm_Steps.IDLE; + protected ElapsedTime hoverTimer = new ElapsedTime(); + public void startHoverArm(){ + if(hoverArmState == Hover_Arm_Steps.IDLE) { + terminateAutoArmMovements(); + robot.startWormTilt(Hardware2025Bot.TILT_ANGLE_DRIVE_DEG); + hoverArmState = Hover_Arm_Steps.ROTATING_ARM; + } + } + public void processHoverArm() { + switch(hoverArmState) { + case ROTATING_ARM: + // Check to see if arm is in the range to start changing the viper length + // and the intake will be ok + if((robot.armTiltAngle > Hardware2025Bot.TILT_ANGLE_ZERO_DEG) && + (robot.armTiltAngle < Hardware2025Bot.TILE_ANGLE_BASKET_SAFE_DEG)) { + robot.startViperSlideExtension(Hardware2025Bot.VIPER_EXTEND_GRAB); + hoverArmState = Hover_Arm_Steps.EXTENDING_ARM; + } + break; + case EXTENDING_ARM: + // Check to see if the arm is out far enough to swing the intake + if(robot.viperMotorPos > Hardware2025Bot.VIPER_EXTEND_SAFE) { + robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_GRAB); + robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_GRAB); + hoverTimer.reset(); + hoverArmState = Hover_Arm_Steps.POSITION_INTAKE; + } + break; + case POSITION_INTAKE: + // Wait for the arm, slides, and intake to complete movement. + if(!robot.viperMotorBusy && !robot.wormTiltMotorBusy && hoverTimer.milliseconds() >= 500) { + hoverArmState = Hover_Arm_Steps.IDLE; + } + break; + case IDLE: + default: + } + } + public void abortHoverArm() { + if(hoverArmState != Hover_Arm_Steps.IDLE) { + robot.abortWormTilt(); + robot.abortViperSlideExtension(); + hoverArmState = Hover_Arm_Steps.IDLE; + } + } + //************************** + // Secure Arm - This should have the robot ready to run, intake up and arm nestled. + //************************** + public enum Secure_Arm_Steps { + IDLE, + ROTATING_ARM, + EXTENDING_ARM, + POSITION_INTAKE, + RETRACT_ARM; + + }; + public Secure_Arm_Steps secureArmState = Secure_Arm_Steps.IDLE; + protected ElapsedTime secureTimer = new ElapsedTime(); + public void startSecureArm(){ + if(secureArmState == Secure_Arm_Steps.IDLE) { + terminateAutoArmMovements(); + robot.startWormTilt(Hardware2025Bot.TILT_ANGLE_DRIVE_DEG); + secureArmState = Secure_Arm_Steps.ROTATING_ARM; + } + } + public void processSecureArm() { + switch(secureArmState) { + case ROTATING_ARM: + // Check to see if arm is in the range to start changing the viper length + // and the intake will be ok + if((robot.armTiltAngle > Hardware2025Bot.TILT_ANGLE_ZERO_DEG) && + (robot.armTiltAngle < Hardware2025Bot.TILE_ANGLE_BASKET_SAFE_DEG)) { + if(robot.viperMotorPos < Hardware2025Bot.VIPER_EXTEND_SAFE) { + robot.startViperSlideExtension(Hardware2025Bot.VIPER_EXTEND_SAFE); + } + secureArmState = Secure_Arm_Steps.EXTENDING_ARM; + } + break; + case EXTENDING_ARM: + // Check to see if the arm is out far enough to swing the intake + if(!robot.viperMotorBusy) { + secureTimer.reset(); + robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_SAFE); + robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_SAFE); + secureArmState = Secure_Arm_Steps.POSITION_INTAKE; + } + break; + case POSITION_INTAKE: + // Wait for the intake to complete movement. + if(secureTimer.milliseconds() >= 500) { + robot.startViperSlideExtension(Hardware2025Bot.VIPER_EXTEND_SECURE); + secureArmState = Secure_Arm_Steps.RETRACT_ARM; + } + break; + case RETRACT_ARM: + if(!robot.viperMotorBusy) { + secureArmState = Secure_Arm_Steps.IDLE; + } + break; + case IDLE: + default: + } + } + public void abortSecureArm() { + if(secureArmState != Secure_Arm_Steps.IDLE) { + robot.abortWormTilt(); + robot.abortViperSlideExtension(); + secureArmState = Secure_Arm_Steps.IDLE; + } + } + //************************** + // Score Arm - This should have the robot ready to score in the top bucket, + // arm up, rotated back, and intake in the score position. + //************************** + public enum Score_Arm_Steps { + IDLE, + ROTATING_ARM, + RETRACTING_ARM, + EXTENDING_ARM, + POSITION_INTAKE; + }; + public Score_Arm_Steps scoreArmState = Score_Arm_Steps.IDLE; + protected ElapsedTime scoreTimer = new ElapsedTime(); + public void startScoreArm(){ + if(scoreArmState == Score_Arm_Steps.IDLE) { + terminateAutoArmMovements(); + robot.startWormTilt(Hardware2025Bot.TILT_ANGLE_BASKET_DEG); + scoreArmState = Score_Arm_Steps.ROTATING_ARM; + } + } + public void processScoreArm() { + switch(scoreArmState) { + case ROTATING_ARM: + // Check to see if arm is past upright value, or above low value + // to start viper retraction + if(robot.armTiltAngle > Hardware2025Bot.TILT_ANGLE_RAISED_DEG) { + scoreArmState = Score_Arm_Steps.RETRACTING_ARM; + } else if ((robot.armTiltAngle > Hardware2025Bot.TILT_ANGLE_ZERO_DEG) && + (robot.armTiltAngle < Hardware2025Bot.TILT_ANGLE_RAISED_DEG)) { + robot.startViperSlideExtension(Hardware2025Bot.VIPER_EXTEND_SAFE); + scoreArmState = Score_Arm_Steps.RETRACTING_ARM; + } + break; + case RETRACTING_ARM: + // Check if the arm is past the upright angle to extend viper slides. + if(robot.armTiltAngle > Hardware2025Bot.TILT_ANGLE_RAISED_DEG) { + robot.startViperSlideExtension(Hardware2025Bot.VIPER_EXTEND_BASKET); + scoreArmState = Score_Arm_Steps.EXTENDING_ARM; + } + break; + case EXTENDING_ARM: + // Check to see if the arm is out far enough to swing the intake + if(!robot.viperMotorBusy) { + robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_SAFE); + robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_SAFE); + scoreTimer.reset(); + scoreArmState = Score_Arm_Steps.POSITION_INTAKE; + } + break; + case POSITION_INTAKE: + if(!robot.viperMotorBusy && !robot.wormTiltMotorBusy && scoreTimer.milliseconds() >= 500) { + scoreArmState = Score_Arm_Steps.IDLE; + } + break; + case IDLE: + default: + } + } + public void abortScoreArm() { + if(scoreArmState != Score_Arm_Steps.IDLE) { + robot.abortWormTilt(); + robot.abortViperSlideExtension(); + scoreArmState = Score_Arm_Steps.IDLE; + } + } + //************************** + // Score Arm Specimen - This should have the robot ready to score on the top bar, + // arm up, rotated back, and intake in the clip position. + //************************** + public enum Score_Arm_Spec_Steps { + IDLE, + ROTATING_ARM, + EXTENDING_ARM, + POSITION_INTAKE; + }; + public Score_Arm_Spec_Steps scoreArmSpecState = Score_Arm_Spec_Steps.IDLE; + protected ElapsedTime scoreSpecTimer = new ElapsedTime(); + public void startScoreArmSpec(){ + if(scoreArmSpecState == Score_Arm_Spec_Steps.IDLE) { + terminateAutoArmMovements(); + robot.startWormTilt(Hardware2025Bot.TILT_ANGLE_AUTO1_DEG); + scoreArmSpecState = Score_Arm_Spec_Steps.ROTATING_ARM; + } + } + public void processScoreArmSpec() { + switch(scoreArmSpecState) { + case ROTATING_ARM: + // Check to see if arm is in the range to start changing the viper length + // and the intake will be ok + if((robot.armTiltAngle > Hardware2025Bot.TILT_ANGLE_ZERO_DEG) && + (robot.armTiltAngle < Hardware2025Bot.TILE_ANGLE_BASKET_SAFE_DEG)) { + robot.startViperSlideExtension(Hardware2025Bot.VIPER_EXTEND_AUTO1); + scoreArmSpecState = Score_Arm_Spec_Steps.EXTENDING_ARM; + } + break; + case EXTENDING_ARM: + // Check to see if the arm is out far enough to swing the intake + if(robot.viperMotorPos > Hardware2025Bot.VIPER_EXTEND_SAFE) { + robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_BAR2); + robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_BAR2); + scoreSpecTimer.reset(); + scoreArmSpecState = Score_Arm_Spec_Steps.POSITION_INTAKE; + } + break; + case POSITION_INTAKE: + if(!robot.viperMotorBusy && !robot.wormTiltMotorBusy && scoreSpecTimer.milliseconds() >= 500) { + scoreArmSpecState = Score_Arm_Spec_Steps.IDLE; + } + break; + case IDLE: + default: + } + } + public void abortScoreArmSpec() { + if(scoreArmSpecState != Score_Arm_Spec_Steps.IDLE) { + robot.abortWormTilt(); + robot.abortViperSlideExtension(); + scoreArmSpecState = Score_Arm_Spec_Steps.IDLE; + } + } +} // Teleop