Turret fop #154
Turret fop #154
Conversation
This reverts commit d19f490.
| @@ -43,6 +43,8 @@ suspend fun Subsystems.digestionTeleop() = startChoreo("Digestion Teleop") { | |||
|
|
|||
| val rezeroTurret by operator.rezeroTurret.readEagerly().withoutStamps | |||
| val reindexCarousel by operator.reindexCarousel.readEagerly().withoutStamps | |||
| val track by operator.trackTarget.readEagerly().withoutStamps | |||
There was a problem hiding this comment.
rename to smth slightly more descriptive
| @@ -79,6 +80,39 @@ suspend fun TurretComponent.trackTarget(limelight: LimelightComponent) = startRo | |||
| } | |||
| } | |||
|
|
|||
| suspend fun TurretComponent.fieldOriented(drivetrain: DrivetrainComponent, limelight: LimelightComponent, turret: TurretComponent) { | |||
There was a problem hiding this comment.
turret doesn't need to be a paremeter, the function is an extension function of TurretComponent
|
|
||
| controller { | ||
| if (lastLimelightPos != null) { | ||
| val pitch by drivetrain.hardware.pitch.readEagerly().withoutStamps |
There was a problem hiding this comment.
move sensor stuff to before the controller block
| @@ -84,6 +86,10 @@ suspend fun Subsystems.digestionTeleop() = startChoreo("Digestion Teleop") { | |||
|
|
|||
| { rezeroTurret } to { turret?.rezero(electrical) ?: freeze() }, | |||
| { reindexCarousel } to { whereAreMyBalls() }, | |||
| {track} to { | |||
| trackTargetTurret() | |||
There was a problem hiding this comment.
you can just do turret?.fieldOriented(drivetrain, limelight) ?: freeze() and get rid of the trackTargetTurret function
| if(reading != null) lastLimelightPos = reading | ||
|
|
||
| controller { | ||
| if (lastLimelightPos != null) { |
There was a problem hiding this comment.
make a copy of lastLimelightPos so you don't have to use !! later in line 95 (what if lastLimlightPos changes to null while this routine is running?)
| val position = limelight.hardware.conversions.outerGoalPosition(lastLimelightPos!!, bearing, pitch) | ||
| val angle = atan2(position.x - drivetrainPosition.x, position.y - drivetrainPosition.y) | ||
| val target = (angle `coterminal +` drivetrainPosition.bearing) | ||
| if (abs(target) > 2.Degree) { |
There was a problem hiding this comment.
if we stop driving and aim to fire then we'll have up to 2 degrees of error D:
| import info.kunalsheth.units.generated.* | ||
|
|
||
| class TurretComponent(hardware: TurretHardware) : | ||
| Component<TurretComponent, TurretHardware, OffloadedOutput>(hardware, Subsystems.shooterTicker) { | ||
|
|
||
| val safeSpeed by pref(1, Volt) | ||
| val windupPosition by pref(-90, Degree) | ||
| var lastLimelightPos: LimelightReading? = null |
Code works now.
Button press --> tracks until any other turret routine is called.
when button pressed, looks for new target and tracks that.