-
Notifications
You must be signed in to change notification settings - Fork 3
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #47 from team4099/auto-scoring-with-zones
Auto scoring with zones
- Loading branch information
Showing
13 changed files
with
765 additions
and
36 deletions.
There are no files selected for viewing
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,74 @@ | ||
| { | ||
| "waypoints": [ | ||
| { | ||
| "anchorPoint": { | ||
| "x": 1.9, | ||
| "y": 4.97 | ||
| }, | ||
| "prevControl": null, | ||
| "nextControl": { | ||
| "x": 2.5159692897510797, | ||
| "y": 6.536703073406147 | ||
| }, | ||
| "holonomicAngle": 180.0, | ||
| "isReversal": false, | ||
| "velOverride": null, | ||
| "isLocked": false, | ||
| "isStopPoint": false, | ||
| "stopEvent": { | ||
| "names": [], | ||
| "executionBehavior": "parallel", | ||
| "waitBehavior": "none", | ||
| "waitTime": 0 | ||
| } | ||
| }, | ||
| { | ||
| "anchorPoint": { | ||
| "x": 4.513683285179071, | ||
| "y": 6.259812754000508 | ||
| }, | ||
| "prevControl": { | ||
| "x": 4.508526802268566, | ||
| "y": 6.264969236911013 | ||
| }, | ||
| "nextControl": { | ||
| "x": 4.518839768089576, | ||
| "y": 6.2546562710900035 | ||
| }, | ||
| "holonomicAngle": 179.6021606941461, | ||
| "isReversal": false, | ||
| "velOverride": null, | ||
| "isLocked": false, | ||
| "isStopPoint": false, | ||
| "stopEvent": { | ||
| "names": [], | ||
| "executionBehavior": "parallel", | ||
| "waitBehavior": "none", | ||
| "waitTime": 0 | ||
| } | ||
| }, | ||
| { | ||
| "anchorPoint": { | ||
| "x": 6.706650249237999, | ||
| "y": 4.908321691643384 | ||
| }, | ||
| "prevControl": { | ||
| "x": 6.655472288809161, | ||
| "y": 4.991216760707332 | ||
| }, | ||
| "nextControl": null, | ||
| "holonomicAngle": 0, | ||
| "isReversal": false, | ||
| "velOverride": null, | ||
| "isLocked": false, | ||
| "isStopPoint": false, | ||
| "stopEvent": { | ||
| "names": [], | ||
| "executionBehavior": "parallel", | ||
| "waitBehavior": "none", | ||
| "waitTime": 0 | ||
| } | ||
| } | ||
| ], | ||
| "markers": [] | ||
| } |
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,74 @@ | ||
| { | ||
| "waypoints": [ | ||
| { | ||
| "anchorPoint": { | ||
| "x": 7.06, | ||
| "y": 4.5 | ||
| }, | ||
| "prevControl": null, | ||
| "nextControl": { | ||
| "x": 6.06, | ||
| "y": 4.5 | ||
| }, | ||
| "holonomicAngle": 0, | ||
| "isReversal": false, | ||
| "velOverride": null, | ||
| "isLocked": false, | ||
| "isStopPoint": false, | ||
| "stopEvent": { | ||
| "names": [], | ||
| "executionBehavior": "parallel", | ||
| "waitBehavior": "none", | ||
| "waitTime": 0 | ||
| } | ||
| }, | ||
| { | ||
| "anchorPoint": { | ||
| "x": 3.13, | ||
| "y": 4.8 | ||
| }, | ||
| "prevControl": { | ||
| "x": 2.13, | ||
| "y": 4.8 | ||
| }, | ||
| "nextControl": { | ||
| "x": 2.13, | ||
| "y": 4.8 | ||
| }, | ||
| "holonomicAngle": 180.0, | ||
| "isReversal": true, | ||
| "velOverride": null, | ||
| "isLocked": false, | ||
| "isStopPoint": false, | ||
| "stopEvent": { | ||
| "names": [], | ||
| "executionBehavior": "parallel", | ||
| "waitBehavior": "none", | ||
| "waitTime": 0 | ||
| } | ||
| }, | ||
| { | ||
| "anchorPoint": { | ||
| "x": 1.9, | ||
| "y": 4.4 | ||
| }, | ||
| "prevControl": { | ||
| "x": 0.8999999999999999, | ||
| "y": 4.4 | ||
| }, | ||
| "nextControl": null, | ||
| "holonomicAngle": 180.0, | ||
| "isReversal": false, | ||
| "velOverride": null, | ||
| "isLocked": false, | ||
| "isStopPoint": false, | ||
| "stopEvent": { | ||
| "names": [], | ||
| "executionBehavior": "parallel", | ||
| "waitBehavior": "none", | ||
| "waitTime": 0 | ||
| } | ||
| } | ||
| ], | ||
| "markers": [] | ||
| } |
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,39 @@ | ||
| package com.team4099.lib.math | ||
|
|
||
| import org.team4099.lib.geometry.Pose2d | ||
| import org.team4099.lib.geometry.Translation2d | ||
| import org.team4099.lib.units.base.inMeters | ||
| import org.team4099.lib.units.base.meters | ||
|
|
||
| class Zone2d(var vertices: List<Translation2d>) { | ||
| fun containsPose(pose: Pose2d): Boolean { | ||
| var isInside = false | ||
| val minX = vertices.minOf { it.x } | ||
| val minY = vertices.minOf { it.y } | ||
| val maxX = vertices.maxOf { it.x } | ||
| val maxY = vertices.maxOf { it.y } | ||
|
|
||
| if ((pose.x !in minX..maxX) || (pose.y !in minY..maxY)) { | ||
| return false | ||
| } | ||
|
|
||
| var vBIndex = vertices.size - 1 | ||
| for (vAIndex in vertices.indices) { | ||
| if ((vertices[vAIndex].y > pose.y) != (vertices[vBIndex].y > pose.y) && | ||
| pose.x < | ||
| ( | ||
| (vertices[vBIndex].x - vertices[vAIndex].x).inMeters * | ||
| (pose.y - vertices[vAIndex].y).inMeters / | ||
| (vertices[vBIndex].y - vertices[vAIndex].y).inMeters + | ||
| vertices[vAIndex].x.inMeters | ||
| ) | ||
| .meters | ||
| ) { | ||
| isInside = !isInside | ||
| } | ||
| vBIndex = vAIndex | ||
| } | ||
|
|
||
| return isInside | ||
| } | ||
| } |
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 |
|---|---|---|
| @@ -0,0 +1,15 @@ | ||
| package com.team4099.robot2022 | ||
|
|
||
| /** | ||
| * Automatically generated file containing build version information. | ||
| */ | ||
| const val MAVEN_GROUP = "" | ||
| const val MAVEN_NAME = "ChargedUp-2023" | ||
| const val VERSION = "unspecified" | ||
| const val GIT_REVISION = 11 | ||
| const val GIT_SHA = "f62076f27df05c9da57e75b873eaa1d38d512be4" | ||
| const val GIT_DATE = "2022-12-13T19:13:28Z" | ||
| const val GIT_BRANCH = "autologannotation" | ||
| const val BUILD_DATE = "2023-07-19T22:09:16Z" | ||
| const val BUILD_UNIX_TIME = 1689822556755L | ||
| const val DIRTY = 0 |
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
89 changes: 89 additions & 0 deletions
89
src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.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,89 @@ | ||
| package com.team4099.robot2023.commands | ||
|
|
||
| import com.team4099.lib.math.Zone2d | ||
| import com.team4099.lib.trajectory.Waypoint | ||
| import com.team4099.robot2023.commands.drivetrain.DrivePathCommand | ||
| import com.team4099.robot2023.config.constants.FieldConstants | ||
| import com.team4099.robot2023.config.constants.GamePiece | ||
| import com.team4099.robot2023.config.constants.NodeTier | ||
| import com.team4099.robot2023.config.constants.WaypointConstants | ||
| import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain | ||
| import com.team4099.robot2023.subsystems.superstructure.Superstructure | ||
| import com.team4099.robot2023.util.AllianceFlipUtil | ||
| import edu.wpi.first.wpilibj2.command.Commands.runOnce | ||
| import edu.wpi.first.wpilibj2.command.SequentialCommandGroup | ||
| import org.team4099.lib.geometry.Pose2d | ||
| import org.team4099.lib.units.base.meters | ||
| import org.team4099.lib.units.derived.Angle | ||
| import org.team4099.lib.units.derived.degrees | ||
| import org.team4099.lib.units.derived.inRotation2ds | ||
| import org.team4099.lib.units.perSecond | ||
|
|
||
| class AutoIntakeCommand(val drivetrain: Drivetrain, val superstructure: Superstructure) : | ||
| SequentialCommandGroup() { | ||
| lateinit var drivePose: Pose2d | ||
| lateinit var intermediaryWaypoints: List<Waypoint> | ||
| var currentZone: Zone2d? = null | ||
| lateinit var finalPose: Pose2d | ||
| lateinit var postAlignPose: Pose2d | ||
| var heading: Angle = 0.0.degrees | ||
| lateinit var gamePiece: GamePiece | ||
| lateinit var nodeTier: NodeTier | ||
|
|
||
| init { | ||
| val setupCommand = | ||
| runOnce({ | ||
| drivePose = drivetrain.odometryPose | ||
| heading = drivetrain.fieldVelocity.heading | ||
| currentZone = FieldConstants.determineZone(FieldConstants.Zones.allZones, drivePose) | ||
| println(currentZone) | ||
| intermediaryWaypoints = | ||
| WaypointConstants.SubstationPaths.getPath(currentZone).map { | ||
| AllianceFlipUtil.apply(it) | ||
| } | ||
| if (intermediaryWaypoints.isEmpty()) { | ||
| intermediaryWaypoints = | ||
| listOf<Waypoint>( | ||
| Waypoint( | ||
| drivePose.pose2d.translation, | ||
| if (drivetrain.fieldVelocity.magnitude.absoluteValue < | ||
| 0.25.meters.perSecond | ||
| ) | ||
| null | ||
| else heading.inRotation2ds, | ||
| drivePose.rotation.inRotation2ds | ||
| ) | ||
| ) | ||
| } | ||
|
|
||
| for (vertex in FieldConstants.Zones.farLeftLane.vertices) { | ||
| println("${vertex.x}, ${vertex.y}") | ||
| } | ||
| println("------") | ||
| for (vertex in FieldConstants.Zones.closeLeftLane.vertices) { | ||
| println("${vertex.x}, ${vertex.y}") | ||
| } | ||
| }) | ||
|
|
||
| addCommands( | ||
| setupCommand, | ||
| DrivePathCommand( | ||
| drivetrain, | ||
| { | ||
| listOf( | ||
| Waypoint( | ||
| drivePose.pose2d.translation, | ||
| if (drivetrain.fieldVelocity.magnitude.absoluteValue < 0.25.meters.perSecond) | ||
| null | ||
| else heading.inRotation2ds, | ||
| drivePose.rotation.inRotation2ds | ||
| ) | ||
| ) + intermediaryWaypoints | ||
| }, | ||
| keepTrapping = true, | ||
| flipForAlliances = false | ||
| ), | ||
| superstructure.groundIntakeConeCommand() | ||
| ) | ||
| } | ||
| } |
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
Oops, something went wrong.