Skip to content

Commit

Permalink
psi bot is ready for competition!!!!!!!
Browse files Browse the repository at this point in the history
  • Loading branch information
@powerfulHermes
powerfulHermes committed Feb 18, 2024
1 parent 4e586bc commit c3add6e
Show file tree
Hide file tree
Showing 5 changed files with 58 additions and 34 deletions.
8 changes: 4 additions & 4 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 @@ -168,11 +168,11 @@ public void launchPlane() {
//pull the pixel pusher servo to take out the pin
//wait a bit here
//turn the drone servo forward
planeServo.setPosition(.09);
planeServo.setPosition(.5);
GlobalOpMode.opMode.sleep(10);
mosaicServo.setPosition(1);
GlobalOpMode.opMode.sleep(10);
planeServo.setPosition(1);
mosaicServo.setPosition(0);
GlobalOpMode.opMode.sleep(1000);
planeServo.setPosition(0);


}
Expand Down
9 changes: 8 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ private TensorFlowDetection.PropPosition detectProp(Robot robot, int timeout) {
position = TensorFlowDetection.PropPosition.CENTER;
}
if (robot.getClass() == PsiBot.class) {
((PsiBot) robot).armMotor.setTargetPosition(-109);
((PsiBot) robot).armMotor.setTargetPosition(-200);
((PsiBot) robot).armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
((PsiBot) robot).armMotor.setPower(.1);
}
Expand Down Expand Up @@ -132,6 +132,13 @@ public void runOpMode() throws InterruptedException {
for (Component component : componentList) {
component.drive();
}
if (robot.getClass() == PsiBot.class) {
while (((PsiBot) robot).armMotor.getCurrentPosition() > 10 || ((PsiBot) robot).armMotor.getCurrentPosition() < -10) {
((PsiBot) robot).armMotor.setTargetPosition(-0);
((PsiBot) robot).armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
((PsiBot) robot).armMotor.setPower(.1);
}
}


}
Expand Down
8 changes: 4 additions & 4 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,14 +78,14 @@ public void runOpMode() throws InterruptedException {

if (robot.getClass() == PsiBot.class) {

if (gamepad1.left_bumper) {
if (gamepad2.left_bumper) {
purplePose = purplePose + 0.1;
} else if (gamepad1.right_bumper) {
} else if (gamepad2.right_bumper) {
purplePose = purplePose - 0.1;
}
if (gamepad2.left_bumper) {
if (gamepad1.left_bumper) {
wristPose += 0.005;
} else if (gamepad2.right_bumper) {
} else if (gamepad1.right_bumper) {
wristPose -= 0.005;
}
if (wristPose < 0) wristPose = 0;
Expand Down
61 changes: 39 additions & 22 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 @@ -38,17 +38,18 @@ public void drive() {
.turn(Math.toRadians(-8));

if (propPos == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(52, -33));
trajB = trajB.lineTo(new Vector2d(48, -37));
} else if (propPos == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(52, -26));
trajB = trajB.lineTo(new Vector2d(48, -31));
} else {
trajB = trajB.lineTo(new Vector2d(52, -30));
trajB = trajB.lineTo(new Vector2d(48, -34));

}
trajB = trajB.addTemporalMarker(() -> {
android.util.Log.i("PLACE PIXEL", "Placed pixel at Red Board");
})
.turnTo(0);
.turnTo(Math.toRadians(180)) .forward(-.5);

if (getRobot().getClass() == PsiBot.class) {
trajB.turn(Math.toRadians(-8));
}
Expand All @@ -59,19 +60,20 @@ public void drive() {
.turn(Math.toRadians(-8));

if (propPos == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(52, -33));
trajB = trajB.lineTo(new Vector2d(50, -40));
} else if (propPos == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(52, -26));
trajB = trajB.lineTo(new Vector2d(50, -34));
} else {
trajB = trajB.lineTo(new Vector2d(52, -30));
trajB = trajB.lineTo(new Vector2d(50, -37));

}
trajB = trajB

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

if (getRobot().getClass() == PsiBot.class) {
trajB.turn(Math.toRadians(-8));
}
Expand All @@ -82,11 +84,11 @@ public void drive() {
.turn(Math.toRadians(-8));

if (propPos == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(52, 33));
trajB = trajB.lineTo(new Vector2d(50, 40));
} else if (propPos == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(52, 26));
trajB = trajB.lineTo(new Vector2d(50, 34));
} else {
trajB = trajB.lineTo(new Vector2d(52, 30));
trajB = trajB.lineTo(new Vector2d(50, 37));

}
trajB = trajB
Expand All @@ -96,7 +98,9 @@ public void drive() {
.addTemporalMarker(() -> {
android.util.Log.i("PLACE PIXEL", "Placed pixel at Blue Board");
})
.turnTo(0);
.turnTo(Math.toRadians(180))
.forward(-.5);

if (getRobot().getClass() == PsiBot.class) {
trajB.turn(Math.toRadians(-8));
}
Expand All @@ -108,27 +112,28 @@ public void drive() {
.turn(Math.toRadians(-8));

if (propPos == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(52, 33));
trajB = trajB.lineTo(new Vector2d(48, 37));
} else if (propPos == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(52, 26));
trajB = trajB.lineTo(new Vector2d(48, 31));
} else {
trajB = trajB.lineTo(new Vector2d(52, 30));
trajB = trajB.lineTo(new Vector2d(48, 34));
}
trajB = trajB

.addTemporalMarker(() -> {
android.util.Log.i("PLACE PIXEL", "Placed pixel at Blue Board");
})
.turnTo(Math.toRadians(0));
.turnTo(Math.toRadians(180))
.forward(-.5);
if (getRobot().getClass() == PsiBot.class) {
trajB.turn(Math.toRadians(-8));
}


}
if (getRobot().getClass() == PsiBot.class) {
trajB.turn(Math.toRadians(180));
}
// if (getRobot().getClass() == PsiBot.class) {
// trajB.turn(Math.toRadians(180));
// }

