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 bebb1e34a50b..ff4d852e7e5e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousBase.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousBase.java @@ -367,7 +367,7 @@ else if( autoViperMotorTimer.milliseconds() > 3000 ) { void autoTiltMotorMoveToTarget(double targetArmAngle ) { // Convert angle to encoder counts - int targetEncoderCount = robot.computeEncoderCountsFromAngle(robot.armTiltAngle); + int targetEncoderCount = Hardware2025Bot.computeEncoderCountsFromAngle(targetArmAngle); // Configure target encoder count robot.wormTiltMotor.setTargetPosition( targetEncoderCount ); // Enable RUN_TO_POSITION mode diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftBlue.java index 84dce395c941..37d1777ac9f4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftBlue.java @@ -312,7 +312,7 @@ private void level1Ascent() { if( opModeIsActive() ) { autoViperMotorMoveToTarget( robot.VIPER_EXTEND_GRAB); - autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_BASKET_DEG); + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ASCENT1_DEG); timeDriveStraight(-DRIVE_SPEED_20,3000); do { if( !opModeIsActive() ) break; 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 be7601630d68..f5f24329cdf6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftRed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftRed.java @@ -312,7 +312,7 @@ private void level1Ascent() { if( opModeIsActive() ) { autoViperMotorMoveToTarget( robot.VIPER_EXTEND_GRAB); - autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_BASKET_DEG); + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ASCENT1_DEG); timeDriveStraight(-DRIVE_SPEED_20,3000); do { if( !opModeIsActive() ) break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightBlue.java index 0fe61a93d5fb..7d791d96ce15 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightBlue.java @@ -336,11 +336,11 @@ private void herdSamples(int samplesToHerd) { pos_angle=-5.0; // angle the bot away from the wall as we herd the final sample driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_60, TURN_SPEED_40, DRIVE_THRU ); // Go fast to the edge of the observation zone - pos_y = 14.0; + pos_y = 15.0; pos_x -= 3.0; // end 5" away from the wall driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_80, TURN_SPEED_40, DRIVE_THRU ); // ease into the observation zone (in case we hit the wall, or another robot) - timeDriveStraight(-DRIVE_SPEED_20,1500); + timeDriveStraight(-DRIVE_SPEED_20,1000); } // opModeIsActive // If we did any herding, turn off the motors if (samplesToHerd > 0) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightRed.java index 5f2bf4bfd64f..c8fb4170951a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightRed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightRed.java @@ -336,11 +336,11 @@ private void herdSamples(int samplesToHerd) { pos_angle=-5.0; // angle the bot away from the wall as we herd the final sample driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_60, TURN_SPEED_40, DRIVE_THRU ); // Go fast to the edge of the observation zone - pos_y = 14.0; + pos_y = 15.0; pos_x -= 3.0; // end 5" away from the wall driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_80, TURN_SPEED_40, DRIVE_THRU ); // ease into the observation zone (in case we hit the wall, or another robot) - timeDriveStraight(-DRIVE_SPEED_20,1500); + timeDriveStraight(-DRIVE_SPEED_20,1000); } // opModeIsActive // If we did any herding, turn off the motors if (samplesToHerd > 0) { 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 7628ce9762a4..73bec6f1023c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware2025Bot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware2025Bot.java @@ -101,32 +101,37 @@ public class Hardware2025Bot protected AnalogInput armTiltEncoder = null; // US Digital absolute magnetic encoder (MA3) public double armTiltAngle = 0.0; // 0V = 0 degrees; 3.3V = 359.99 degrees - public double armTiltAngleOffset = 129.0; // allows us to adjust the 0-360 deg range - public double turretAngleTarget = 0.0; // Automatic movement target angle (degrees) + public static final double armTiltAngleOffset = 129.0; // allows us to adjust the 0-360 deg range + public double turretAngleTarget = 0.0; // Automatic movement target angle (degrees) public int TILT_ANGLE_HW_MAX = 3675; // encoder at maximum rotation UP/BACK (horizontal = -200) - public int TILT_ANGLE_BASKET = 3675; // encoder at rotation back to the basket for scoring - public int TILT_ANGLE_RAISED = 2000; // encoder at rotation back to the basket for scoring - public int TILT_ANGLE_HANG1 = 1400; // encoder when preparing for level 2 ascent - public int TILT_ANGLE_HANG2 = 400; // encoder at the end of level 2 ascent - public int TILT_ANGLE_ZERO = 0; // encoder for parking fullyh reset in auto - public int TILT_ANGLE_DRIVE = 200; // encoder for parking in auto or driving around - public int TILT_ANGLE_AUTO1 = 2005; // tilted up for autonomous specimen scoring (above bar) - public int TILT_ANGLE_AUTO2 = 1780; // tilted up for autonomous specimen scoring (clipped) - public int TILT_ANGLE_HW_MIN = -2000; // encoder at maximum rotation DOWN/FWD - - public static double startingTurretAngle = 97.0; - public final static double ENCODER_COUNTS_PER_DEG = 5675.0 / 90.9; - public final static double TILT_ANGLE_HW_MAX_DEG = 38.1; // encoder at maximum rotation UP/BACK (horizontal = -200) - public final static double TILT_ANGLE_BASKET_DEG = 38.1; // encoder at rotation back to the basket for scoring - public final static double TILT_ANGLE_RAISED_DEG = 64.9; // encoder at rotation back to the basket for scoring - public final static double TILT_ANGLE_HANG1_DEG = 74.5; // encoder when preparing for level 2 ascent - public final static double TILT_ANGLE_HANG2_DEG = 90.6; // encoder at the end of level 2 ascent - public final static double TILT_ANGLE_ZERO_DEG = 97.0; // encoder for parking fullyh reset in auto - public final static double TILT_ANGLE_DRIVE_DEG = 93.8; // encoder for parking in auto or driving around - public final static double TILT_ANGLE_AUTO1_DEG = 64.9; // tilted up for autonomous specimen scoring (above bar) - public final static double TILT_ANGLE_AUTO2_DEG = 68.5; // tilted up for autonomous specimen scoring (clipped) - public final static double TILT_ANGLE_HW_MIN_DEG = 129.0; // encoder at maximum rotation DOWN/FWD + public int TILT_ANGLE_BASKET = 3675; // 93.8 deg at 3582 encoder at rotation back to the basket for scoring + public int TILT_ANGLE_RAISED = 2000; // 54.5 deg encoder at rotation back to the basket for scoring + public int TILT_ANGLE_HANG1 = 1400; // 40.1 deg encoder when preparing for level 2 ascent + public int TILT_ANGLE_HANG2 = 400; // 16.4 deg encoder at the end of level 2 ascent + public int TILT_ANGLE_ZERO = 0; // 7 deg encoder for parking fullyh reset in auto + public int TILT_ANGLE_DRIVE = 200; // 11.8 deg encoder for parking in auto or driving around + public int TILT_ANGLE_AUTO1 = 2005; // 54.8 deg tilted up for autonomous specimen scoring (above bar) + public int TILT_ANGLE_AUTO2 = 1780; // 49.6 tilted up for autonomous specimen scoring (clipped) + public int TILT_ANGLE_HW_MIN = -2000; // does not exist encoder at maximum rotation DOWN/FWD + + // This value is set at init. + public static double startingArmTiltAngle = 0.0; + // Delta math from -0.1 deg -3891 encoder counts + // 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_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 //====== Viper slide MOTOR (RUN_USING_ENCODER) ===== protected DcMotorEx viperMotor = null; @@ -269,7 +274,7 @@ public void init(HardwareMap ahwMap, boolean isAutonomous ) { wormPanMotor = hwMap.get(DcMotorEx.class,"WormPan"); // Control Hub port 0 wormTiltMotor = hwMap.get(DcMotorEx.class,"WormTilt"); // Control Hub port 1 armTiltEncoder = hwMap.get(AnalogInput.class, "tiltMA3"); // Expansion Hub analog 0 - startingTurretAngle = computeAbsoluteAngle( armTiltEncoder.getVoltage(), armTiltAngleOffset); + startingArmTiltAngle = computeAbsoluteAngle( armTiltEncoder.getVoltage(), armTiltAngleOffset); wormPanMotor.setDirection(DcMotor.Direction.FORWARD); wormTiltMotor.setDirection(DcMotor.Direction.FORWARD); @@ -394,6 +399,19 @@ public double computeAbsoluteAngle( double measuredVoltage, double zeroAngleOffs return correctedAngle; } // computeAbsoluteAngle + public double computeRawAngle( double measuredVoltage ) + { + final double DEGREES_PER_ROTATION = 360.0; // One full rotation measures 360 degrees + final double MAX_MA3_ANALOG_VOLTAGE = 3.3; // 3.3V maximum analog output + // NOTE: when vertical the angle is 38.1deg, when horizontal 129.0 (prior to offset below) + double measuredAngle = (measuredVoltage / MAX_MA3_ANALOG_VOLTAGE) * DEGREES_PER_ROTATION; + // Correct for the offset angle (see note above) + // Enforce that any wrap-around remains in the range of -180 to +180 degrees + while( measuredAngle < -180.0 ) measuredAngle += 360.0; + while( measuredAngle > +180.0 ) measuredAngle -= 360.0; + return measuredAngle; + } // computeAbsoluteAngle + /*--------------------------------------------------------------------------------------------*/ /* NOTE: The absolute magnetic encoders may not be installed with 0deg rotated to the "right" */ /* rotational angle to put ZERO DEGREES where we want it. By defining a starting offset, and */ @@ -402,7 +420,7 @@ public double computeAbsoluteAngle( double measuredVoltage, double zeroAngleOffs /*--------------------------------------------------------------------------------------------*/ public static int computeEncoderCountsFromAngle( double angle ) { - return (int)((startingTurretAngle - angle) * ENCODER_COUNTS_PER_DEG); + return (int)((angle - startingArmTiltAngle) * ENCODER_COUNTS_PER_DEG); } // computeEncoderCountsFromAngle /*--------------------------------------------------------------------------------------------*/ 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 8c2f5b4be578..34ec45046fde 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java @@ -238,7 +238,7 @@ public void runOpMode() throws InterruptedException { // 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 %.1f deg", robot.wormTiltMotorPos, robot.armTiltAngle); + telemetry.addData("Tilt", "%d counts %.1f deg %.1f raw deg", robot.wormTiltMotorPos, robot.armTiltAngle, robot.computeRawAngle(robot.armTiltEncoder.getVoltage())); 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() ); @@ -593,8 +593,8 @@ else if(panAngleTweaked) { void processTiltControls() { // The encoder is backwards from our definition of MAX and MIN. Maybe change the // convention in hardware class? - boolean safeToManuallyLower = (robot.armTiltAngle < robot.TILT_ANGLE_HW_MIN_DEG); - boolean safeToManuallyRaise = (robot.armTiltAngle > robot.TILT_ANGLE_HW_MAX_DEG); + boolean safeToManuallyLower = (robot.armTiltAngle > Hardware2025Bot.TILT_ANGLE_HW_MIN_DEG); + boolean safeToManuallyRaise = (robot.armTiltAngle < Hardware2025Bot.TILT_ANGLE_HW_MAX_DEG); double gamepad2_right_stick = gamepad2.right_stick_y; boolean manual_tilt_control = ( Math.abs(gamepad2_right_stick) > 0.08 ); @@ -664,7 +664,7 @@ else if( gamepad2_dpad_right_now && !gamepad2_dpad_right_last) // 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; + 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) { @@ -824,7 +824,7 @@ void processLevel2Ascent() { robot.viperMotor.setPower( -0.10 ); // hold power } // Do we need to further store the robot drivetrain? - boolean tiltRetractionIncomplete = (robot.armTiltAngle > robot.TILT_ANGLE_HANG2_DEG)? true : false; + boolean tiltRetractionIncomplete = (robot.armTiltAngle < Hardware2025Bot.TILT_ANGLE_HANG2_DEG)? true : false; boolean tiltMotorLoadOkay = (robot.wormTiltMotorAmps < 8.5)? true : false; if( viperRetractionIncomplete && viperMotorLoadOkay ) { robot.wormTiltMotor.setPower( 0.30 ); // test at lower power!!!