Skip to content

Commit

Permalink
more component stuff
Browse files Browse the repository at this point in the history
  • Loading branch information
@JJTech0130
JJTech0130 committed Jan 6, 2024
1 parent c79e4ff commit d00290c
Show file tree
Hide file tree
Showing 4 changed files with 112 additions and 24 deletions.
27 changes: 27 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/TrajectoryDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -354,4 +354,31 @@ public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel,
public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
return new ProfileAccelerationConstraint(maxAccel);
}

public enum Quadrant {
RED_AUDIENCE,
RED_BOARD,
BLUE_AUDIENCE,
BLUE_BOARD
}

public static TrajectoryDrive.Quadrant whichQuadrant(Pose2d pose) {
if (pose.getX() < 0) {
if (pose.getY() < 0) {
return TrajectoryDrive.Quadrant.RED_AUDIENCE;
} else {
return TrajectoryDrive.Quadrant.BLUE_AUDIENCE;
}
} else {
if (pose.getY() < 0) {
return TrajectoryDrive.Quadrant.RED_BOARD;
} else {
return TrajectoryDrive.Quadrant.BLUE_BOARD;
}
}
}

public Quadrant currentQuadrant() {
return whichQuadrant(getPoseEstimate());
}
}
51 changes: 27 additions & 24 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoPaths.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,9 @@
import static org.firstinspires.ftc.teamcode.drive.panthera.PantheraDriveConstants.MAX_ANG_ACCEL;
import static org.firstinspires.ftc.teamcode.drive.panthera.PantheraDriveConstants.MAX_ANG_VEL;

import static org.firstinspires.ftc.teamcode.vision.AprilTagLocalizer.Quadrant.*;
import static org.firstinspires.ftc.teamcode.vision.TensorFlowDetection.PropPosition.*;

import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;

Expand Down Expand Up @@ -88,29 +91,29 @@ public static TrajectorySequence buildTrajectory(
PantheraDrive.VEL_CONSTRAINT, PantheraDrive.ACCEL_CONSTRAINT,
MAX_ANG_VEL, MAX_ANG_ACCEL
);
// // If we're on the RED side, flip the offset
// if (quadrant == RED_BOARD || quadrant == RED_AUDIENCE) {
// BOT_DROPPER_OFFSET = BOT_DROPPER_OFFSET.times(-1);
// }
// // If it's a CENTER prop, move forward to the coords
// if (prop == CENTER) {
// trajBuilder.lineTo(lineCoords(quadrant, prop));
// }
//// builder.lineTo(new Vector2d(BLUE_BOARD_LEFT_LINE.getX() - BOT_DROPPER_OFFSET.getX(), BLUE_BOARD_LEFT_LINE.getY() + BOT_DROPPER_OFFSET.getY()))
//// // Drop the pixel
//// .addTemporalMarker(()->{
//// //PIXEL DROP
//// Log.i("DROP", "dropping purple");
//// purpleServo.setPosition(1);
//// })
// trajBuilder.waitSeconds(1.0)
// // Back up to the BLUE PARK Y coord
// .lineTo(new Vector2d(BLUE_BOARD_LEFT_LINE.getX(), BLUE_PARK.getY()))
// // Face forwards
// .turn(Math.toRadians(90))
// // Park
// .lineTo(BLUE_PARK)
// .build();
return null;
// If we're on the RED side, flip the offset
if (quadrant == RED_BOARD || quadrant == RED_AUDIENCE) {
BOT_DROPPER_OFFSET = BOT_DROPPER_OFFSET.times(-1);
}
// If it's a CENTER prop, move forward to the coords
if (prop == CENTER) {
trajBuilder.lineTo(lineCoords(quadrant, prop));
}
// builder.lineTo(new Vector2d(BLUE_BOARD_LEFT_LINE.getX() - BOT_DROPPER_OFFSET.getX(), BLUE_BOARD_LEFT_LINE.getY() + BOT_DROPPER_OFFSET.getY()))
// // Drop the pixel
// .addTemporalMarker(()->{
// //PIXEL DROP
// Log.i("DROP", "dropping purple");
// purpleServo.setPosition(1);
// })
trajBuilder.waitSeconds(1.0)
// Back up to the BLUE PARK Y coord
.lineTo(new Vector2d(BLUE_BOARD_LEFT_LINE.getX(), BLUE_PARK.getY()))
// Face forwards
.turn(Math.toRadians(90))
// Park
.lineTo(BLUE_PARK)
.build();
return trajBuilder.build();
}
}
10 changes: 10 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/Component.java
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,18 @@
package org.firstinspires.ftc.teamcode.opmode.components;

import com.acmerobotics.roadrunner.geometry.Vector2d;

import org.firstinspires.ftc.teamcode.drive.Robot;

public abstract class Component {
static Vector2d BLUE_BOARD_CENTER_LINE = new Vector2d(12, 24.5);
static Vector2d BLUE_BOARD_LEFT_LINE = new Vector2d(23, 30);
static Vector2d BLUE_BOARD_RIGHT_LINE = new Vector2d(1, 30);
static Vector2d RED_BOARD_CENTER_LINE = new Vector2d(12, -24.5);
// LEFT MISSING
static Vector2d RED_BOARD_RIGHT_LINE = new Vector2d(23, -30);
//static Vector2d RED_AUDIENCE_CENTER_LINE = new Vector2d(12, -24.5);

private final Robot robot;
protected Component(Robot robot) {
this.robot = robot;
Expand Down
48 changes: 48 additions & 0 deletions .../src/main/java/org/firstinspires/ftc/teamcode/opmode/components/PurplePixelComponent.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
package org.firstinspires.ftc.teamcode.opmode.components;

import com.acmerobotics.roadrunner.geometry.Vector2d;

import org.firstinspires.ftc.teamcode.drive.Robot;
import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.vision.TensorFlowDetection;

public class PurplePixelComponent extends Component {
private TensorFlowDetection.PropPosition propPosition;
public PurplePixelComponent(Robot robot, TensorFlowDetection.PropPosition propPosition) {
super(robot);
this.propPosition = propPosition;
}

@Override
public void drive() {
TrajectorySequenceBuilder trajBuilder = getRobot().getDrive().trajectorySequenceBuilder(getRobot().getDrive().getPoseEstimate());

if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD) {
if (propPosition == TensorFlowDetection.PropPosition.LEFT) {
trajBuilder = trajBuilder.lineTo(new Vector2d(3 * 12 + 7, 5 * 12 + 4));
} else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) {
trajBuilder = trajBuilder.lineTo(new Vector2d(3 * 12 + 7, -5 * 12 + 4));
} else {
// CENTER, drive straight forward
trajBuilder = trajBuilder.lineTo(BLUE_BOARD_CENTER_LINE);
}
}

TrajectorySequence traj = trajBuilder.build();
getRobot().getDrive().followTrajectorySequence(traj);

// trajBuilder = trajBuilder.lineTo(new Vector2d(startPose.getX(), -2*12 - 3));
// .addTemporalMarker(() -> {
// Log.i("DROP", "dropping purple");
// purpleServo.setPosition(1);
// })
// .waitSeconds(1)
// .lineTo(new Vector2d(startPose.getX(), -5 * 12+3))
// .turn(Math.toRadians(90))
// .lineTo(new Vector2d(3*12+7, -5*12+4))
// .build();

}
}

0 comments on commit d00290c

Please sign in to comment.