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

Claw, Gantry, and Lift

This commit is contained in:
Thomas 2023-11-28 17:07:59 -06:00
parent 0d0a315734
commit 9a332e27d9
7 changed files with 53 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
TeamCode/src/main/java/opmodes/AutoBase.java
View File

@ -138,8 +138,15 @@ public abstract class AutoBase extends LinearOpMode {
protected void moveToBackstage() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(36, 11, 0));
builder.lineToLinearHeading(new Pose2d(36, 38, 0));
if (this.alliance == CenterStageCommon.Alliance.Blue) {
builder.lineToLinearHeading(new Pose2d(36, 11, 0));
builder.lineToLinearHeading(new Pose2d(36, 38, 0));
} else {
builder.lineToLinearHeading(new Pose2d(36, -11, 0));
builder.lineToLinearHeading(new Pose2d(36, -38, 0));
}
this.robot.getDrive().followTrajectorySequence(builder.build());
}

2
TeamCode/src/main/java/opmodes/BlueFrontStage.java
View File

@ -63,4 +63,4 @@ public class BlueFrontStage extends AutoBase {
builder.lineToLinearHeading(this.rendezvous);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
}
}

8
TeamCode/src/main/java/opmodes/MainTeleOp.java
View File

@ -49,6 +49,9 @@ public class MainTeleOp extends OpMode {
boolean clawDownSafe = gamepad2.dpad_down; // dpad-down
boolean clawDown = gamepad2.a || clawDownSafe; // A
// Robot Drone Launch
boolean robotDroneLaunch = gamepad1.right_bumper; // Change if Merck wants (RT)
// Robot Lift
boolean robotLiftRotation = gamepad2.right_trigger > 0.05; // RT
boolean robotLiftExtend = gamepad2.right_trigger > 0.5; // RT
@ -114,6 +117,11 @@ public class MainTeleOp extends OpMode {
gantryXPosition = Math.max(X_MIN, Math.min(gantryXPosition, X_MAX));
this.robot.getGantry().setX(gantryXPosition);
// Robot Drone
if (robotDroneLaunch) {
this.robot.getDrone().raise();
}
// Robot Lift
if (robotLiftRotation || this.liftArmShouldBeUp) {

6
TeamCode/src/main/java/opmodes/RedFrontStage.java
View File

@ -20,6 +20,7 @@ public class RedFrontStage extends AutoBase {
new Pose2d(-12, 62));
}
// propLeft will be a reverse of BlueFrontpropRight
protected void propLeft() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-54, -17, Math.toRadians(123)));
@ -50,6 +51,7 @@ public class RedFrontStage extends AutoBase {
this.robot.getDrive().followTrajectorySequence(builder.build());
}
// propRight will be the reverse of BlueFrontpropRight
protected void propRight() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-52, -31, Math.toRadians(-180)));
@ -60,5 +62,9 @@ public class RedFrontStage extends AutoBase {
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(this.rendezvous);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
}

21
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drone.java Normal file
View File

@ -0,0 +1,21 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DRONE_ROTATION_UP;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DRONE_ROTATION_UP_NAME;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
public class Drone {
private Servo droneUpServo;
public Drone(HardwareMap hardwareMap) {
this.droneUpServo = hardwareMap.get(Servo.class, DRONE_ROTATION_UP_NAME);
}
public void raise() {
this.droneUpServo.setPosition(0.2);
}
}

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

@ -28,6 +28,9 @@ public class Robot {
@Getter
private Camera camera;
@Getter
private Drone drone;
@Getter
CenterStageCommon.Alliance alliance;
@ -68,7 +71,6 @@ public class Robot {
this.lift.update();
this.drive.update();
this.claw.update();
this.telemetry.update();
}
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java
View File

@ -16,6 +16,7 @@ public class RobotConfig {
public static final String GANTRY_X_NAME = "gantry_x";
public static final String GANTRY_ARM_NAME = "gantryArm";
public static final String GANTRY_SCREW_NAME = "screw";
public static final String DRONE_ROTATION_UP_NAME = "droneUp";
public static final String ROBOT_LIFT_ROTATION_NAME = "liftRotation";
public static final String ROBOT_LIFT_LIFT_NAME = "liftLift";
public static final String WEBCAM_NAME = "webcam";
@ -38,8 +39,8 @@ public class RobotConfig {
public static double CLAW_KP = 0.3;
// Gantry
public static double GANTRY_ARM_MIN = 0.435;
public static double GANTRY_ARM_MAX = 0.94;
public static double GANTRY_ARM_MIN = 0.42;
public static double GANTRY_ARM_MAX = 0.96;
public static int GANTRY_LIFT_DELTA = 50;
public static double GANTRY_ARM_KP = 0.1;
public static double X_KP = 0.1;
@ -50,6 +51,8 @@ public class RobotConfig {
public static int SLIDE_UP = 820;
public static double SLIDE_POWER = 0.5;
// Robot Drone Launch
public static double DRONE_ROTATION_UP = 0.2;
// Robot Lift
public static double LIFT_ROTATION_UP = 0.4;