Skip to content

Commit

Permalink
lots and lots of testing and refinement
Browse files Browse the repository at this point in the history
  • Loading branch information
@powerfulHermes
powerfulHermes committed Jan 25, 2024
1 parent ca27ada commit 4afb1bb
Show file tree
Hide file tree
Showing 7 changed files with 94 additions and 25 deletions.
1 change: 1 addition & 0 deletions MeepMeepTesting/src/main/java/com/example/meepmeeptesting/GoToBoard.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@ public void drive() {
.addTemporalMarker(() -> {
// android.util.Log.i("PLACE PIXEL", "Placed pixel at Blue Board");
});
//.turn

}

Expand Down
1 change: 1 addition & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/Configuration.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,4 +3,5 @@
public class Configuration {
String park = "none";
boolean placePixel = false;
boolean fieldCentric = true;
}
10 changes: 10 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/Configurator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,17 +58,21 @@ private static void save(Configuration config, String filename) {
@Override
public void runOpMode() throws InterruptedException {
waitForStart();
telemetry.addData("CONTROLS", "A: place pixel toggle \nB: inner park \nY: outer park \nX: board park \ndpad_down: no park \ndpad_up: field centric toggle");

Toggle purpleToggle = new Toggle();
Toggle fieldCentricToggle = new Toggle();
Debouncer innerPark = new Debouncer();
Debouncer outerPark = new Debouncer();
Debouncer placePixel = new Debouncer();


//String park = "none";

Configuration config = Configurator.load();

purpleToggle.state = config.placePixel;
fieldCentricToggle.state = config.fieldCentric;

// Toggle innerParkToggle = new Toggle();
// Toggle outerParkToggle = new Toggle();
Expand All @@ -81,6 +85,12 @@ public void runOpMode() throws InterruptedException {

config.placePixel = purpleToggle.state;

fieldCentricToggle.update(gamepad1.dpad_up);

telemetry.addData("field centric", fieldCentricToggle.state);

config.fieldCentric = fieldCentricToggle.state;

if (config.placePixel) {

if (innerPark.update(gamepad1.b)) {
Expand Down
8 changes: 4 additions & 4 deletions 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 @@ -29,23 +29,23 @@ public void runOpMode() throws InterruptedException {
TestBot testBot = new TestBot(hardwareMap);

//Pose2d startPose = new Pose2d(12,-62, Math.toRadians(270)); // RED_BOARD
//Pose2d startPose = new Pose2d(-36,-62, Math.toRadians(270)); // RED_AUDIENCE
Pose2d startPose = new Pose2d(-36,-62, Math.toRadians(270)); // RED_AUDIENCE
//Pose2d startPose = new Pose2d(12,62, Math.toRadians(90)); // BLUE_BOARD
Pose2d startPose = new Pose2d(-36,62, Math.toRadians(90)); // BLUE_AUDIENCE
//Pose2d startPose = new Pose2d(-36,62, Math.toRadians(90)); // BLUE_AUDIENCE

testBot.getDrive().setPoseEstimate(startPose);

List<Component> componentList = new ArrayList<>();
Configuration config = Configurator.load();
if (config.placePixel) {
componentList.add(new PurplePixelComponent(testBot, TensorFlowDetection.PropPosition.LEFT));
componentList.add(new PurplePixelComponent(testBot, TensorFlowDetection.PropPosition.RIGHT));
}
if (Objects.equals(config.park, "outer")) {
componentList.add(new ParkingOut(testBot));
} else if (Objects.equals(config.park, "inner")) {
componentList.add(new ParkingIn(testBot));
} else if (Objects.equals(config.park, "board")) {
componentList.add(new GoToBoard(testBot));
componentList.add(new GoToBoard(testBot, TensorFlowDetection.PropPosition.RIGHT));
}

waitForStart();
Expand Down
82 changes: 61 additions & 21 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 @@ -9,10 +9,15 @@
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 GoToBoard extends Component{

public GoToBoard(Robot robot) {

TensorFlowDetection.PropPosition propPos;
public GoToBoard(Robot robot, TensorFlowDetection.PropPosition prop) {
super(robot);
propPos = prop;
}

@Override
Expand All @@ -24,41 +29,76 @@ public void drive() {

if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE) {
trajB = trajB.lineTo(new Vector2d(startPose.getX(), -12))
.lineTo(new Vector2d(46, -12))
.lineTo(new Vector2d(46, -36))

.addTemporalMarker(() -> {
android.util.Log.i("PLACE PIXEL", "Placed pixel at Red Board");
});
.lineTo(new Vector2d(46, -12));
if (propPos == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(46, -28.25));
} else if (propPos == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(46, -42.5));
} else {
trajB = trajB.lineTo(new Vector2d(46, -35));

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

} else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD) {
trajB = trajB.lineTo(new Vector2d(startPose.getX(), -12))
.lineTo(new Vector2d(46, -12))
.lineTo(new Vector2d(46, -36))

.addTemporalMarker(() -> {
android.util.Log.i("PLACE PIXEL", "Placed pixel at Red Board");
});
.lineTo(new Vector2d(46, -12));
if (propPos == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(46, -28.25));
} else if (propPos == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(46, -42.5));
} else {
trajB = trajB.lineTo(new Vector2d(46, -35));

}
trajB=trajB

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

} else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD) {
trajB = trajB.lineTo(new Vector2d(startPose.getX(), 12))
.lineTo(new Vector2d(46, 12))
.lineTo(new Vector2d(46, 36))
.lineTo(new Vector2d(46, 12));
if (propPos == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(46, 28.25));
} else if (propPos == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(46, 42.5));
} else {
trajB = trajB.lineTo(new Vector2d(46, 35));

}
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");
});
})
.turnTo(0);


}else {
trajB = trajB.lineTo(new Vector2d(startPose.getX(), 12))
.lineTo(new Vector2d(46, 12))
.lineTo(new Vector2d(46, 36))
.lineTo(new Vector2d(46, 12));
if (propPos == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(46, 28.25));
} else if (propPos == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(46, 42.5));
} else {
trajB = trajB.lineTo(new Vector2d(46, 35));
}
trajB=trajB

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


}

Expand Down
9 changes: 9 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
Expand Up @@ -122,6 +122,15 @@ public void drive() {
getRobot().dropPurplePixel(true);
})
.lineTo(new Vector2d(RED_AUDIENCE_TAPE_MARKS.getX(), RED_AUDIENCE_TAPE_MARKS.getY()));
} else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) {
trajB= trajB
.forward(-12 * turned)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
})
.lineTo(new Vector2d(RED_AUDIENCE_TAPE_MARKS.getX(), RED_AUDIENCE_TAPE_MARKS.getY()));




} else {
Expand Down
8 changes: 8 additions & 0 deletions ...ain/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -457,6 +457,14 @@ public TrajectorySequenceBuilder turn(double angle, double maxAngVel, double max
return this;
}

public TrajectorySequenceBuilder turnTo(double angle) {
return turnTo(angle, currentTurnConstraintMaxAngVel, currentTurnConstraintMaxAngAccel);
}

public TrajectorySequenceBuilder turnTo(double angle, double maxAngVel, double maxAngAccel) {
return turn(angle - lastPose.getHeading(), maxAngVel, maxAngAccel);
}

public TrajectorySequenceBuilder waitSeconds(double seconds) {
pushPath();
sequenceSegments.add(new WaitSegment(lastPose, seconds, Collections.emptyList()));
Expand Down

0 comments on commit 4afb1bb

Please sign in to comment.