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

arm encoders, and states made, not tested

Browse Source
This commit is contained in:
robotics2
2023-10-17 17:10:53 -07:00
parent eb81e1b4b6
commit 18418825d2
1 changed files with 74 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

75
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FullRobotControl.java
View File

@ -23,7 +23,12 @@ public class FullRobotControl extends OpMode {
DcMotor armMotor;
Servo gripper;
Servo wrist;
DcMotor arm;
public ElapsedTime runtime = new ElapsedTime();
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);
/**
@ -64,6 +69,7 @@ public class FullRobotControl extends OpMode {
gripper = hardwareMap.servo.get("gripper");
gripper.setPosition(1);
wrist = hardwareMap.servo.get("wrist");
arm = hardwareMap.dcMotor.get("arm raise");
wrist.setPosition(1);
}
@ -165,6 +171,43 @@ public class FullRobotControl extends OpMode {
gripper.setPosition(1);
}
int armState = 0;
if(gamepad2.x)
{
armState = 1;
raisearm(0);
}
if(gamepad2.y)
{
armState = 2;
raisearm(30);
}
if(gamepad2.y)
{
armState = 2;
raisearm(200);
}
if(gamepad2.right_bumper)
{
armState =+ 1;
}
if(gamepad2.left_bumper)
{
armState =- 1;
}
if(armState == 1)
{
raisearm(0);
}
if(armState == 2)
{
raisearm(30);
}
if(armState == 1)
{
raisearm(200);
}
@ -182,4 +225,34 @@ public class FullRobotControl extends OpMode {
telemetry.update();
}
}
public void raisearm(int degrees)
{
armEncoder(.2, degrees*TICKS_TO_DEGREES);
}
public void armEncoder(double speed,
double Inches) {
int newarmTarget;
// Determine new target position, and pass to motor controller
newarmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
arm.setTargetPosition(newarmTarget);
// Turn On RUN_TO_POSITION
arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// reset the timeout time and start motion.
runtime.reset();
arm.setPower(Math.abs(speed));
arm.setPower(0);
// Turn off RUN_TO_POSITION
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
}