-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
3ac191f
commit 0e0ff94
Showing
9 changed files
with
319 additions
and
65 deletions.
There are no files selected for viewing
Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.
Oops, something went wrong.
Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
218 changes: 218 additions & 0 deletions
218
TeamCode/src/main/java/org/thenuts/powerplay/opmode/auto/PassthruAuto.kt
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,218 @@ | ||
| package org.thenuts.powerplay.opmode.auto | ||
|
|
||
| import com.acmerobotics.roadrunner.geometry.Pose2d | ||
| import com.acmerobotics.roadrunner.geometry.Vector2d | ||
| import com.qualcomm.robotcore.eventloop.opmode.Autonomous | ||
| import org.thenuts.powerplay.acme.trajectorysequence.TrajectorySequenceBuilder | ||
| import org.thenuts.powerplay.game.Alliance | ||
| import org.thenuts.powerplay.game.Mode | ||
| import org.thenuts.powerplay.game.Signal | ||
| import org.thenuts.powerplay.opmode.CommandLinearOpMode | ||
| import org.thenuts.powerplay.opmode.commands.go | ||
| import org.thenuts.powerplay.subsystems.output.VerticalSlides | ||
| import org.thenuts.powerplay.subsystems.October | ||
| import org.thenuts.powerplay.subsystems.output.Output | ||
| import org.thenuts.switchboard.command.Command | ||
| import org.thenuts.switchboard.command.CommandScheduler | ||
| import org.thenuts.switchboard.dsl.mkSequential | ||
| import kotlin.math.PI | ||
| import kotlin.time.Duration.Companion.milliseconds | ||
|
|
||
| abstract class PassthruAuto(val right: Boolean) : CommandLinearOpMode<October>(::October, Alliance.RED, Mode.AUTO) { | ||
| lateinit var cmd: Command | ||
|
|
||
| override fun postInitHook() { | ||
| bot.vision?.front?.startDebug() | ||
| bot.vision?.signal?.enable() | ||
| bot.vision?.gamepad = gamepad1 | ||
|
|
||
| val reference = Pose2d(if (right) -36.0 else 36.0, 72.0, -PI/2.0) | ||
|
|
||
| fun vec2d(x: Double, y: Double): Vector2d { | ||
| if (REFERENCE) | ||
| return Vector2d(x, y).rotated(reference.heading) + reference.vec() | ||
| else return Vector2d(x, y) | ||
| } | ||
|
|
||
| fun pose2d(x: Double, y: Double, heading: Double): Pose2d { | ||
| if (REFERENCE) | ||
| return Pose2d(vec2d(x, y), heading + reference.heading) | ||
| else return Pose2d(x, y, heading) | ||
| } | ||
|
|
||
| fun heading(heading: Double): Double { | ||
| if (REFERENCE) | ||
| return heading + reference.heading | ||
| else return heading | ||
| } | ||
|
|
||
| val offset = 5.0 | ||
| val startPose = pose2d(0.0, offset, PI) | ||
| var intakePose = pose2d(if (right) 52.5 else 52.5, if (right) -21.0 else 20.25, if (right) PI/2.0 else -PI/2.0) | ||
| var outputPose = pose2d(if (right) 48.5 else 50.5, if (right) 11.0 else -12.0, if (right) PI else PI) | ||
| val MIDDLE = if (right) PI/2.0 else -PI/2.0 | ||
| val SIDE = if (right) -PI/2.0 else PI/2.0 | ||
| val cycleOffset = if (right) | ||
| pose2d(0.0, 0.0, 0.0) | ||
| else | ||
| pose2d(0.0, 2.75, 0.0) | ||
|
|
||
| bot.drive.poseEstimate = startPose | ||
| cmd = mkSequential { | ||
| // add(ExtendCommand(bot.output, scoreHeight, Output.OutputSide.SAMESIDE)) | ||
| task { bot.output.claw.state = Output.ClawState.CLOSED } | ||
| // delay(500.milliseconds) | ||
|
|
||
| go(bot.drive, startPose) { | ||
| setTangent(-PI/4.0) | ||
| splineToConstantHeading(vec2d(3.0, offset/2.0), heading(-PI/4.0)) | ||
| // back(3.0) | ||
| addDisplacementMarker { | ||
| bot.output.arm.state = Output.ArmState.MAX_UP | ||
| } | ||
| // if (right) { | ||
| // strafeRight(24.0 - offset) | ||
| // back(48.0) | ||
| // } else { | ||
| // strafeLeft(24.0 + offset) | ||
| // back(48.0) | ||
| // } | ||
| // strafeLeft(offset) | ||
| splineToConstantHeading(vec2d(8.0, 0.0), heading(0.0)) | ||
| splineToConstantHeading(vec2d(52.0, 0.0), heading(0.0)) | ||
| addDisplacementMarker { | ||
| bot.output.lift.state = | ||
| VerticalSlides.State.RunTo(VerticalSlides.Height.HIGH.pos) | ||
| } | ||
| splineToConstantHeading(outputPose.vec(), heading(MIDDLE)) | ||
| } | ||
|
|
||
| fun output(height: Int) { | ||
| task { | ||
| bot.output.lift.state = | ||
| VerticalSlides.State.RunTo(height) | ||
| } | ||
| task { bot.output.arm.state = Output.ArmState.CLEAR } | ||
| await { !bot.output.lift.isBusy } | ||
| // delay(1000.milliseconds) | ||
| task { bot.output.arm.state = Output.ArmState.PASSTHRU_OUTPUT } | ||
| delay(300.milliseconds) | ||
| // task { | ||
| // bot.output.lift.state = | ||
| // VerticalSlides.State.RunTo(VerticalSlides.Height.MID.pos) | ||
| // } | ||
| // await { !bot.output.lift.isBusy } | ||
| task { bot.output.claw.state = Output.ClawState.OPEN } | ||
| delay(150.milliseconds) | ||
| task { bot.output.arm.state = Output.ArmState.CLEAR } | ||
| delay(200.milliseconds) | ||
| } | ||
|
|
||
| output(VerticalSlides.Height.HIGH.pos) | ||
|
|
||
| repeat(2) { i -> | ||
| val stackHeight = 5 - i | ||
| // | ||
| // go(bot.drive, outputPose) { | ||
| // turnWrap(intakePose.heading - outputPose.heading) | ||
| // } | ||
|
|
||
| task { bot.output.lift.state = VerticalSlides.State.RunTo(0) } | ||
| await { !bot.output.isBusy } | ||
|
|
||
| task { bot.output.arm.state = Output.ArmState.values()[stackHeight - 1] } | ||
| await { !bot.output.isBusy } | ||
|
|
||
| go(bot.drive, outputPose/*.copy(heading = intakePose.heading)*/) { | ||
| setTangent((outputPose.heading + PI).angleWrap()) | ||
| splineTo(intakePose.vec(), heading(SIDE)) | ||
| } | ||
|
|
||
| task { bot.output.claw.state = Output.ClawState.CLOSED } | ||
| delay(500.milliseconds) | ||
|
|
||
| task { bot.output.lift.state = VerticalSlides.State.RunTo(VerticalSlides.Height.ABOVE_STACK.pos) } | ||
| await { !bot.output.lift.isBusy } | ||
| task { bot.output.arm.state = Output.ArmState.MAX_UP } | ||
|
|
||
|
|
||
| go(bot.drive, intakePose) { | ||
| // lineToLinearHeading(outputPose) | ||
| setTangent(heading(SIDE)) | ||
| splineTo(outputPose.vec(), outputPose.heading) | ||
| turnWrap(outputPose.heading - intakePose.heading) | ||
| } | ||
|
|
||
| intakePose += cycleOffset | ||
| outputPose += cycleOffset | ||
|
|
||
|
|
||
| output(VerticalSlides.Height.HIGH.pos) | ||
| } | ||
|
|
||
| switch({ bot.vision!!.signal.finalTarget }) { | ||
| value(Signal.LEFT) { | ||
| go(bot.drive, outputPose) { | ||
| setTangent(heading(SIDE)) | ||
| splineToConstantHeading(vec2d(51.0, 0.0), SIDE) | ||
| addDisplacementMarker { bot.output.lift.runTo(VerticalSlides.Height.MID.pos) } | ||
| splineToConstantHeading(vec2d(32.0, 0.0), PI) | ||
| splineToConstantHeading(vec2d(32.0, 23.0), PI/2.0) | ||
| splineToConstantHeading(vec2d(35.0, 23.0), 0.0) | ||
| } | ||
| } | ||
| value(Signal.MID) { | ||
| go(bot.drive, outputPose) { | ||
| setTangent(heading(SIDE)) | ||
| splineToConstantHeading(vec2d(51.0, 0.0), SIDE) | ||
| addDisplacementMarker { bot.output.lift.runTo(VerticalSlides.Height.MID.pos) } | ||
| splineToConstantHeading(vec2d(35.0, 0.0), PI) | ||
| } | ||
| } | ||
| value(Signal.RIGHT) { | ||
| go(bot.drive, outputPose) { | ||
| setTangent(heading(SIDE)) | ||
| splineToConstantHeading(vec2d(51.0, 0.0), SIDE) | ||
| addDisplacementMarker { bot.output.lift.runTo(VerticalSlides.Height.MID.pos) } | ||
| splineToConstantHeading(vec2d(32.0, 0.0), PI) | ||
| splineToConstantHeading(vec2d(32.0, -23.0), -PI/2.0) | ||
| splineToConstantHeading(vec2d(35.0, -23.0), 0.0) | ||
| } | ||
| } | ||
| } | ||
| task { bot.output.lift.runTo(0) } | ||
| delay(1000.milliseconds) | ||
| task { stop() } | ||
| } | ||
| } | ||
|
|
||
| override fun stopHook() { | ||
| bot.vision?.front?.stopDebug() | ||
| bot.vision?.signal?.disable() | ||
| } | ||
|
|
||
| override fun initLoopHook() { | ||
| log.out["signal"] = bot.vision?.signal?.finalTarget | ||
| } | ||
|
|
||
| override fun postStartHook() { | ||
| bot.vision?.front?.stopDebug() | ||
| bot.vision?.signal?.disable() | ||
| bot.vision?.gamepad = null | ||
| sched.addCommand(cmd) | ||
| } | ||
|
|
||
| override fun loopHook() { | ||
| log.out["COMMAND ORDER"] = sched.nodes.filterIsInstance<CommandScheduler.CommandNode>().map { it.runner.cmd::class.simpleName } | ||
| } | ||
|
|
||
| companion object { | ||
| @JvmField var REFERENCE = false | ||
| } | ||
| } | ||
|
|
||
| @Autonomous(preselectTeleOp = "OctoberTele", group = "_official") | ||
| class LeftPassthruAuto : PassthruAuto(false) | ||
|
|
||
| @Autonomous(preselectTeleOp = "OctoberTele", group = "_official") | ||
| class RightPassthruAuto : PassthruAuto(true) |
18 changes: 18 additions & 0 deletions
18
TeamCode/src/main/java/org/thenuts/powerplay/opmode/auto/ext.kt
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,18 @@ | ||
| package org.thenuts.powerplay.opmode.auto | ||
|
|
||
| import org.thenuts.powerplay.acme.trajectorysequence.TrajectorySequenceBuilder | ||
| import kotlin.math.PI | ||
|
|
||
|
|
||
| fun Double.angleWrap(): Double { | ||
| var x = this; | ||
| while (x > PI) { | ||
| x -= 2 * PI | ||
| } | ||
| while (x <= -PI) { | ||
| x += 2 * PI | ||
| } | ||
| return x | ||
| } | ||
|
|
||
| fun TrajectorySequenceBuilder.turnWrap(angle: Double) = turn(angle.angleWrap()) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,4 +1,4 @@ | ||
| ## TeamCode Module | ||
| x## TeamCode Module | ||
|
|
||
| Welcome! | ||
|
|
||
|
|
||
Oops, something went wrong.