Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/develop' into develop
Browse files Browse the repository at this point in the history
  • Loading branch information
@powerfulHermes
powerfulHermes committed Feb 15, 2024
2 parents cf02590 + 1b66d13 commit 9751934
Show file tree
Hide file tree
Showing 3 changed files with 58 additions and 29 deletions.
15 changes: 9 additions & 6 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.control.PIDCoefficients;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
Expand Down Expand Up @@ -65,9 +66,10 @@ public static double getMotorVelocityF(double ticksPerSecond) {
public static double FORWARD_OFFSET = -7.5;

private Encoder leftEncoder, rightEncoder, frontEncoder;
public static double X_MULTIPLIER = 96.0/94.8; // Multiplier in the X direction
public static double X_MULTIPLIER = 1; // Multiplier in the X direction
// BAD: 92.8 93.3 92.9
public static double Y_MULTIPLIER = 96.0/89.2; // Multiplier in the Y direction
public static double Y_MULTIPLIER = 1; // Multiplier in the Y direction

// BAD: 87.8 87.2 87.8

public final Servo purpleServo;
Expand All @@ -79,9 +81,9 @@ public static double getMotorVelocityF(double ticksPerSecond) {
public PsiBot(HardwareMap hardwareMap) {
super(hardwareMap);
cameras = new AprilTagCamera[3];
cameras[0] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Left"), 5.5, -6.5);
cameras[0] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Left"), 5.5, -5);
cameras[1] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Center"), 0, 0);
cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 5.5, 6.5);
cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 5.5, 5);

purpleServo = hardwareMap.get(Servo.class, "purple");
planeServo = hardwareMap.get(Servo.class, "plane");
Expand Down Expand Up @@ -125,6 +127,7 @@ public PsiBot(HardwareMap hardwareMap) {
DW_GEAR_RATIO,
DW_WHEEL_RADIUS,
DW_TICKS_PER_REV

);
drive.leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
drive.leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
Expand All @@ -148,12 +151,12 @@ public AprilTagCamera getPrimaryCamera() {
public void dropPurplePixel(boolean state) {
if (state) {
//purpleServo.setPosition(1);
for (double i = 0; i < 0.7; i += 0.1) {
for (double i = .7; i > 0; i -= 0.1) {
purpleServo.setPosition(i);
//GlobalOpMode.opMode.sleep(10);
}
} else {
purpleServo.setPosition(0);
purpleServo.setPosition(.7);
}
}

Expand Down
46 changes: 27 additions & 19 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,13 +34,15 @@ public void drive() {

if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE) {
trajB = trajB.lineTo(new Vector2d(startPose.getX(), -12))
.lineTo(new Vector2d(40, -12));
.lineTo(new Vector2d(40, -12))
.turn(Math.toRadians(-8));

if (propPos == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(40, -28.25));
trajB = trajB.lineTo(new Vector2d(56, -28.25));
} else if (propPos == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(40, -42.5));
trajB = trajB.lineTo(new Vector2d(56, -42.5));
} else {
trajB = trajB.lineTo(new Vector2d(40, -35));
trajB = trajB.lineTo(new Vector2d(56, -35));

}
trajB = trajB.addTemporalMarker(() -> {
Expand All @@ -50,13 +52,15 @@ public void drive() {

} else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD) {
trajB = trajB.lineTo(new Vector2d(startPose.getX(), -12))
.lineTo(new Vector2d(40, -12));
.lineTo(new Vector2d(40, -12))
.turn(Math.toRadians(-8));

if (propPos == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(40, -28.25));
trajB = trajB.lineTo(new Vector2d(56, -28.25));
} else if (propPos == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(40, -42.5));
trajB = trajB.lineTo(new Vector2d(56, -42.5));
} else {
trajB = trajB.lineTo(new Vector2d(40, -35));
trajB = trajB.lineTo(new Vector2d(56, -35));

}
trajB = trajB
Expand All @@ -68,18 +72,20 @@ public void drive() {

} else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD) {
trajB = trajB.lineTo(new Vector2d(startPose.getX(), 12))
.lineTo(new Vector2d(40, 12));
.lineTo(new Vector2d(40, 12))
.turn(Math.toRadians(-8));

if (propPos == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(40, 28.25));
trajB = trajB.lineTo(new Vector2d(56, 33));
} else if (propPos == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(40, 42.5));
trajB = trajB.lineTo(new Vector2d(56, 22));
} else {
trajB = trajB.lineTo(new Vector2d(40, 35));
trajB = trajB.lineTo(new Vector2d(56, 28.5));

}
trajB = trajB

