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;
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.ARMPICKUPSTACK;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMREST;
@ -69,19 +68,17 @@ public class Robot {
@Getter
private Slides slides;
// GamepadEx controller2 = new GamepadEx(gamepad2);
// GamepadEx controller1 = new GamepadEx(gamepad1);
public Robot init(HardwareMap hardwareMap) {
this.drive = new MecanumDrive(hardwareMap);
// this.hang = new Hang().init(hardwareMap);
// this.arm = new Arm().init(hardwareMap);
// this.wrist = new Wrist().init(hardwareMap);
// this.claw = new Claw().init(hardwareMap);
this.hang = new Hang().init(hardwareMap);
this.arm = new Arm().init(hardwareMap);
this.wrist = new Wrist().init(hardwareMap);
this.claw = new Claw().init(hardwareMap);
// this.camera = new Camera(hardwareMap);
// this.plane = new Plane().init(hardwareMap);
// this.slides= new Slides().init(hardwareMap);
this.plane = new Plane().init(hardwareMap);
this.slides= new Slides().init(hardwareMap);
// this.led = new Led().init(hardwareMap);
return this;
}
@ -130,7 +127,16 @@ public class Robot {
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:
if (runtime > delay) {
this.getArm().armRest();
gamepad1.rumble(300);
pickupMacroState = pickupMacroStates.IDLE;
}
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 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 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.TrajectoryAccelerationConstraint;
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.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
@ -324,15 +323,15 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr
return new ProfileAccelerationConstraint(maxAccel);
}
public void setInput(GamepadEx controller1, GamepadEx controller2) {
double speedScale = controller1.isDown(GamepadKeys.Button.Y) ? SLOWMODE_SPEED : SPEED;
double turnScale = controller1.isDown(GamepadKeys.Button.Y) ? SLOWMODE_TURN : TURN;
public void setInput(Gamepad gamepad1, Gamepad gamepad2) {
double speedScale = gamepad1.y ? SLOWMODE_SPEED : SPEED;
double turnScale = gamepad1.y ? SLOWMODE_TURN : TURN;
this.setWeightedDrivePower(
new Pose2d(
controller1.getLeftY()* -1 * speedScale,
controller1.getLeftX()*-1 * speedScale,
-controller1.getRightX() * turnScale
gamepad1.left_stick_y* -1 * speedScale,
gamepad1.left_stick_x*-1 * speedScale,
-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 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 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,
FLIPYELLOW,
SCOREYELLOW,
PARK,
FIRSTCYCLE,
RETRACT,
SCORECYCLEONE,
}
protected void scorePreloadOne() {
@ -270,12 +272,40 @@ public class AutoRedFarTwoPlusTwo extends AutoBase {
case SCOREYELLOW:
if (!robot.getDrive().isBusy()){
this.robot.getClaw().open();
state = autoState.PARK;
runtime = getRuntime();
state = autoState.FIRSTCYCLE;
}
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;
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;
@ -10,74 +11,76 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
public class MainTeleOp extends OpMode {
private Robot robot;
private MecanumDrive drive;
GamepadEx controller1;
GamepadEx controller2;
@Override
public void init() {
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() {
// 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;
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;
controller1.readButtons();
controller2.readButtons();
//Drive
robot.getDrive().setInput(this.controller1, this.controller2);
robot.getDrive().setInput(gamepad1, gamepad2);
//slides
// if (slideUp) {
// this.robot.getSlides().slideUp();
// } else if (slideDown) {
// this.robot.getSlides().slideDown();
// } else if (controller2.wasJustReleased(GamepadKeys.Button.DPAD_UP)
// || controller2.wasJustReleased(GamepadKeys.Button.DPAD_LEFT)) {
// this.robot.getSlides().slideStop();
// }
if (slideUp) {
this.robot.getSlides().slideUp();
} else if (slideDown) {
this.robot.getSlides().slideDown();
} else if (controller2.wasJustReleased(GamepadKeys.Button.DPAD_UP)
|| controller2.wasJustReleased(GamepadKeys.Button.DPAD_LEFT)) {
this.robot.getSlides().slideStop();
}
////Macros
// this.robot.pickupMacro(controller2, getRuntime()); //DPADDOWN
this.robot.pickupMacro(controller2, getRuntime()); //DPADDOWN
//
////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();
// }
//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();
}
////Claw
// if (controller2.wasJustPressed(GamepadKeys.Button.B)) {
// gamepad1.rumble(300);
// } else if (controller2.isDown(GamepadKeys.Button.B)){
// this.robot.getClaw().open();
// } else if (controller2.wasJustReleased(GamepadKeys.Button.B)){
// this.robot.getClaw().close();
// }
if (controller2.wasJustPressed(GamepadKeys.Button.B)) {
gamepad1.rumble(300);
} else if (controller2.isDown(GamepadKeys.Button.B)){
this.robot.getClaw().open();
} else if (controller2.wasJustReleased(GamepadKeys.Button.B)){
this.robot.getClaw().close();
}
////Hang
// if (hang) {
// this.robot.getHang().hang();
// } else if (hangRelease){
// this.robot.getHang().hangRelease();
// } else if (hangPlane) {
// this.robot.getHang().hangPlane();
// }
//
if (hang) {
this.robot.getHang().hang();
} else if (hangRelease){
this.robot.getHang().hangRelease();
} else if (hangPlane) {
this.robot.getHang().hangPlane();
}
// 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();
// }
//Plane
if (plane) {
this.robot.getPlane().planeLaunch();
}else {
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);
//Servo Positions
public static double ARMREST = 0.8;
public static double ARMREST = 0.91;
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 PICKUP = 0.835;
public static double WRISTPICKUP = 0.28;
public static double WRISTSCORE = .96;
public static double OPEN = 0.82;
public static double WRISTPICKUP = 0.3;
public static double WRISTSCORE = .98;
public static double OPEN = 0.483;
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 PLANELAUNCH = 0.9;
@ -39,9 +39,9 @@ public class Configurables {
public static int SLIDELAYERONE = 60;
public static int SLIDEAUTOSTACKS = 250;
public static int SLIDEUP = 1150;
public static int HANGRELEASE = 2500;
public static int HANGRELEASE = 1550;
public static int HANG = 0;
public static int HANGPLANE = 1800;
public static int HANGPLANE = 1150;
public static int SLIDELAYERTWO = 350;
public static int SLIDESTACK = 80;
public static int SLIDEPICKUPSTACKSTWO = 30;