Skip to content
This repository has been archived by the owner on Jan 19, 2025. It is now read-only.

Commit

Permalink
Fix pivot
Browse files Browse the repository at this point in the history
  • Loading branch information
@kevinthegreat1
kevinthegreat1 committed Jan 15, 2024
1 parent bcf5612 commit c167cde
Show file tree
Hide file tree
Showing 4 changed files with 43 additions and 31 deletions.
24 changes: 13 additions & 11 deletions TeleOp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.firstinspires.ftc.teamcode.modules.DriveTrain;
import org.firstinspires.ftc.teamcode.modules.output.LinearSlide;
import org.firstinspires.ftc.teamcode.modules.output.MotorOutputPivot;
Expand Down Expand Up @@ -51,7 +50,7 @@ public void runOpMode() {
driveTrain = new DriveTrain(this);
intakeWheel = hardwareMap.get(DcMotor.class, "intakeWheel");
outputSlide = new LinearSlide("linearSlide", 0.5);
pivot = new MotorOutputPivot("outputClaw", 0.5);
pivot = new MotorOutputPivot("outputPivot", 0.5);
driveTrain.init(hardwareMap);
outputSlide.init(hardwareMap);
pivot.init(hardwareMap);
Expand Down Expand Up @@ -86,7 +85,9 @@ public void runOpMode() {
TelemetryWrapper.setLine(6, "RightSlideTargetPos" + outputSlide.getTargetPosition()[1]);
TelemetryWrapper.setLine(7, "LeftSlideButton" + outputSlide.isElevatorBtnPressed()[0]);
TelemetryWrapper.setLine(8, "RightSlideButton" + outputSlide.isElevatorBtnPressed()[1]);
TelemetryWrapper.setLine(9, "PivotButton" + pivot.intakeButton.isPressed());
TelemetryWrapper.setLine(9, "PivotPos" + pivot.getCurrentPosition());
TelemetryWrapper.setLine(10, "PivotTargetPos" + pivot.getTargetPosition());
TelemetryWrapper.setLine(11, "PivotButton" + pivot.isPressed());
// Update ButtonHelper
gp1.update();
gp2.update();
Expand All @@ -96,23 +97,24 @@ public void runOpMode() {

intakeWheel.setPower((gamepad2.right_trigger - gamepad2.left_trigger) * 0.8);

outputSlide.startMoveToRelativePos((int) -gamepad2.left_stick_y * 1000);
if (Math.abs(gamepad2.left_stick_y) > 0.001) {
outputSlide.startMoveToRelativePos((int) -gamepad2.left_stick_y * 1000);
}
if (gp2.pressing(ButtonHelper.b)) {
outputSlide.startMoveToPos(1430);
}
if (gp2.pressing(ButtonHelper.a)) {
pivot.startMoveToPos(false);
outputSlide.startRetraction();
}
outputSlide.tick();

if (gp1.pressed(ButtonHelper.right_bumper)) {
pivot.startMoveToRelativePos(1000);
} else if (gp1.pressed(ButtonHelper.left_bumper) && !pivot.isPressed()) {
pivot.startMoveToRelativePos(-1000);
} else if (pivot.isBusy()) {
pivot.startMoveToRelativePos(0);
if (gamepad1.right_trigger > 0.001 && !pivot.isPressed()) {
pivot.startMoveToRelativePos((int) -gamepad1.right_trigger * 1000);
} else if (gamepad1.left_trigger > 0.001) {
pivot.startMoveToRelativePos((int) gamepad1.left_trigger * 1000);
}
if (gp2.pressed(ButtonHelper.dpad_up)) {
if (gp2.pressing(ButtonHelper.dpad_up)) {
pivot.startMoveToPosToggle();
}
pivot.tick();
Expand Down
38 changes: 24 additions & 14 deletions modules/output/MotorOutputPivot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,36 +2,42 @@

import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;

import com.qualcomm.robotcore.hardware.TouchSensor;
import org.firstinspires.ftc.teamcode.modules.Modulable;
import org.firstinspires.ftc.teamcode.modules.Tickable;

public class MotorOutputPivot implements Modulable, Tickable {
public static final int OUTPUT_POS = 0;
private static final int RELOAD_POS = 0;
/**
* The position to start moving towards when moving to intake.
* -1000 because this ensures the pivot moves all the way back and activates the button,
* no matter where the pivot was initialized.
* The encoder is then reset to 0 when the button is pressed and {@link #tick()} is called.
*/
private static final int INTAKE_TARGET_POS = -1000;
private static final int INTAKE_POS = -5;
public static final int OUTPUT_POS = 480;
private final String name;
private final double power;
protected DcMotor pivot;
public TouchSensor intakeButton;
private DcMotor pivot;
private TouchSensor intakeButton;
private boolean atOutput;

public MotorOutputPivot(String name, double power) {
this.name = name;
this.power = power;
}

public void moveUsingEncoder(double power){
public void moveUsingEncoder(double power) {
pivot.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
pivot.setPower(power);
}

public int getPosition() {
public int getCurrentPosition() {
return pivot.getCurrentPosition();
}

public void setPosition(int position) {
pivot.setTargetPosition(position);
public int getTargetPosition() {
return pivot.getTargetPosition();
}

public boolean isBusy() {
Expand All @@ -52,7 +58,7 @@ public void init(HardwareMap map) {
intakeButton = map.get(TouchSensor.class, "intakeButton");
}

public void startMovePivot(){
public void startMovePivot() {
pivot.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}

Expand All @@ -62,15 +68,16 @@ public void startMovePivot(){
* @param atOutput start move towards the intake or output position of the pivot
*/
public void startMoveToPos(boolean atOutput) {
startMoveToPos(atOutput ? OUTPUT_POS : RELOAD_POS);
this.atOutput = atOutput;
startMoveToPos(atOutput ? OUTPUT_POS : INTAKE_TARGET_POS);
}

public void startMoveToPosToggle() {
startMoveToPos(atOutput = !atOutput);
}

public void startMoveToRelativePos(int relativePosition) {
startMoveToPos(Math.max(pivot.getCurrentPosition() + relativePosition, 10));
startMoveToPos(Math.max(pivot.getCurrentPosition() + relativePosition, INTAKE_TARGET_POS));
}

public void startMoveToPos(int position) {
Expand All @@ -81,11 +88,14 @@ public void startMoveToPos(int position) {

@Override
public void tick() {
if (pivot.isBusy() && intakeButton.isPressed()) {
if (pivot.getTargetPosition() <= INTAKE_POS && !intakeButton.isPressed()) {
startMoveToPos(false);
}
if (pivot.isBusy() && pivot.getTargetPosition() < INTAKE_POS && intakeButton.isPressed()) {
int targetPos = pivot.getTargetPosition();
double power = pivot.getPower();
pivot.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
pivot.setTargetPosition(Math.max(targetPos, 0));
pivot.setTargetPosition(Math.max(targetPos, INTAKE_POS));
pivot.setMode(DcMotor.RunMode.RUN_TO_POSITION);
pivot.setPower(power);
}
Expand Down
2 changes: 1 addition & 1 deletion state/ButtonState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,6 @@
*/
public class ButtonState extends State {
public ButtonState(String name, ButtonHelper gamepad, int button) {
super(name, () -> gamepad.pressed(button));
super(name, () -> gamepad.held(button));
}
}
10 changes: 5 additions & 5 deletions util/ButtonHelper.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ public class ButtonHelper {
private final Gamepad gamepad;

private final boolean[] buttons = new boolean[15];
private final boolean[] held = new boolean[15];
private final boolean[] prev = new boolean[15];

/**
* Buttons for use in the pressed(), released(), and pressing() methods
Expand Down Expand Up @@ -64,15 +64,15 @@ public void update() {
buttons[left_stick_button] = gamepad.left_stick_button;
buttons[right_stick_button] = gamepad.right_stick_button;
for (int i = 0; i < 15; i++) {
held[i] = buttons[i] && buttons2[i];
prev[i] = buttons[i] && buttons2[i];
}
}

/**
* Return whether button is pressed on gamepad
* @param idx Gamepad button value
*/
public boolean pressed(int idx) {
public boolean held(int idx) {
return buttons[idx];
}

Expand All @@ -81,7 +81,7 @@ public boolean pressed(int idx) {
* @param idx Gamepad button value
*/
public boolean released(int idx) {
return !pressed(idx);
return !held(idx);
}

/**
Expand All @@ -91,6 +91,6 @@ public boolean released(int idx) {
* @param idx Gamepad button value
*/
public boolean pressing(int idx) {
return !held[idx] && pressed(idx);
return !prev[idx] && held(idx);
}
}

0 comments on commit c167cde

Please sign in to comment.