SCDS/FtcRobotController
7
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

manual control complete, and states started again

Browse Source
This commit is contained in:
robotics2
2023-10-31 17:10:06 -07:00
parent 9a4561b29c
commit 089ca0cf3d
1 changed files with 22 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmStates.java
View File

@ -16,9 +16,10 @@ public class
ArmStates extends OpMode { ArmStates extends OpMode {
DcMotor arm; DcMotor arm;
Servo wrist;
static final double TICKS_TO_DEGREES = 0.07462686567; static final double TICKS_TO_DEGREES = 0.07462686567;
static final double COUNTS_PER_MOTOR_REV = 537.6; static final double COUNTS_PER_MOTOR_REV = 537.6;
static final double DRIVE_GEAR_REDUCTION = 75; static final double DRIVE_GEAR_REDUCTION = 60;
static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.75 * Math.PI); static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.75 * Math.PI);
@ -53,6 +54,7 @@ ArmStates extends OpMode {
telemetry.addData("Status", "In Init()"); telemetry.addData("Status", "In Init()");
telemetry.update(); telemetry.update();
arm = hardwareMap.dcMotor.get("arm raise"); arm = hardwareMap.dcMotor.get("arm raise");
wrist = hardwareMap.servo.get("wrist");
arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
} }
@ -109,6 +111,10 @@ ArmStates extends OpMode {
if (gamepad2.b) { if (gamepad2.b) {
armState = 3; armState = 3;
} }
if (gamepad2.a)
{
armState = 0;
}
if (gamepad2.right_bumper) { if (gamepad2.right_bumper) {
armState += 1; armState += 1;
} }
@ -140,10 +146,9 @@ ArmStates extends OpMode {
telemetry.addData("Currently at", " %7d", arm.getCurrentPosition()); telemetry.addData("Currently at", " %7d", arm.getCurrentPosition());
telemetry.update(); telemetry.update();
} }
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
arm.setPower(0); arm.setPower(0);
// Turn off RUN_TO_POSITION // Turn off RUN_TO_POSITION
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
@ -152,28 +157,39 @@ ArmStates extends OpMode {
public void raisearm(int degrees) { public void raisearm(int degrees) {
arm.setDirection(DcMotor.Direction.REVERSE); arm.setDirection(DcMotor.Direction.REVERSE);
armDriveWithEncoder(.1, degrees); armDriveWithEncoder(.05, degrees);
} }
public void updateState() { public void updateState() {
if (armState == 0) { if (armState == 0) {
// arm state 0 means on the ground with gripper down // arm state 0 means on the ground with gripper down
wrist.setPosition(.5);
raisearm(0); raisearm(0);
telemetry.addData("state", 0); telemetry.addData("state", 0);
} }
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
arm.setPower(0);
if (armState == 1) { if (armState == 1) {
// arm state 1 is for driving // arm state 1 is for driving
raisearm(200); raisearm(200);
telemetry.addData("state", 1); telemetry.addData("state", 1);
} }
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
arm.setPower(0);
if (armState == 2) { if (armState == 2) {
raisearm(4400); raisearm(697);
wrist.setPosition(0);
raisearm(697);
telemetry.addData("state", 2); telemetry.addData("state", 2);
} }
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
arm.setPower(.0);
if (armState == 3) { if (armState == 3) {
raisearm(4800); raisearm(780);
telemetry.addData("state", 3); telemetry.addData("state", 3);
telemetry.addData("arm pos", arm.getCurrentPosition()); telemetry.addData("arm pos", arm.getCurrentPosition());
} }
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
arm.setPower(0);
} }
} }