Skip to content

Commit

Permalink
tuning psi bot
Browse files Browse the repository at this point in the history
  • Loading branch information
@powerfulHermes
powerfulHermes committed Feb 15, 2024
1 parent 9751934 commit 1b3a24d
Show file tree
Hide file tree
Showing 4 changed files with 57 additions and 38 deletions.
6 changes: 3 additions & 3 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 @@ -39,7 +39,7 @@ public static double getMotorVelocityF(double ticksPerSecond) {
public static double GEAR_RATIO = 1;
public static double WHEEL_RADIUS = 1.8898;
// public static double MAX_VEL = ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85;
public static double MAX_VEL = 30;
public static double MAX_VEL = 50;

public static double MAX_ACCEL = MAX_VEL;
// public static double MAX_ANG_ACCEL = Math.toRadians(80.66924999999999);
Expand Down Expand Up @@ -151,12 +151,12 @@ public AprilTagCamera getPrimaryCamera() {
public void dropPurplePixel(boolean state) {
if (state) {
//purpleServo.setPosition(1);
for (double i = .7; i > 0; i -= 0.1) {
for (double i = 0; i < .7; i += 0.1) {
purpleServo.setPosition(i);
//GlobalOpMode.opMode.sleep(10);
}
} else {
purpleServo.setPosition(.7);
purpleServo.setPosition(0);
}
}

Expand Down
32 changes: 20 additions & 12 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,11 +78,19 @@ public void runOpMode() throws InterruptedException {

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

if (gamepad1.left_bumper || gamepad2.left_bumper) {
if (gamepad1.left_bumper) {
purplePose = purplePose + 0.1;
} else if (gamepad1.right_bumper|| gamepad2.right_bumper) {
} else if (gamepad1.right_bumper) {
purplePose = purplePose - 0.1;
}
if (gamepad2.left_bumper) {
wristPose += 0.005;
} else if (gamepad2.right_bumper) {
wristPose -= 0.005;
}
if (wristPose < 0) wristPose = 0;
if (wristPose > 1) wristPose = 1;
((PsiBot) robot).wristServo.setPosition(wristPose);
if (purplePose > 1) purplePose = 1;
if (purplePose < 0) purplePose = 0;
((PsiBot) robot).planeServo.setPosition(purplePose);
Expand Down Expand Up @@ -131,22 +139,22 @@ public void runOpMode() throws InterruptedException {



if (gamepad1.left_bumper) {
wristPose += 0.05;
} else if (gamepad1.right_bumper) {
wristPose -= 0.05;
}
if (wristPose < 0) wristPose = 0;
if (wristPose > 1) wristPose = 1;
if (gamepad1.left_bumper) {
wristPose += 0.05;
} else if (gamepad1.right_bumper) {
wristPose -= 0.05;
}
if (wristPose < 0) wristPose = 0;
if (wristPose > 1) wristPose = 1;




//rrobot.planeAngleServo.setPosition(wristPose);

//rrobot..setPosition(wristPose);
//rrobot.wristServo.setPosition(wristPose);

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

}

double p;
if (gamepad1.dpad_left) {
Expand Down
38 changes: 25 additions & 13 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,29 +38,32 @@ public void drive() {
.turn(Math.toRadians(-8));

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

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

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

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

}
trajB = trajB
Expand All @@ -69,18 +72,21 @@ public void drive() {
android.util.Log.i("PLACE PIXEL", "Placed pixel at Red Board");
})
.turnTo(0);
if (getRobot().getClass() == PsiBot.class) {
trajB.turn(Math.toRadians(-8));
}

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

if (propPos == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(56, 33));
trajB = trajB.lineTo(new Vector2d(52, 33));
} else if (propPos == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(56, 22));
trajB = trajB.lineTo(new Vector2d(52, 22));
} else {
trajB = trajB.lineTo(new Vector2d(56, 28.5));
trajB = trajB.lineTo(new Vector2d(52, 30));

}
trajB = trajB
Expand All @@ -91,6 +97,9 @@ public void drive() {
android.util.Log.i("PLACE PIXEL", "Placed pixel at Blue Board");
})
.turnTo(0);
if (getRobot().getClass() == PsiBot.class) {
trajB.turn(Math.toRadians(-8));
}


} else {
Expand All @@ -99,18 +108,21 @@ public void drive() {
.turn(Math.toRadians(-8));

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

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


}
Expand All @@ -127,7 +139,7 @@ public void drive() {
while ((((PsiBot) getRobot()).armMotor.getCurrentPosition() >= -1000 || ((PsiBot) getRobot()).armMotor.getCurrentPosition() <= -1100))
if (getRobot().getClass() == PsiBot.class) {

((PsiBot) getRobot()).armMotor.setTargetPosition(-1052);
((PsiBot) getRobot()).armMotor.setTargetPosition(-1056);
((PsiBot) getRobot()).armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
((PsiBot) getRobot()).armMotor.setPower(.1);
Log.i("PROGRESS", "4");
Expand Down
19 changes: 9 additions & 10 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 @@ -53,8 +53,8 @@ public void drive() {
//trajB = trajB.turn(Math.toRadians(180));
driveBackwards = -1;
} else {
PURPLE_X_OFFSET=7.5;
PURPLE_Y_OFFSET=11.5;
PURPLE_X_OFFSET=0;
PURPLE_Y_OFFSET=0;
}

if (propPosition == TensorFlowDetection.PropPosition.LEFT) {
Expand All @@ -66,21 +66,20 @@ public void drive() {
} else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) {
trajB.turn(Math.toRadians(-90))
.forward(12 * driveBackwards);
trajB.waitSeconds(1)
trajB
.addTemporalMarker(() -> getRobot().dropPurplePixel(true))
.waitSeconds(2);
} else {
if (moveTowardsCenter) {
trajB.turn(Math.toRadians(180))

trajB.forward(5)
.turn(Math.toRadians(180))
.forward(-20 * driveBackwards)
.forward(8 * driveBackwards + PURPLE_Y_OFFSET);
trajB.waitSeconds(1)
trajB
.addTemporalMarker(() -> getRobot().dropPurplePixel(true))
.waitSeconds(2);
if (getRobot().getClass() == PsiBot.class) {
trajB.forward(4 * driveBackwards)
.strafeLeft(16);
}
.waitSeconds(1);

} else {
trajB.forward(10 * driveBackwards);
}
Expand Down

0 comments on commit 1b3a24d

Please sign in to comment.