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

teleop testing

This commit is contained in:
sihan 2024-03-07 12:14:04 -06:00
parent f488ef6a24
commit e2223813dc
7 changed files with 118 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,6 +1,5 @@
package org.firstinspires.ftc.teamcode.hardware; package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad1;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCORE; 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.ARMPICKUPSTACK;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMREST; import static org.firstinspires.ftc.teamcode.util.Configurables.ARMREST;
@ -69,19 +68,17 @@ public class Robot {
@Getter @Getter
private Slides slides; private Slides slides;
// GamepadEx controller2 = new GamepadEx(gamepad2);
// GamepadEx controller1 = new GamepadEx(gamepad1);
public Robot init(HardwareMap hardwareMap) { public Robot init(HardwareMap hardwareMap) {
this.drive = new MecanumDrive(hardwareMap); this.drive = new MecanumDrive(hardwareMap);
// this.hang = new Hang().init(hardwareMap); this.hang = new Hang().init(hardwareMap);
// this.arm = new Arm().init(hardwareMap); this.arm = new Arm().init(hardwareMap);
// this.wrist = new Wrist().init(hardwareMap); this.wrist = new Wrist().init(hardwareMap);
// this.claw = new Claw().init(hardwareMap); this.claw = new Claw().init(hardwareMap);
// this.camera = new Camera(hardwareMap); // this.camera = new Camera(hardwareMap);
// this.plane = new Plane().init(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); // this.led = new Led().init(hardwareMap);
return this; return this;
} }
@ -130,7 +127,16 @@ public class Robot {
public void slidePickupStackTwo(){this.slideTo(SLIDEPICKUPSTACKSTWO, SLIDE_POWER_UP);} public void slidePickupStackTwo(){this.slideTo(SLIDEPICKUPSTACKSTWO, SLIDE_POWER_UP);}
public void slideStop() {this.slideTo(slidesRight.getCurrentPosition(), 1.0);} public void slideStop() {
this.slidesLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.slidesLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.slidesLeft.setTargetPosition(this.slidesLeft.getCurrentPosition());
this.slidesLeft.setPower(1);
this.slidesRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.slidesRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.slidesRight.setTargetPosition(this.slidesRight.getCurrentPosition());
this.slidesRight.setPower(1);}
} }
@ -351,7 +357,6 @@ public class Robot {
case NEUTRAL: case NEUTRAL:
if (runtime > delay) { if (runtime > delay) {
this.getArm().armRest(); this.getArm().armRest();
gamepad1.rumble(300);
pickupMacroState = pickupMacroStates.IDLE; pickupMacroState = pickupMacroStates.IDLE;
} }
break; break;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java
View File

@ -46,7 +46,7 @@ public class DriveConstants {
*/ */
public static double WHEEL_RADIUS = 1.88976; // in public static double WHEEL_RADIUS = 1.88976; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed
public static double TRACK_WIDTH = 12.244; // in public static double TRACK_WIDTH = 12.438; // in
// public static double WHEEL_RADIUS = 1.88976; // in // public static double WHEEL_RADIUS = 1.88976; // in
// public static double GEAR_RATIO = 652.5/435; // output (wheel) speed / input (motor) speed // public static double GEAR_RATIO = 652.5/435; // output (wheel) speed / input (motor) speed

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

@ -32,12 +32,11 @@ import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint; import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.hardware.PIDFCoefficients;
@ -324,15 +323,15 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr
return new ProfileAccelerationConstraint(maxAccel); return new ProfileAccelerationConstraint(maxAccel);
} }
public void setInput(GamepadEx controller1, GamepadEx controller2) { public void setInput(Gamepad gamepad1, Gamepad gamepad2) {
double speedScale = controller1.isDown(GamepadKeys.Button.Y) ? SLOWMODE_SPEED : SPEED; double speedScale = gamepad1.y ? SLOWMODE_SPEED : SPEED;
double turnScale = controller1.isDown(GamepadKeys.Button.Y) ? SLOWMODE_TURN : TURN; double turnScale = gamepad1.y ? SLOWMODE_TURN : TURN;
this.setWeightedDrivePower( this.setWeightedDrivePower(
new Pose2d( new Pose2d(
controller1.getLeftY()* -1 * speedScale, gamepad1.left_stick_y* -1 * speedScale,
controller1.getLeftX()*-1 * speedScale, gamepad1.left_stick_x*-1 * speedScale,
-controller1.getRightX() * turnScale -gamepad1.right_stick_x * turnScale
)); ));
} }
} }

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java
View File

