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