From 9a4561b29c168efa55dc1e9bb8e71d65037efbe6 Mon Sep 17 00:00:00 2001 From: Xander Haemel Date: Sun, 29 Oct 2023 20:55:39 -0700 Subject: [PATCH] arm state machine controls, needs large motor adjustments (see comment in code) --- .../firstinspires/ftc/teamcode/ArmStates.java | 155 +++++++++--------- 1 file changed, 74 insertions(+), 81 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmStates.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmStates.java index 0e1166e..96f3123 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmStates.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmStates.java @@ -12,18 +12,19 @@ import com.qualcomm.robotcore.util.ElapsedTime; @TeleOp( name = "ArmState") -public class ArmStates extends OpMode { +public class +ArmStates extends OpMode { DcMotor arm; - DcMotor armThroughBoreEncoder; static final double TICKS_TO_DEGREES = 0.07462686567; static final double COUNTS_PER_MOTOR_REV = 537.6; - static final double DRIVE_GEAR_REDUCTION = 1.0; - static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI); + static final double DRIVE_GEAR_REDUCTION = 75; + static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.75 * Math.PI); /** * this function takes a long milliseconds parameter and sleeps + * * @param millis milliseconds to sleep */ public void sleepmillis(long millis) { @@ -40,6 +41,7 @@ public class ArmStates extends OpMode { arm.setPower(0); } + /** * User defined init method * This method will be called once when the INIT button is pressed. @@ -48,10 +50,10 @@ public class ArmStates extends OpMode { public void init() { - telemetry.addData("Status","In Init()"); + telemetry.addData("Status", "In Init()"); telemetry.update(); arm = hardwareMap.dcMotor.get("arm raise"); - armThroughBoreEncoder = hardwareMap.dcMotor.get("armThroughBoreEncoder"); + arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); } @@ -60,11 +62,12 @@ public class ArmStates extends OpMode { * This method will be called repeatedly when the INIT button is pressed. * This method is optional. By default this method takes no action. */ - public void init_loop(){ + public void init_loop() { // Wait for the game to start (driver presses PLAY) telemetry.addData("Status", "Initialized"); telemetry.update(); } + /** * User defined start method. * This method will be called once when the PLAY button is first pressed. @@ -79,7 +82,7 @@ public class ArmStates extends OpMode { * This method will be called when this op mode is first disabled. * The stop method is optional. By default this method takes no action. */ - public void stop(){ + public void stop() { } @@ -88,85 +91,41 @@ public class ArmStates extends OpMode { * User defined loop method. * This method will be called repeatedly in a loop while this op mode is running */ + public int armState = 0; + public void loop() { -if (gamepad1.right_bumper) -{ - raisearm(200); -} - - -// int armState = 0; -// int armPos = 0; -// if(gamepad2.x) -// { -// armState = 1; -// raisearm(0 - armPos); -// armPos = 0; -// } -// if(gamepad2.y) -// { -// armState = 2; -// raisearm(1000); -// armPos = 30; -// } -// if(gamepad2.b) -// { -// armState = 2; -// raisearm(200 - armPos); -// armPos = 200; -// } -// if(gamepad2.right_bumper) -// { -// armState =+ 1; -// } -// if(gamepad2.left_bumper) -// { -// armState =- 1; -// } -// if(armState == 1) -// { -// raisearm(0 - armPos); -// armPos = 0; -// } -// if(armState == 2) -// { -// raisearm(30 - armPos); -// armPos = 30; -// } -// if(armState == 3) -// { -// raisearm(200 - armPos); -// armPos = 200; -// -// } - - - - - -// Show the elapsed game time and wheel power -// telemetry.addData("Status", "Run Time: " + runtime.toString()); -// telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); -// telemetry.addData("Back left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); -// telemetry.addData("Speed", speed); - + int currentArmState = armState; + telemetry.addData("state", armState); + telemetry.addData("arm pos", arm.getCurrentPosition()); telemetry.update(); + //Something is wrong, the arm is going the wrong way. Maybe it's just my robot, but you should look at it + if (gamepad2.x) { + armState = 1; + } + if (gamepad2.y) { + armState = 2; + } + if (gamepad2.b) { + armState = 3; + } + if (gamepad2.right_bumper) { + armState += 1; + } + if (gamepad2.left_bumper) { + armState -= 1; + } + if (armState != currentArmState) { + updateState(); + } } - public void raisearm(int degrees) - { - arm.setDirection(DcMotor.Direction.REVERSE); - armDriveWithEncoder(.2, degrees); - } + public void armDriveWithEncoder(double speed, - double Inches) { - int newarmTarget; - + int ticks) { // Determine new target position, and pass to motor controller - newarmTarget = armThroughBoreEncoder.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH); - armThroughBoreEncoder.setTargetPosition(newarmTarget); + arm.setTargetPosition(ticks); // Turn On RUN_TO_POSITION arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); @@ -174,13 +133,47 @@ if (gamepad1.right_bumper) // reset the timeout time and start motion. arm.setPower(Math.abs(speed)); + while //(opModeIsActive() && + //(runtime.seconds() < timeoutS) && + (arm.isBusy()) { + telemetry.addData("Running to", " %7d ", ticks); + telemetry.addData("Currently at", " %7d", arm.getCurrentPosition()); + telemetry.update(); + } arm.setPower(0); // Turn off RUN_TO_POSITION - armThroughBoreEncoder.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); } -} + + public void raisearm(int degrees) { + arm.setDirection(DcMotor.Direction.REVERSE); + armDriveWithEncoder(.1, degrees); + } + + public void updateState() { + if (armState == 0) { + // arm state 0 means on the ground with gripper down + raisearm(0); + telemetry.addData("state", 0); + } + if (armState == 1) { + // arm state 1 is for driving + raisearm(200); + telemetry.addData("state", 1); + } + if (armState == 2) { + raisearm(4400); + telemetry.addData("state", 2); + } + if (armState == 3) { + raisearm(4800); + telemetry.addData("state", 3); + telemetry.addData("arm pos", arm.getCurrentPosition()); + } + } +}