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

arm state machine controls, needs large motor adjustments (see comment in code)

Browse Source
This commit is contained in:
Xander Haemel
2023-10-29 20:55:39 -07:00
parent 8d880cd330
commit 9a4561b29c
1 changed files with 74 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -12,18 +12,19 @@ import com.qualcomm.robotcore.util.ElapsedTime;
@TeleOp( name = "ArmState") @TeleOp( name = "ArmState")
public class ArmStates extends OpMode { public class
ArmStates extends OpMode {
DcMotor arm; DcMotor arm;
DcMotor armThroughBoreEncoder;
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 = 1.0; static final double DRIVE_GEAR_REDUCTION = 75;
static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI); 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 * this function takes a long milliseconds parameter and sleeps
*
* @param millis milliseconds to sleep * @param millis milliseconds to sleep
*/ */
public void sleepmillis(long millis) { public void sleepmillis(long millis) {
@ -40,6 +41,7 @@ public class ArmStates extends OpMode {
arm.setPower(0); arm.setPower(0);
} }
/** /**
* User defined init method * User defined init method
* This method will be called once when the INIT button is pressed. * This method will be called once when the INIT button is pressed.
@ -48,10 +50,10 @@ public class ArmStates extends OpMode {
public void init() { public void init() {
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");
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 will be called repeatedly when the INIT button is pressed.
* This method is optional. By default this method takes no action. * 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) // Wait for the game to start (driver presses PLAY)
telemetry.addData("Status", "Initialized"); telemetry.addData("Status", "Initialized");
telemetry.update(); telemetry.update();
} }
/** /**
* User defined start method. * User defined start method.
* This method will be called once when the PLAY button is first pressed. * 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. * This method will be called when this op mode is first disabled.
* The stop method is optional. By default this method takes no action. * 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. * User defined loop method.
* This method will be called repeatedly in a loop while this op mode is running * This method will be called repeatedly in a loop while this op mode is running
*/ */
public int armState = 0;
public void loop() { public void loop() {
if (gamepad1.right_bumper) int currentArmState = armState;
{
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);
telemetry.addData("state", armState);
telemetry.addData("arm pos", arm.getCurrentPosition());
telemetry.update(); 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, public void armDriveWithEncoder(double speed,
double Inches) { int ticks) {
int newarmTarget;
// Determine new target position, and pass to motor controller // Determine new target position, and pass to motor controller
newarmTarget = armThroughBoreEncoder.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH); arm.setTargetPosition(ticks);
armThroughBoreEncoder.setTargetPosition(newarmTarget);
// Turn On RUN_TO_POSITION // Turn On RUN_TO_POSITION
arm.setMode(DcMotor.RunMode.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. // reset the timeout time and start motion.
arm.setPower(Math.abs(speed)); 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); arm.setPower(0);
// Turn off RUN_TO_POSITION // 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());
}
}
}