.turn(Math.toRadians(90 - startPose.getHeading()))

//TODO:fix error with temporal marker because ryan is a dum dum
.addTemporalMarker(() -> {
android.util.Log.i("PLACE PIXEL", "Placed pixel at Blue Board");
Expand All @@ -88,21 +94,23 @@ public void drive() {


} else {
trajB = trajB.lineTo(new Vector2d(startPose.getX(), 12))
.lineTo(new Vector2d(40, 12));
trajB = trajB.lineTo(new Vector2d(startPose.getX(), 8))
.lineTo(new Vector2d(40, 8))
.turn(Math.toRadians(-8));

if (propPos == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(40, 28.25));
trajB = trajB.lineTo(new Vector2d(56, 28.25));
} else if (propPos == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(40, 42.5));
trajB = trajB.lineTo(new Vector2d(56, 42.5));
} else {
trajB = trajB.lineTo(new Vector2d(40, 38.5));
trajB = trajB.lineTo(new Vector2d(56, 38.5));
}
trajB = trajB

.addTemporalMarker(() -> {
android.util.Log.i("PLACE PIXEL", "Placed pixel at Blue Board");
})
.turnTo(0);
.turnTo(Math.toRadians(0));


}
Expand Down
26 changes: 22 additions & 4 deletions .../src/main/java/org/firstinspires/ftc/teamcode/opmode/components/PurplePixelComponent.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@

import com.acmerobotics.roadrunner.geometry.Vector2d;

import org.firstinspires.ftc.teamcode.drive.PsiBot;
import org.firstinspires.ftc.teamcode.drive.Robot;
import org.firstinspires.ftc.teamcode.drive.RoboticaBot;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
Expand Down Expand Up @@ -41,6 +42,8 @@ private Vector2d currentTapeMarks() {

@Override
public void drive() {
double PURPLE_X_OFFSET=0;
double PURPLE_Y_OFFSET=0;

TrajectorySequenceBuilder trajB = getRobot().getDrive().trajectorySequenceBuilder(getRobot().getDrive().getPoseEstimate());
trajB.lineTo(currentTapeMarks());
Expand All @@ -49,26 +52,41 @@ public void drive() {
if (getRobot().getClass() == RoboticaBot.class) {
//trajB = trajB.turn(Math.toRadians(180));
driveBackwards = -1;
} else {
PURPLE_X_OFFSET=7.5;
PURPLE_Y_OFFSET=11.5;
}

if (propPosition == TensorFlowDetection.PropPosition.LEFT) {
trajB.turn(Math.toRadians(90))
.forward(8 * driveBackwards); // orig 13
trajB.waitSeconds(1)
.addTemporalMarker(() -> getRobot().dropPurplePixel(true))
.waitSeconds(2);
} else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) {
trajB.turn(Math.toRadians(-90))
.forward(12 * driveBackwards);
trajB.waitSeconds(1)
.addTemporalMarker(() -> getRobot().dropPurplePixel(true))
.waitSeconds(2);
} else {
if (moveTowardsCenter) {
trajB.turn(Math.toRadians(180))
.forward(-16 * driveBackwards);
.forward(-20 * driveBackwards)
.forward(8 * driveBackwards + PURPLE_Y_OFFSET);
trajB.waitSeconds(1)
.addTemporalMarker(() -> getRobot().dropPurplePixel(true))
.waitSeconds(2);
if (getRobot().getClass() == PsiBot.class) {
trajB.forward(4 * driveBackwards)
.strafeLeft(16);
}
} else {
trajB.forward(10 * driveBackwards);
}
}

trajB.waitSeconds(1)
.addTemporalMarker(() -> getRobot().dropPurplePixel(true))
.waitSeconds(2);


//if (propPosition == TensorFlowDetection.PropPosition.CENTER && (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE || getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE )) {
if (moveTowardsCenter && propPosition == TensorFlowDetection.PropPosition.CENTER) {
Expand Down

0 comments on commit 9751934

Please sign in to comment.