@ -38,7 +38,7 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
public static double WHEEL_RADIUS = 0.944882; // in public static double WHEEL_RADIUS = 0.944882; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
public static double PARALLEL_X = 5.786; // X is the up and down direction public static double PARALLEL_X = 129.5; // X is the up and down direction
public static double PARALLEL_Y = -4.118; // Y is the strafe direction public static double PARALLEL_Y = -4.118; // Y is the strafe direction
public static double PERPENDICULAR_X = 3.209; public static double PERPENDICULAR_X = 3.209;

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

@ -42,7 +42,9 @@ public class AutoRedFarTwoPlusTwo extends AutoBase {
YELLOW, YELLOW,
FLIPYELLOW, FLIPYELLOW,
SCOREYELLOW, SCOREYELLOW,
PARK, FIRSTCYCLE,
RETRACT,
SCORECYCLEONE,
} }
protected void scorePreloadOne() { protected void scorePreloadOne() {
@ -270,12 +272,40 @@ public class AutoRedFarTwoPlusTwo extends AutoBase {
case SCOREYELLOW: case SCOREYELLOW:
if (!robot.getDrive().isBusy()){ if (!robot.getDrive().isBusy()){
this.robot.getClaw().open(); this.robot.getClaw().open();
state = autoState.PARK; runtime = getRuntime();
state = autoState.FIRSTCYCLE;
} }
break; break;
case PARK: case FIRSTCYCLE:
builder = this.robot.getTrajectorySequenceBuilder();
builder.splineToConstantHeading(TO_BOARD.vec(),Math.toRadians(0));
builder.lineToLinearHeading(READY_TRUSS);
builder.splineToConstantHeading(GET_STACK.vec(),Math.toRadians(0));
builder.lineToLinearHeading(PICKUP_STACK,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
runtime = getRuntime();
state = autoState.RETRACT;
break;
case RETRACT:
if (getRuntime() > runtime + .3) {
this.robot.getClaw().close();
this.robot.getArm().armRest();
this.robot.getWrist().wristPickup();
this.robot.getSlides().slideDown();
}
if (getRuntime() > runtime + 2) {
this.robot.getClaw().openStack();
this.robot.getArm().armPickupStack();
}
if (!robot.getDrive().isBusy()) {
this.robot.getClaw().close();
state = autoState.SCORECYCLEONE;
}
break;
case SCORECYCLEONE:
break;
} }
} }

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

