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

Goog

This commit is contained in:
sihan 2024-03-08 06:49:00 -06:00
parent 8aea72a9cf
commit f6cdb245d6
13 changed files with 303 additions and 212 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,27 +1,5 @@
package org.firstinspires.ftc.teamcode.hardware;
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;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMSCORE;
import static org.firstinspires.ftc.teamcode.util.Configurables.BIGOPEN;
import static org.firstinspires.ftc.teamcode.util.Configurables.CLOSE;
import static org.firstinspires.ftc.teamcode.util.Configurables.HANG;
import static org.firstinspires.ftc.teamcode.util.Configurables.HANGPLANE;
import static org.firstinspires.ftc.teamcode.util.Configurables.HANGRELEASE;
import static org.firstinspires.ftc.teamcode.util.Configurables.OPEN;
import static org.firstinspires.ftc.teamcode.util.Configurables.PICKUP;
import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELAUNCH;
import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELOCK;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEAUTOSTACKS;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDELAYERONE;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDELAYERTWO;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEPICKUPSTACKSTWO;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEUP;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDE_POWER_DOWN;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDE_POWER_UP;
import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTPICKUP;
import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTSCORE;
import static org.firstinspires.ftc.teamcode.util.Constants.CLAW;
import static org.firstinspires.ftc.teamcode.util.Constants.HANGLEFT;
import static org.firstinspires.ftc.teamcode.util.Constants.HANGRIGHT;
@ -35,6 +13,7 @@ import static org.firstinspires.ftc.teamcode.util.Constants.WRIST;
import com.acmerobotics.dashboard.config.Config;
import com.arcrobotics.ftclib.controller.PController;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
@ -48,15 +27,21 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.Tra
import org.firstinspires.ftc.teamcode.vision.Camera;
import lombok.Getter;
@Config
public class Robot {
public static boolean clawIsOpen;
public static double WRISTDELAY = .3;
double delay;
public pickupMacroStates pickupMacroState = pickupMacroStates.IDLE;
public armMacroStates armMacroState = armMacroStates.IDLE;
@Getter
public Arm arm;
@Getter
private MecanumDrive drive;
@Getter
private Led led;
@Getter
private Arm arm;
@Getter
private Wrist wrist;
@Getter
private Claw claw;
@ -69,8 +54,6 @@ public class Robot {
@Getter
private Slides slides;
public Robot init(HardwareMap hardwareMap) {
this.drive = new MecanumDrive(hardwareMap);
this.hang = new Hang().init(hardwareMap);
@ -79,12 +62,100 @@ public class Robot {
this.claw = new Claw().init(hardwareMap);
// this.camera = new Camera(hardwareMap);
this.plane = new Plane().init(hardwareMap);
this.slides= new Slides().init(hardwareMap);
this.slides = new Slides().init(hardwareMap);
// this.led = new Led().init(hardwareMap);
return this;
}
public void pickupMacro(GamepadEx gamepadEx, double runtime) {
switch (pickupMacroState) {
case IDLE:
if (gamepadEx.wasJustReleased(GamepadKeys.Button.DPAD_DOWN)) {
pickupMacroState = pickupMacroStates.DROP;
}
break;
case OPEN:
delay = runtime + .3;
this.getClaw().open();
pickupMacroState = pickupMacroStates.DROP;
break;
case DROP:
if (runtime > delay) {
this.getArm().pickup(true);
delay = runtime + .2;
pickupMacroState = pickupMacroStates.CLOSE;
}
break;
case CLOSE:
if (runtime > delay) {
this.getClaw().close();
delay = runtime + .3;
pickupMacroState = pickupMacroStates.NEUTRAL;
}
break;
case NEUTRAL:
if (runtime > delay) {
this.getArm().armRest(true);
pickupMacroState = pickupMacroStates.IDLE;
}
break;
}
}
public void armMacro(GamepadEx gamepadEx, double runtime) {
switch (armMacroState) {
case IDLE:
if (gamepadEx.wasJustPressed(GamepadKeys.Button.X)) {
armMacroState = armMacroStates.ARMSWING;
}
break;
case ARMSWING:
arm.armSecondaryScore();
delay = runtime + WRISTDELAY;
armMacroState = armMacroStates.WRIST;
break;
case WRIST:
if (runtime > delay) {
wrist.wristScore();
armMacroState = armMacroStates.IDLE;
}
break;
}
}
public TrajectorySequenceBuilder getTrajectorySequenceBuilder() {
this.drive.update();
return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate());
}
public void update() {
this.arm.update();
this.wrist.update();
this.drive.update();
}
public enum pickupMacroStates {
IDLE, OPEN, DROP, CLOSE, NEUTRAL
}
public enum armMacroStates {
IDLE, ARMSWING, WRIST
}
@Config
public static class Slides {
//Values
public static double SLIDE_POWER_UP = .7;
public static double SLIDE_POWER_DOWN = .5;
public static int SLIDELAYERONE = 60;
public static int SLIDEAUTOSTACKS = 250;
public static int SLIDEUP = 1150;
public static int SLIDELAYERTWO = 350;
public static int SLIDESTACK = 80;
public static int SLIDEPICKUPSTACKSTWO = 30;
//Motors
public DcMotor slidesRight = null;
public DcMotor slidesLeft = null;
@ -116,17 +187,29 @@ public class Robot {
this.slidesRight.setPower(power);
}
public void slideUp(){this.slideTo(SLIDEUP, SLIDE_POWER_UP);}
public void slideUp() {
this.slideTo(SLIDEUP, SLIDE_POWER_UP);
}
public void slideDown(){this.slideTo(0, SLIDE_POWER_DOWN);}
public void slideDown() {
this.slideTo(0, SLIDE_POWER_DOWN);
}
public void slideFirstLayer(){this.slideTo(SLIDELAYERONE, SLIDE_POWER_UP);}
public void slideFirstLayer() {
this.slideTo(SLIDELAYERONE, SLIDE_POWER_UP);
}
public void slideScoreStack(){this.slideTo(SLIDELAYERTWO, SLIDE_POWER_UP);}
public void slideScoreStack() {
this.slideTo(SLIDELAYERTWO, SLIDE_POWER_UP);
}
public void slideAutoStacks(){this.slideTo(SLIDEAUTOSTACKS, SLIDE_POWER_UP);}
public void slideAutoStacks() {
this.slideTo(SLIDEAUTOSTACKS, SLIDE_POWER_UP);
}
public void slidePickupStackTwo(){this.slideTo(SLIDEPICKUPSTACKSTWO, SLIDE_POWER_UP);}
public void slidePickupStackTwo() {
this.slideTo(SLIDEPICKUPSTACKSTWO, SLIDE_POWER_UP);
}
public void slideStop() {
this.slidesLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
@ -137,11 +220,18 @@ public class Robot {
this.slidesRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.slidesRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.slidesRight.setTargetPosition(this.slidesRight.getCurrentPosition());
this.slidesRight.setPower(1);}
this.slidesRight.setPower(1);
}
}
@Config
public static class Hang {
//Values
public static int HANGRELEASE = 1550;
public static int HANG = 0;
public static int HANGPLANE = 1150;
//Motors
public DcMotor hangLeft = null;
public DcMotor hangRight = null;
@ -161,43 +251,48 @@ public class Robot {
return this;
}
public void hangTo(int hangPos, double daPower) {
public void hangTo(int hangPos, double Power) {
this.hangLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.hangLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.hangLeft.setTargetPosition(hangPos);
this.hangLeft.setPower(daPower);
this.hangLeft.setPower(Power);
this.hangRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.hangRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.hangRight.setTargetPosition(hangPos);
this.hangRight.setPower(daPower);
this.hangRight.setPower(Power);
}
public void hangRelease(){
this.hangTo(HANGRELEASE,1);
public void hangRelease() {
this.hangTo(HANGRELEASE, 1);
}
public void hang(){
this.hangTo(HANG,1);
}
public void hangPlane(){
this.hangTo(HANGPLANE,1);
public void hang() {
this.hangTo(HANG, 1);
}
public void hangIdle() {
this.hangLeft.setPower(0);
this.hangRight.setPower(0);
public void hangPlane() {
this.hangTo(HANGPLANE, 1);
}
}
@Config
public static class Arm {
//Values
public static double DIFFY_KP = 0.1;
public static double DIFFY_TOL = 0.01;
public static double ARM_MAX_DELTA = 0.02;
public static double ARMREST = 0.89;
public static double ARMSCORE = 0.4;
public static double ARMACCSCORE = .57;
public static double ARMPICKUPSTACK = 0.815;
public static double PICKUP = 0.92;
//PControler
public PController armPController;
private double armTarget;
//Servos
private Servo leftArm;
private Servo rightArm;
private PController armController;
public Arm init(HardwareMap hardwareMap) {
this.leftArm = hardwareMap.get(Servo.class, LEFTARM);
@ -205,62 +300,143 @@ public class Robot {
this.rightArm.setDirection(Servo.Direction.REVERSE);
this.rightArm.setPosition(ARMREST);
this.leftArm.setPosition(ARMREST);
this.armPController = new PController(DIFFY_KP);
this.armRest(true);
return this;
}
public void pickup() {
this.leftArm.setPosition(PICKUP);
this.rightArm.setPosition(PICKUP);
pickup(false);
}
public void pickup(boolean now) {
moveArm(PICKUP, now);
}
public void armScore() {
this.leftArm.setPosition(ARMSCORE);
this.rightArm.setPosition(ARMSCORE);
this.armSecondaryScore();
}
public void armSecondaryScore() {
this.leftArm.setPosition(ARMACCSCORE);
this.rightArm.setPosition(ARMACCSCORE);
moveArm(ARMACCSCORE, false);
}
public void armPickupStack() {
this.leftArm.setPosition(ARMPICKUPSTACK);
this.rightArm.setPosition(ARMPICKUPSTACK);
moveArm(ARMPICKUPSTACK, false);
}
public void armRest() {
this.leftArm.setPosition(ARMREST);
this.rightArm.setPosition(ARMREST);
armRest(false);
}
public void armRest(boolean now) {
moveArm(ARMREST, now);
}
private void moveArm(double position, boolean now) {
this.armTarget = position;
this.armPController.setSetPoint(this.armTarget);
if (now) {
this.leftArm.setPosition(position);
this.rightArm.setPosition(position);
}
}
public boolean isAtTarget() {
return this.armPController.atSetPoint();
}
public void update() {
this.armPController.setSetPoint(this.armTarget);
this.armPController.setTolerance(DIFFY_TOL);
this.armPController.setP(DIFFY_KP);
if (!isAtTarget()) {
double armDelta = this.armPController.calculate(leftArm.getPosition());
if (Math.abs(this.armPController.getPositionError()) > 0.1) {
armDelta = Math.copySign(ARM_MAX_DELTA, armDelta);
}
this.leftArm.setPosition(leftArm.getPosition() + armDelta);
this.rightArm.setPosition(rightArm.getPosition() + armDelta);
}
//
// this.rightPController.setSetPoint(this.rightTarget);
// this.rightPController.setTolerance(DIFFY_TOL);
// this.rightPController.setP(DIFFY_KP);
// double rightDelta = this.rightPController.calculate(rightArm.getPosition());
// rightDelta = Math.max(-1 * DIFFY_MAX_DELTA, Math.min(DIFFY_MAX_DELTA, rightDelta));
// this.rightArm.setPosition(rightArm.getPosition() + rightDelta);
// A is where I am
// B is where I want to get to
// E is the difference between them
// X is the midpoint between A and B
// while I am at on the left side of X, delta should be constant
// once I am on the right side of X, delta should be whatever ther P controller says
}
}
@Config
public static class Wrist {
//Pcontroller
public static double KP = 0.2;
public static double TOL = 0.005;
public static double MAX_DELTA = 0.04;
private PIDFController wristPController;
//Values
public static double WRISTPICKUP = 0.3;
public static double WRISTSCORE = .98;
//Servo
private Servo wrist;
public Wrist init(HardwareMap hardwareMap) {
this.wrist = hardwareMap.get(Servo.class, WRIST);
this.wrist.setPosition(WRISTPICKUP);
this.wristPController = new PController(KP);
this.wristPController.setSetPoint(WRISTPICKUP);
return this;
}
public void wristPickup() {
this.wrist.setPosition(WRISTPICKUP);
this.wristPController.setSetPoint(WRISTPICKUP);
}
public void wristScore() {
this.wrist.setPosition(WRISTSCORE);
this.wristPController.setSetPoint(WRISTSCORE);
}
public void update() {
this.wristPController.setTolerance(TOL);
this.wristPController.setP(KP);
if (!wristPController.atSetPoint()) {
double wristDelta = this.wristPController.calculate(wrist.getPosition());
if (Math.abs(this.wristPController.getPositionError()) > 0.1) {
wristDelta = Math.copySign(MAX_DELTA, wristDelta);
}
this.wrist.setPosition(wrist.getPosition() + wristDelta);
}
}
}
public static boolean clawIsOpen;
@Config
public static class Claw {
//Values
public static double OPEN = 0.45;
public static double BIGOPEN = 0f;
public static double CLOSE = 0.85;
public static double CLAW_MIN = 0.46;
public static double CLAW_MAX = 0.5;
//Servo
private Servo claw;
public Claw init(HardwareMap hardwareMap) {
this.claw = hardwareMap.get(Servo.class, CLAW);
this.claw.scaleRange(CLAW_MIN, CLAW_MAX);
close();
return this;
}
@ -286,6 +462,7 @@ public class Robot {
}
@Config
public static class Led {
private RevBlinkinLedDriver led;
@ -311,7 +488,12 @@ public class Robot {
}
}
@Config
public static class Plane {
//Values
public static double PLANELOCK = 0.1;
public static double PLANELAUNCH = 0.9;
//Servo
private Servo plane;
public Plane init(HardwareMap hardwareMap) {
@ -329,59 +511,5 @@ public class Robot {
}
}
public void pickupMacro(GamepadEx gamepadEx, double runtime) {
switch (pickupMacroState) {
case IDLE:
if (gamepadEx.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)){
pickupMacroState = pickupMacroStates.OPEN;
}
break;
case OPEN:
delay = runtime + .3;
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 + .3;
pickupMacroState = pickupMacroStates.NEUTRAL;
}
break;
case NEUTRAL:
if (runtime > delay) {
this.getArm().armRest();
pickupMacroState = pickupMacroStates.IDLE;
}
break;
}
}
public pickupMacroStates pickupMacroState = pickupMacroStates.IDLE;
public enum pickupMacroStates{
IDLE,
OPEN,
DROP,
CLOSE,
NEUTRAL
}
double delay;
public TrajectorySequenceBuilder getTrajectorySequenceBuilder() {
this.drive.update();
return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate());
}
}

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java
View File

@ -11,10 +11,10 @@ import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveCons
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLOWMODE_SPEED;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLOWMODE_TURN;
import static org.firstinspires.ftc.teamcode.util.Configurables.SPEED;
import static org.firstinspires.ftc.teamcode.util.Configurables.TURN;
import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.SLOWMODE_SPEED;
import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.SLOWMODE_TURN;
import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.SPEED;
import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.TURN;
import androidx.annotation.NonNull;
@ -329,9 +329,9 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr
this.setWeightedDrivePower(
new Pose2d(
gamepad1.left_stick_y* -1 * speedScale,
gamepad1.left_stick_x*-1 * speedScale,
-gamepad1.right_stick_x * turnScale
gamepad2.left_stick_y * -1 * speedScale,
gamepad2.left_stick_x * -1 * speedScale,
-gamepad2.right_stick_x * turnScale
));
}
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java
View File

@ -1,7 +1,6 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@ -9,7 +8,6 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.opmodes.AutoRedFarTwoPlusTwo.autoState;
@Config
public abstract class AutoBase extends LinearOpMode {
protected Pose2d initialPosition;
Robot robot;
@ -37,11 +35,11 @@ public abstract class AutoBase extends LinearOpMode {
this.telemetry.addData("Park Position", parkLocation);
this.telemetry.addData("Delay", delay);
this.telemetry.update();
}
while (state != autoState.STOP) {
followTrajectories();
robot.getDrive().update();
robot.update();
}
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java
View File

@ -1,7 +1,6 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@ -10,7 +9,6 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Config
@Autonomous(name = "autoBlue")
public class AutoBlue extends LinearOpMode {
protected Pose2d initialPosition;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java
View File

@ -1,7 +1,6 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@ -10,7 +9,6 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Config
@Autonomous(name = "autoBlueFar")
public class AutoBlueFarStem extends LinearOpMode {
protected Pose2d initialPosition;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java
View File

@ -1,7 +1,6 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@ -12,7 +11,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Config
@Autonomous(name = "autoBlue2+2")
public class AutoBlueTwoPlusTwo extends LinearOpMode {
protected Pose2d initialPosition;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java
View File

@ -1,7 +1,6 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@ -12,7 +11,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Config
@Autonomous(name = "autoRed")
public class AutoRed extends LinearOpMode {
protected Pose2d initialPosition;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java
View File

@ -1,7 +1,6 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@ -12,7 +11,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Config
@Autonomous(name = "AutoRedFar")
public class AutoRedFar extends LinearOpMode {
protected Pose2d initialPosition;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java
View File

@ -1,6 +1,5 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@ -8,7 +7,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Config
@Autonomous(name = "AutoRedFar2+2",preselectTeleOp = "Main TeleOp")
public class AutoRedFarTwoPlusTwo extends AutoBase {
//Pose2ds

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFaronepluszero.java
View File

@ -1,7 +1,6 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@ -12,7 +11,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Config
@Autonomous(name = "AutoRedFar1+0")
public class AutoRedFaronepluszero extends LinearOpMode {
protected Pose2d initialPosition;

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java
View File

@ -1,7 +1,6 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@ -11,8 +10,9 @@ import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Config
@Autonomous(name = "autoRed2+2")
public class AutoRedTwoPlusTwo extends LinearOpMode {
protected Pose2d initialPosition;
private Robot robot;
@ -37,7 +37,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
final static Pose2d APPROACHING_BOARD = new Pose2d(70, -31, Math.toRadians(360));
final static Pose2d SCORE_STACK = new Pose2d(73.5, -31, Math.toRadians(360));
//Park
final static Pose2d BACK_OFF = new Pose2d(60,-58,Math.toRadians(360));
final static Pose2d BACK_OFF = new Pose2d(60, -58, Math.toRadians(360));
final static Pose2d PARK = new Pose2d(80, -60, Math.toRadians(360));
protected void scorePreloadOne() {
@ -64,7 +64,8 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
case "LEFT":
builder.lineToLinearHeading(LEFT_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));;
MecanumDrive.getAccelerationConstraint(20));
;
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_BOARD,
@ -89,7 +90,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
builder.addTemporalMarker(.3, robot.getArm()::armRest);
builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
builder.addTemporalMarker(1.5,robot.getClaw()::openStack);
builder.addTemporalMarker(1.5, robot.getClaw()::openStack);
builder.addTemporalMarker(1.5, robot.getArm()::pickup);
builder.lineToLinearHeading(TO_STACK);
builder.lineToLinearHeading(TO_STACK_SLOW,
@ -104,7 +105,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
builder.addTemporalMarker(.3, robot.getArm()::armRest);
builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
builder.addTemporalMarker(1.5,robot.getClaw()::openStack);
builder.addTemporalMarker(1.5, robot.getClaw()::openStack);
builder.addTemporalMarker(1.5, robot.getArm()::pickup);
builder.lineToLinearHeading(TO_STACK);
builder.lineToLinearHeading(TO_STACK_SLOW2,
@ -113,7 +114,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void scoreStack() {
protected void scoreStack() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(BACK_THROUGH_GATE);
builder.lineToLinearHeading(APPROACHING_BOARD);
@ -129,7 +130,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
builder.lineToLinearHeading(new Pose2d(75, -31, Math.toRadians(360)),
MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
builder.addTemporalMarker(.2,this::clawSlowOpen);
builder.addTemporalMarker(.2, this::clawSlowOpen);
this.robot.getDrive().followTrajectorySequence(builder.build());
}

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

@ -6,14 +6,13 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.util.Configurables;
@com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "Main TeleOp", group = "Development")
public class MainTeleOp extends OpMode {
private Robot robot;
private MecanumDrive drive;
GamepadEx controller1;
GamepadEx controller2;
private Robot robot;
private MecanumDrive drive;
@Override
public void init() {
@ -22,15 +21,15 @@ public class MainTeleOp extends OpMode {
this.controller1 = new GamepadEx(this.gamepad1);
}
public void loop() {
//GamePad Controls
boolean slideUp = controller2.isDown(GamepadKeys.Button.DPAD_UP);
boolean slideDown = controller2.isDown(GamepadKeys.Button.DPAD_LEFT);
boolean hang = gamepad2.left_bumper;
boolean hangRelease = gamepad2.right_bumper;
boolean hangPlane = gamepad2.y;
boolean plane = gamepad2.dpad_right;
//Read Controller
controller1.readButtons();
controller2.readButtons();
//Drive
@ -44,51 +43,55 @@ public class MainTeleOp extends OpMode {
|| controller2.wasJustReleased(GamepadKeys.Button.DPAD_LEFT)) {
this.robot.getSlides().slideStop();
}
if (gamepad2.left_trigger > .5){
Configurables.SLIDE_POWER_UP = .3;
if (gamepad2.left_trigger > .1) {
Robot.Slides.SLIDE_POWER_UP = .3;
} else {
Configurables.SLIDE_POWER_UP = .7;
Robot.Slides.SLIDE_POWER_UP = .7;
}
////Macros
this.robot.pickupMacro(controller2, getRuntime()); //DPADDOWN
this.robot.armMacro(controller2, getRuntime()); //X
//
//Arm and Wrist
if (controller2.wasJustPressed(GamepadKeys.Button.X)) {
this.robot.getArm().armSecondaryScore();
this.robot.getWrist().wristScore();
} else if (controller2.wasJustPressed(GamepadKeys.Button.A)) {
// if (controller2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) {
// this.robot.getArm().pickup(true);
// } else
// if (controller2.wasJustReleased(GamepadKeys.Button.DPAD_DOWN)) {
// this.robot.getArm().armRest(true);
// } else
if (controller2.wasJustPressed(GamepadKeys.Button.A)) {
this.robot.getArm().armRest();
this.robot.getWrist().wristPickup();
}
////Claw
//Claw
if (controller2.wasJustPressed(GamepadKeys.Button.B)) {
gamepad1.rumble(300);
} else if (controller2.isDown(GamepadKeys.Button.B)){
} else if (controller2.isDown(GamepadKeys.Button.B) || controller2.isDown(GamepadKeys.Button.DPAD_DOWN)) {
this.robot.getClaw().open();
} else if (controller2.wasJustReleased(GamepadKeys.Button.B)){
} else if (controller2.wasJustReleased(GamepadKeys.Button.B)) {
this.robot.getClaw().close();
}
////Hang
//Hang
if (hang) {
this.robot.getHang().hang();
} else if (hangRelease){
} else if (hangRelease) {
this.robot.getHang().hangRelease();
} else if (hangPlane) {
this.robot.getHang().hangPlane();
}
int PositionLeft = this.robot.getSlides().slidesLeft.getCurrentPosition();
telemetry.addData("positionLeft",(PositionLeft));
int PositionRight = this.robot.getSlides().slidesRight.getCurrentPosition();
telemetry.addData("positionRight",(PositionRight));
telemetry.update();
//
//Plane
if (plane) {
this.robot.getPlane().planeLaunch();
}else {
} else {
this.robot.getPlane().planeLock();
}
//
//Telemetry
int PositionLeft = this.robot.getSlides().slidesLeft.getCurrentPosition();
telemetry.addData("positionLeft", (PositionLeft));
int PositionRight = this.robot.getSlides().slidesRight.getCurrentPosition();
telemetry.addData("positionRight", (PositionRight));
telemetry.update();
//Update Robot
this.robot.update();
}
}

39
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java
View File

@ -13,37 +13,12 @@ public class Configurables {
public static Color FTC_WHITE_LOWER = new Color(0, 0, 40);
public static Color FTC_WHITE_UPPER = new Color(180, 30, 255);
//Servo Positions
public static double ARMREST = 0.91;
public static double ARMSCORE = 0.4;
public static double ARMACCSCORE = .57;
public static double ARMPICKUPSTACK = 0.815;
public static double PICKUP = 0.94;
public static double WRISTPICKUP = 0.3;
public static double WRISTSCORE = .98;
public static double OPEN = 0.481;
public static double BIGOPEN = 0.65;
public static double CLOSE = .51;
public static double PLANELOCK = 0.1;
public static double PLANELAUNCH = 0.9;
//Drive Speed
public static double SPEED = 1;
public static double SLOWMODE_SPEED = 0.3;
public static double TURN = .75;
public static double SLOWMODE_TURN = 0.3;
//Motor Positions
public static double SLIDE_POWER_UP = .7;
public static double SLIDE_POWER_DOWN = .5;
public static int SLIDELAYERONE = 60;
public static int SLIDEAUTOSTACKS = 250;
public static int SLIDEUP = 1150;
public static int HANGRELEASE = 1550;
public static int HANG = 0;
public static int HANGPLANE = 1150;
public static int SLIDELAYERTWO = 350;
public static int SLIDESTACK = 80;
public static int SLIDEPICKUPSTACKSTWO = 30;
@Config
public static class driveSpeed {
public static double SPEED = 1;
public static double SLOWMODE_SPEED = 0.3;
public static double TURN = .75;
public static double SLOWMODE_TURN = 0.3;
}
}