TrajectorySequence traj = trajB.build();
Log.i("PROGRESS", "1");
Expand All @@ -137,11 +142,23 @@ public void drive() {
Log.i("PROGRESS", "2");
GlobalOpMode.opMode.telemetry.log().add("2");
while ((((PsiBot) getRobot()).armMotor.getCurrentPosition() >= -1000 || ((PsiBot) getRobot()).armMotor.getCurrentPosition() <= -1100))

if (getRobot().getClass() == PsiBot.class) {

((PsiBot) getRobot()).armMotor.setTargetPosition(-1056);
((PsiBot) getRobot()).armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
((PsiBot) getRobot()).armMotor.setPower(.1);

while (((PsiBot) getRobot()).armMotor.getCurrentPosition() > -1046 || ((PsiBot) getRobot()).armMotor.getCurrentPosition() < -1066) {
((PsiBot) getRobot()).wristServo.setPosition(.65);

// ((PsiBot) getRobot()).armMotor.setTargetPosition(-1056);
((PsiBot) getRobot()).armMotor.setTargetPosition(-1100);
((PsiBot) getRobot()).armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
((PsiBot) getRobot()).armMotor.setPower(.1);
}
// GlobalOpMode.opMode.sleep(400);
while (((PsiBot) getRobot()).wristServo.getPosition() > .25 || ((PsiBot) getRobot()).wristServo.getPosition() < .15 ) {

((PsiBot) getRobot()).wristServo.setPosition(.2);
}
Log.i("PROGRESS", "4");

}
Expand Down
6 changes: 3 additions & 3 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 @@ -15,8 +15,8 @@
public class PurplePixelComponent extends Component {
Vector2d RED_AUDIENCE_TAPE_MARKS = new Vector2d(fi(-3,0), fi(-3,0));
Vector2d BLUE_AUDIENCE_TAPE_MARKS = new Vector2d(-3 * 12, 2 * 12 + 12);
Vector2d RED_BOARD_TAPE_MARKS = new Vector2d(fi(1,0), fi(-3,0));
Vector2d BLUE_BOARD_TAPE_MARKS = new Vector2d(12, 2 * 12 + 12);
Vector2d RED_BOARD_TAPE_MARKS = new Vector2d(13.5, (-2*12)-14);
Vector2d BLUE_BOARD_TAPE_MARKS = new Vector2d(13.5, 2 * 12 + 14);
private final TensorFlowDetection.PropPosition propPosition;
private final boolean moveTowardsCenter;

Expand Down Expand Up @@ -60,7 +60,7 @@ public void drive() {

if (propPosition == TensorFlowDetection.PropPosition.LEFT) {
trajB.turn(Math.toRadians(90))
.forward(8 * driveBackwards)
.forward(10 * driveBackwards)
.forward(-3.5); // orig 13
trajB
.addTemporalMarker(() -> getRobot().dropPurplePixel(true))
Expand Down

0 comments on commit c3add6e

Please sign in to comment.