@ -1,6 +1,7 @@
package org.firstinspires.ftc.teamcode.opmodes; package org.firstinspires.ftc.teamcode.opmodes;
import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot; import org.firstinspires.ftc.teamcode.hardware.Robot;
@ -10,74 +11,76 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
public class MainTeleOp extends OpMode { public class MainTeleOp extends OpMode {
private Robot robot; private Robot robot;
private MecanumDrive drive; private MecanumDrive drive;
GamepadEx controller1;
GamepadEx controller2;
@Override @Override
public void init() { public void init() {
this.robot = new Robot().init(hardwareMap); this.robot = new Robot().init(hardwareMap);
this.controller2 = new GamepadEx(this.gamepad2);
this.controller1 = new GamepadEx(this.gamepad1);
} }
GamepadEx controller2 = new GamepadEx(this.gamepad2);
GamepadEx controller1 = new GamepadEx(this.gamepad1);
public void loop() { public void loop() {
// boolean slideUp = controller2.isDown(GamepadKeys.Button.DPAD_UP); boolean slideUp = controller2.isDown(GamepadKeys.Button.DPAD_UP);
// boolean slideDown = controller2.isDown(GamepadKeys.Button.DPAD_LEFT); boolean slideDown = controller2.isDown(GamepadKeys.Button.DPAD_LEFT);
// boolean hang = gamepad2.left_bumper; boolean hang = gamepad2.left_bumper;
// boolean hangRelease = gamepad2.right_bumper; boolean hangRelease = gamepad2.right_bumper;
// boolean hangPlane = gamepad2.y; boolean hangPlane = gamepad2.y;
// boolean plane = gamepad2.dpad_right; boolean plane = gamepad2.dpad_right;
controller1.readButtons(); controller1.readButtons();
controller2.readButtons(); controller2.readButtons();
//Drive //Drive
robot.getDrive().setInput(this.controller1, this.controller2); robot.getDrive().setInput(gamepad1, gamepad2);
//slides //slides
// if (slideUp) { if (slideUp) {
// this.robot.getSlides().slideUp(); this.robot.getSlides().slideUp();
// } else if (slideDown) { } else if (slideDown) {
// this.robot.getSlides().slideDown(); this.robot.getSlides().slideDown();
// } else if (controller2.wasJustReleased(GamepadKeys.Button.DPAD_UP) } else if (controller2.wasJustReleased(GamepadKeys.Button.DPAD_UP)
// || controller2.wasJustReleased(GamepadKeys.Button.DPAD_LEFT)) { || controller2.wasJustReleased(GamepadKeys.Button.DPAD_LEFT)) {
// this.robot.getSlides().slideStop(); this.robot.getSlides().slideStop();
// } }
////Macros ////Macros
// this.robot.pickupMacro(controller2, getRuntime()); //DPADDOWN this.robot.pickupMacro(controller2, getRuntime()); //DPADDOWN
// //
////Arm and Wrist //Arm and Wrist
// if (controller2.wasJustPressed(GamepadKeys.Button.X)) { if (controller2.wasJustPressed(GamepadKeys.Button.X)) {
// this.robot.getArm().armSecondaryScore(); this.robot.getArm().armSecondaryScore();
// this.robot.getWrist().wristScore(); this.robot.getWrist().wristScore();
// } else if (controller2.wasJustPressed(GamepadKeys.Button.A)) { } else if (controller2.wasJustPressed(GamepadKeys.Button.A)) {
// this.robot.getArm().armRest(); this.robot.getArm().armRest();
// this.robot.getWrist().wristPickup(); this.robot.getWrist().wristPickup();
// } }
////Claw ////Claw
// if (controller2.wasJustPressed(GamepadKeys.Button.B)) { if (controller2.wasJustPressed(GamepadKeys.Button.B)) {
// gamepad1.rumble(300); gamepad1.rumble(300);
// } else if (controller2.isDown(GamepadKeys.Button.B)){ } else if (controller2.isDown(GamepadKeys.Button.B)){
// this.robot.getClaw().open(); this.robot.getClaw().open();
// } else if (controller2.wasJustReleased(GamepadKeys.Button.B)){ } else if (controller2.wasJustReleased(GamepadKeys.Button.B)){
// this.robot.getClaw().close(); this.robot.getClaw().close();
// } }
////Hang ////Hang
// if (hang) { if (hang) {
// this.robot.getHang().hang(); this.robot.getHang().hang();
// } else if (hangRelease){ } else if (hangRelease){
// this.robot.getHang().hangRelease(); this.robot.getHang().hangRelease();
// } else if (hangPlane) { } else if (hangPlane) {
// this.robot.getHang().hangPlane(); this.robot.getHang().hangPlane();
// } }
//
// int Position = this.robot.getHang().hangRight.getCurrentPosition(); // int Position = this.robot.getHang().hangRight.getCurrentPosition();
// telemetry.addData("position",(Position)); // telemetry.addData("position",(Position));
// telemetry.update(); // telemetry.update();
// //
////Plane //Plane
// if (plane) { if (plane) {
// this.robot.getPlane().planeLaunch(); this.robot.getPlane().planeLaunch();
// }else { }else {
// this.robot.getPlane().planeLock(); this.robot.getPlane().planeLock();
// } }
// //
} }
} }

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

@ -14,16 +14,16 @@ public class Configurables {
public static Color FTC_WHITE_UPPER = new Color(180, 30, 255); public static Color FTC_WHITE_UPPER = new Color(180, 30, 255);
//Servo Positions //Servo Positions
public static double ARMREST = 0.8; public static double ARMREST = 0.91;
public static double ARMSCORE = 0.4; public static double ARMSCORE = 0.4;
public static double ARMACCSCORE = .37; public static double ARMACCSCORE = .57;
public static double ARMPICKUPSTACK = 0.815; public static double ARMPICKUPSTACK = 0.815;
public static double PICKUP = 0.835; public static double PICKUP = 0.835;
public static double WRISTPICKUP = 0.28; public static double WRISTPICKUP = 0.3;
public static double WRISTSCORE = .96; public static double WRISTSCORE = .98;
public static double OPEN = 0.82; public static double OPEN = 0.483;
public static double BIGOPEN = 0.65; public static double BIGOPEN = 0.65;
public static double CLOSE = 0.91; public static double CLOSE = .51;
public static double PLANELOCK = 0.1; public static double PLANELOCK = 0.1;
public static double PLANELAUNCH = 0.9; public static double PLANELAUNCH = 0.9;
@ -39,9 +39,9 @@ public class Configurables {
public static int SLIDELAYERONE = 60; public static int SLIDELAYERONE = 60;
public static int SLIDEAUTOSTACKS = 250; public static int SLIDEAUTOSTACKS = 250;
public static int SLIDEUP = 1150; public static int SLIDEUP = 1150;
public static int HANGRELEASE = 2500; public static int HANGRELEASE = 1550;
public static int HANG = 0; public static int HANG = 0;
public static int HANGPLANE = 1800; public static int HANGPLANE = 1150;
public static int SLIDELAYERTWO = 350; public static int SLIDELAYERTWO = 350;
public static int SLIDESTACK = 80; public static int SLIDESTACK = 80;
public static int SLIDEPICKUPSTACKSTWO = 30; public static int SLIDEPICKUPSTACKSTWO = 30;