eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Pickup macro, teleop cleaned up a lil bit

This commit is contained in:
sihan 2024-02-28 12:56:22 -06:00
parent 3efb8adf21
commit 0ef239ca91
2 changed files with 65 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -1,5 +1,6 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad2;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCORE;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMPICKUPSTACK;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMREST;
@ -34,10 +35,11 @@ import static org.firstinspires.ftc.teamcode.util.Constants.SLIDERIGHT;
import static org.firstinspires.ftc.teamcode.util.Constants.WRIST;
import com.acmerobotics.dashboard.config.Config;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
@ -67,6 +69,8 @@ public class Robot {
@Getter
private Slides slides;
GamepadEx controller2 = new GamepadEx(gamepad2);
public Robot init(HardwareMap hardwareMap) {
this.drive = new MecanumDrive(hardwareMap);
this.hang = new Hang().init(hardwareMap);
@ -237,29 +241,33 @@ public class Robot {
public void wristScore() {
this.wrist.setPosition(WRISTSCORE);
}
}
public static class Claw {
private static final double CLAW_KP = 0.15;
public static boolean clawIsOpen;
public static class Claw {
private Servo claw;
public Claw init(HardwareMap hardwareMap) {
this.claw = hardwareMap.get(Servo.class, CLAW);
this.claw.setPosition(CLOSE);
close();
return this;
}
public void close() {
this.claw.setPosition(CLOSE);
clawIsOpen = false;
}
public void open() {
this.claw.setPosition(OPEN);
clawIsOpen = true;
}
public void openStack() {
this.claw.setPosition(BIGOPEN);
clawIsOpen = true;
}
public void setPos(double pos) {
@ -276,6 +284,14 @@ public class Robot {
return this;
}
public void LED() {
if (clawIsOpen) {
white();
} else {
gold();
}
}
public void gold() {
this.led.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE_VIOLET);
}
@ -304,25 +320,45 @@ public class Robot {
}
public void pickupMacro(Gamepad gamepad, Runtime runtime) {
public void pickupMacro(GamepadEx gamepadEx, double runtime) {
switch (pickupMacroState) {
case IDLE:
if (gamepad.dpad_down)
if (controller2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)){
pickupMacroState = pickupMacroStates.OPEN;
}
break;
case OPEN:
delay = runtime + .2;
this.getClaw().open();
pickupMacroState = pickupMacroStates.DROP;
break;
case DROP:
if (runtime > delay) {
this.getArm().pickup();
delay= runtime + .2;
pickupMacroState = pickupMacroStates.CLOSE;
}
break;
case CLOSE:
if (runtime > delay) {
this.getClaw().close();
delay= runtime + .1;
pickupMacroState = pickupMacroStates.NEUTRAL;
}
break;
case NEUTRAL:
if (runtime > delay) {
this.getArm().armRest();
pickupMacroState = pickupMacroStates.IDLE;
}
break;
}
}
pickupMacroStates pickupMacroState = pickupMacroStates.IDLE;
enum pickupMacroStates{
public pickupMacroStates pickupMacroState = pickupMacroStates.IDLE;
public enum pickupMacroStates{
IDLE,
OPEN,
DROP,
@ -331,9 +367,7 @@ public class Robot {
}
double runtime;
double delay;
public TrajectorySequenceBuilder getTrajectorySequenceBuilder() {
this.drive.update();

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java
View File

@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
@ -15,16 +17,11 @@ public class MainTeleOp extends OpMode {
this.robot = new Robot().init(hardwareMap);
}
@Override
GamepadEx controller2 = new GamepadEx(gamepad2);
public void loop() {
boolean slideUp = gamepad2.dpad_up;
boolean restArm = gamepad2.x;
boolean pickupArm = gamepad2.dpad_down;
boolean scoreArm = gamepad2.a;
boolean plane = gamepad2.right_bumper;
boolean claw = gamepad2.b;
boolean pickupWrist = gamepad2.x;
boolean scoreWrist = gamepad2.a;
boolean slideDown = gamepad2.dpad_left;
boolean hang = gamepad2.left_bumper;
boolean hangRelease = gamepad2.y;
@ -39,27 +36,23 @@ public class MainTeleOp extends OpMode {
} else {
this.robot.getSlides().slideStop();
}
//Arm
if (pickupArm) {
this.robot.getArm().pickup();
} else if (restArm) {
this.robot.getArm().armRest();
} else if (scoreArm) {
//Macros
this.robot.getLed().LED();
this.robot.pickupMacro(controller2,getRuntime());
//Arm and Wrist
if (controller2.wasJustPressed(GamepadKeys.Button.X)){
this.robot.getArm().armSecondaryScore();
this.robot.getWrist().wristScore();
} else if (controller2.wasJustPressed(GamepadKeys.Button.A)){
this.robot.getArm().armRest();
this.robot.getWrist().wristPickup();
this.robot.getClaw().close();
}
//Claw
if (claw) {
if (controller2.wasJustPressed(GamepadKeys.Button.B)){
this.robot.getClaw().open();
this.robot.getLed().white();
} else {
this.robot.getClaw().close();
this.robot.getLed().gold();
}
//Wrist
if (pickupWrist) {
this.robot.getWrist().wristPickup();
} else if (scoreWrist) {
this.robot.getWrist().wristScore();
}
//Hang
if (hang) {
@ -68,17 +61,18 @@ public class MainTeleOp extends OpMode {
this.robot.getHang().hangRelease();
} else if (hangPlane) {
this.robot.getHang().hangPlane();
} else {
this.robot.getHang().hangIdle();
}
int Position = this.robot.getHang().hangRight.getCurrentPosition();
telemetry.addData("position",(Position));
telemetry.update();
//Plane
if (plane) {
this.robot.getPlane().planeLaunch();
}else {
this.robot.getPlane().planeLock();
}
}
}