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 3d4f07ea62d4..bebb1e34a50b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousBase.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousBase.java @@ -364,8 +364,10 @@ else if( autoViperMotorTimer.milliseconds() > 3000 ) { } // autoViperMotorWaitToComplete /*---------------------------------------------------------------------------------*/ - void autoTiltMotorMoveToTarget(int targetEncoderCount ) + void autoTiltMotorMoveToTarget(double targetArmAngle ) { + // Convert angle to encoder counts + int targetEncoderCount = robot.computeEncoderCountsFromAngle(robot.armTiltAngle); // Configure target encoder count robot.wormTiltMotor.setTargetPosition( targetEncoderCount ); // Enable RUN_TO_POSITION mode @@ -480,7 +482,7 @@ protected double getError(double targetAngle) { */ protected double getAngleError(double targetAngle) { // calculate error in -179 to +180 range ( - double robotError = targetAngle - robot.headingAngle; + double robotError = targetAngle - robot.imuHeadingAngle; while (robotError > 180.0) robotError -= 360.0; while (robotError <= -180.0) robotError += 360.0; return robotError; 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 2fff7a6b3ffc..84dce395c941 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftBlue.java @@ -189,7 +189,7 @@ private void scoreSpecimenPreload() { // 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( robot.TILT_ANGLE_AUTO1); + 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); @@ -228,7 +228,7 @@ private void scoreSpecimenPreload() { // Rotate lift down to get specimen close to bar if( opModeIsActive() ) { robot.geckoServo.setPower(-0.50); // hold it while we clip - autoTiltMotorMoveToTarget( robot.TILT_ANGLE_AUTO2); + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO2_DEG); do { if( !opModeIsActive() ) break; // wait for lift/tilt to finish... @@ -273,7 +273,7 @@ private void scoreSpecimenPreload() { // Lower the arm for parking if( opModeIsActive() ) { - autoTiltMotorMoveToTarget( robot.TILT_ANGLE_ZERO); + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ZERO_DEG); do { if( !opModeIsActive() ) break; // wait for lift/tilt to finish... @@ -312,7 +312,7 @@ private void level1Ascent() { if( opModeIsActive() ) { autoViperMotorMoveToTarget( robot.VIPER_EXTEND_GRAB); - autoTiltMotorMoveToTarget( robot.TILT_ANGLE_BASKET); + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_BASKET_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 2ac0ebe71c5b..be7601630d68 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftRed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftRed.java @@ -189,7 +189,7 @@ private void scoreSpecimenPreload() { // 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( robot.TILT_ANGLE_AUTO1); + 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); @@ -228,7 +228,7 @@ private void scoreSpecimenPreload() { // Rotate lift down to get specimen close to bar if( opModeIsActive() ) { robot.geckoServo.setPower(-0.50); // hold it while we clip - autoTiltMotorMoveToTarget( robot.TILT_ANGLE_AUTO2); + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO2_DEG); do { if( !opModeIsActive() ) break; // wait for lift/tilt to finish... @@ -273,7 +273,7 @@ private void scoreSpecimenPreload() { // Lower the arm for parking if( opModeIsActive() ) { - autoTiltMotorMoveToTarget( robot.TILT_ANGLE_ZERO); + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ZERO_DEG); do { if( !opModeIsActive() ) break; // wait for lift/tilt to finish... @@ -312,7 +312,7 @@ private void level1Ascent() { if( opModeIsActive() ) { autoViperMotorMoveToTarget( robot.VIPER_EXTEND_GRAB); - autoTiltMotorMoveToTarget( robot.TILT_ANGLE_BASKET); + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_BASKET_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 b3e1e79feb69..0fe61a93d5fb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightBlue.java @@ -199,7 +199,7 @@ private void scoreSpecimenPreload() { // 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( robot.TILT_ANGLE_AUTO1); + 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); @@ -238,7 +238,7 @@ private void scoreSpecimenPreload() { // Rotate lift down to get specimen close to bar if( opModeIsActive() ) { robot.geckoServo.setPower(-0.50); // hold it while we clip - autoTiltMotorMoveToTarget( robot.TILT_ANGLE_AUTO2); + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO2_DEG); do { if( !opModeIsActive() ) break; // wait for lift/tilt to finish... @@ -283,7 +283,7 @@ private void scoreSpecimenPreload() { // Lower the arm for parking if( opModeIsActive() ) { - autoTiltMotorMoveToTarget( robot.TILT_ANGLE_ZERO); + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ZERO_DEG); do { if( !opModeIsActive() ) break; // wait for lift/tilt to finish... 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 37719b99d064..5f2bf4bfd64f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightRed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightRed.java @@ -199,7 +199,7 @@ private void scoreSpecimenPreload() { // 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( robot.TILT_ANGLE_AUTO1); + 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); @@ -238,7 +238,7 @@ private void scoreSpecimenPreload() { // Rotate lift down to get specimen close to bar if( opModeIsActive() ) { robot.geckoServo.setPower(-0.50); // hold it while we clip - autoTiltMotorMoveToTarget( robot.TILT_ANGLE_AUTO2); + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO2_DEG); do { if( !opModeIsActive() ) break; // wait for lift/tilt to finish... @@ -283,7 +283,7 @@ private void scoreSpecimenPreload() { // Lower the arm for parking if( opModeIsActive() ) { - autoTiltMotorMoveToTarget( robot.TILT_ANGLE_ZERO); + autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ZERO_DEG); do { if( !opModeIsActive() ) break; // wait for lift/tilt to finish... 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 4129a0aa35e2..7628ce9762a4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware2025Bot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware2025Bot.java @@ -33,8 +33,8 @@ public class Hardware2025Bot //====== INERTIAL MEASUREMENT UNIT (IMU) ===== protected BNO055IMU imu = null; - public double headingAngle = 0.0; - public double tiltAngle = 0.0; + public double imuHeadingAngle = 0.0; + public double imuTiltAngle = 0.0; //====== GOBILDA PINPOINT ODOMETRY COMPUTER ====== GoBildaPinpointDriver odom; @@ -99,9 +99,9 @@ public class Hardware2025Bot public double wormTiltMotorSetPwr = 0.0; // requested power setting public double wormTiltMotorPwr = 0.0; // current power setting - protected AnalogInput turretEncoder = null; // US Digital absolute magnetic encoder (MA3) - public double turretAngle = 0.0; // 0V = 0 degrees; 3.3V = 359.99 degrees - public double turretAngleOffset = 129.0; // allows us to adjust the 0-360 deg range + 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 int TILT_ANGLE_HW_MAX = 3675; // encoder at maximum rotation UP/BACK (horizontal = -200) @@ -115,7 +115,7 @@ public class Hardware2025Bot 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 double startingTurretAngle = 97.0; + 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 @@ -268,8 +268,8 @@ public void init(HardwareMap ahwMap, boolean isAutonomous ) { // Define and Initialize pan and tilt motors wormPanMotor = hwMap.get(DcMotorEx.class,"WormPan"); // Control Hub port 0 wormTiltMotor = hwMap.get(DcMotorEx.class,"WormTilt"); // Control Hub port 1 - turretEncoder = hwMap.get(AnalogInput.class, "tiltMA3"); // Expansion Hub analog 0 - startingTurretAngle = computeAbsoluteAngle( turretEncoder.getVoltage(), turretAngleOffset ); + armTiltEncoder = hwMap.get(AnalogInput.class, "tiltMA3"); // Expansion Hub analog 0 + startingTurretAngle = computeAbsoluteAngle( armTiltEncoder.getVoltage(), armTiltAngleOffset); wormPanMotor.setDirection(DcMotor.Direction.FORWARD); wormTiltMotor.setDirection(DcMotor.Direction.FORWARD); @@ -351,9 +351,9 @@ public void initIMU() public double headingIMU() { Orientation angles = imu.getAngularOrientation( AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES ); - headingAngle = angles.firstAngle; - tiltAngle = angles.secondAngle; - return -headingAngle; // degrees (+90 is CW; -90 is CCW) + imuHeadingAngle = angles.firstAngle; + imuTiltAngle = angles.secondAngle; + return -imuHeadingAngle; // degrees (+90 is CW; -90 is CCW) } // headingIMU /*--------------------------------------------------------------------------------------------*/ @@ -400,7 +400,7 @@ public double computeAbsoluteAngle( double measuredVoltage, double zeroAngleOffs /* using this function to account for that offset, we can place zero where we want it in s/w. */ /* Having DEGREES_PER_ROTATION as a variable lets us adjust for the 3.3V vs. 5.0V difference. */ /*--------------------------------------------------------------------------------------------*/ - public int computeEncoderCountsFromAngle( double angle ) + public static int computeEncoderCountsFromAngle( double angle ) { return (int)((startingTurretAngle - angle) * ENCODER_COUNTS_PER_DEG); } // computeEncoderCountsFromAngle @@ -433,7 +433,7 @@ public void readBulkData() { wormTiltMotorPos = wormTiltMotor.getCurrentPosition(); wormTiltMotorVel = wormTiltMotor.getVelocity(); wormTiltMotorPwr = wormTiltMotor.getPower(); - turretAngle = computeAbsoluteAngle( turretEncoder.getVoltage(), turretAngleOffset ); + armTiltAngle = computeAbsoluteAngle( armTiltEncoder.getVoltage(), armTiltAngleOffset); // NOTE: motor mA data is NOT part of the bulk-read, so increases cycle time! // frontLeftMotorAmps = frontLeftMotor.getCurrent(MILLIAMPS); // frontRightMotorAmps = frontRightMotor.getCurrent(MILLIAMPS); 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 b6abbcbbe450..8c2f5b4be578 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.turretAngle); + telemetry.addData("Tilt", "%d counts %.1f deg", robot.wormTiltMotorPos, robot.armTiltAngle); 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() ); @@ -591,8 +591,10 @@ else if(panAngleTweaked) { /*---------------------------------------------------------------------------------*/ void processTiltControls() { - boolean safeToManuallyLower = (robot.wormTiltMotorPos > robot.TILT_ANGLE_HW_MIN); - boolean safeToManuallyRaise = (robot.wormTiltMotorPos < robot.TILT_ANGLE_HW_MAX); + // 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); double gamepad2_right_stick = gamepad2.right_stick_y; boolean manual_tilt_control = ( Math.abs(gamepad2_right_stick) > 0.08 ); @@ -662,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.viperMotorPos > robot.TILT_ANGLE_RAISED)? 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) { @@ -784,7 +786,7 @@ void processLevel2Ascent() { break; case ASCENT_STATE_SETUP: // Send TILT motor to hang position - robot.wormTiltMotor.setTargetPosition( robot.TILT_ANGLE_HANG1 ); + robot.wormTiltMotor.setTargetPosition( robot.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 @@ -794,7 +796,7 @@ void processLevel2Ascent() { ascent2state = ASCENT_STATE_MOVING; break; case ASCENT_STATE_MOVING : - boolean tiltReady = (Math.abs(robot.wormTiltMotorPos - robot.TILT_ANGLE_HANG1) < 20)? true:false; + 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; break; case ASCENT_STATE_READY : @@ -822,7 +824,7 @@ void processLevel2Ascent() { robot.viperMotor.setPower( -0.10 ); // hold power } // Do we need to further store the robot drivetrain? - boolean tiltRetractionIncomplete = (robot.wormTiltMotorPos < robot.TILT_ANGLE_HANG2)? true : false; + boolean tiltRetractionIncomplete = (robot.armTiltAngle > robot.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!!!