forked from FIRST-Tech-Challenge/FtcRobotController
-
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
Showing
9 changed files
with
192 additions
and
11 deletions.
There are no files selected for viewing
20 changes: 20 additions & 0 deletions
20
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DownsampledWriter.java
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,20 @@ | ||
| package org.firstinspires.ftc.teamcode; | ||
|
|
||
| import com.acmerobotics.roadrunner.ftc.FlightRecorder; | ||
|
|
||
| public class DownsampledWriter { | ||
| public final String channel; | ||
| public final long maxPeriod; | ||
| private long nextWriteTimestamp = System.nanoTime(); | ||
| public DownsampledWriter(String channel, long maxPeriod) { | ||
| this.channel = channel; | ||
| this.maxPeriod = maxPeriod; | ||
| } | ||
| public void write(Object msg) { | ||
| long now = System.nanoTime(); | ||
| if (now >= nextWriteTimestamp) { | ||
| nextWriteTimestamp = now + maxPeriod; | ||
| FlightRecorder.write(channel, msg); | ||
| } | ||
| } | ||
| } |
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
24 changes: 24 additions & 0 deletions
24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/DriveCommandMessage.java
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,24 @@ | ||
| package org.firstinspires.ftc.teamcode.messages; | ||
|
|
||
| import com.acmerobotics.roadrunner.PoseVelocity2dDual; | ||
| import com.acmerobotics.roadrunner.Time; | ||
|
|
||
| public final class DriveCommandMessage { | ||
| public long timestamp; | ||
| public double forwardVelocity; | ||
| public double forwardAcceleration; | ||
| public double lateralVelocity; | ||
| public double lateralAcceleration; | ||
| public double angularVelocity; | ||
| public double angularAcceleration; | ||
|
|
||
| public DriveCommandMessage(PoseVelocity2dDual<Time> poseVelocity) { | ||
| this.timestamp = System.nanoTime(); | ||
| this.forwardVelocity = poseVelocity.linearVel.x.get(0); | ||
| this.forwardAcceleration = poseVelocity.linearVel.x.get(1); | ||
| this.lateralVelocity = poseVelocity.linearVel.y.get(0); | ||
| this.lateralAcceleration = poseVelocity.linearVel.y.get(1); | ||
| this.angularVelocity = poseVelocity.angVel.get(0); | ||
| this.angularAcceleration = poseVelocity.angVel.get(1); | ||
| } | ||
| } |
27 changes: 27 additions & 0 deletions
27
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumEncodersMessage.java
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,27 @@ | ||
| package org.firstinspires.ftc.teamcode.messages; | ||
|
|
||
| import com.acmerobotics.roadrunner.ftc.PositionVelocityPair; | ||
|
|
||
| public final class MecanumEncodersMessage { | ||
| public long timestamp; | ||
| public int rawLeftFrontPosition; | ||
| public int rawLeftFrontVelocity; | ||
| public int rawLeftBackPosition; | ||
| public int rawLeftBackVelocity; | ||
| public int rawRightBackPosition; | ||
| public int rawRightBackVelocity; | ||
| public int rawRightFrontPosition; | ||
| public int rawRightFrontVelocity; | ||
|
|
||
| public MecanumEncodersMessage(PositionVelocityPair leftFront, PositionVelocityPair leftBack, PositionVelocityPair rightBack, PositionVelocityPair rightFront) { | ||
| this.timestamp = System.nanoTime(); | ||
| this.rawLeftFrontPosition = leftFront.rawPosition; | ||
| this.rawLeftFrontVelocity = leftFront.rawVelocity; | ||
| this.rawLeftBackPosition = leftBack.rawPosition; | ||
| this.rawLeftBackVelocity = leftBack.rawVelocity; | ||
| this.rawRightBackPosition = rightBack.rawPosition; | ||
| this.rawRightBackVelocity = rightBack.rawVelocity; | ||
| this.rawRightFrontPosition = rightFront.rawPosition; | ||
| this.rawRightFrontVelocity = rightFront.rawVelocity; | ||
| } | ||
| } |
2 changes: 1 addition & 1 deletion
2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/PoseMessage.java
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
29 changes: 29 additions & 0 deletions
29
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankEncodersMessage.java
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,29 @@ | ||
| package org.firstinspires.ftc.teamcode.messages; | ||
|
|
||
| import com.acmerobotics.roadrunner.ftc.PositionVelocityPair; | ||
|
|
||
| import java.util.List; | ||
|
|
||
| public final class TankEncodersMessage { | ||
| public long timestamp; | ||
| public int[] rawLeftPositions; | ||
| public int[] rawLeftVelocities; | ||
| public int[] rawRightPositions; | ||
| public int[] rawRightVelocities; | ||
|
|
||
| public TankEncodersMessage(List<PositionVelocityPair> left, List<PositionVelocityPair> right) { | ||
| this.timestamp = System.nanoTime(); | ||
| this.rawLeftPositions = new int[left.size()]; | ||
| this.rawLeftVelocities = new int[left.size()]; | ||
| this.rawRightPositions = new int[right.size()]; | ||
| this.rawRightVelocities = new int[right.size()]; | ||
| for (int i = 0; i < left.size(); i++) { | ||
| this.rawLeftPositions[i] = left.get(i).rawPosition; | ||
| this.rawLeftVelocities[i] = left.get(i).rawVelocity; | ||
| } | ||
| for (int i = 0; i < right.size(); i++) { | ||
| this.rawRightPositions[i] = right.get(i).rawPosition; | ||
| this.rawRightVelocities[i] = right.get(i).rawVelocity; | ||
| } | ||
| } | ||
| } |
23 changes: 23 additions & 0 deletions
23
.../src/main/java/org/firstinspires/ftc/teamcode/messages/ThreeDeadWheelEncodersMessage.java
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,23 @@ | ||
| package org.firstinspires.ftc.teamcode.messages; | ||
|
|
||
| import com.acmerobotics.roadrunner.ftc.PositionVelocityPair; | ||
|
|
||
| public final class ThreeDeadWheelEncodersMessage { | ||
| public long timestamp; | ||
| public int rawPar0Position; | ||
| public int rawPar0Velocity; | ||
| public int rawPar1Position; | ||
| public int rawPar1Velocity; | ||
| public int rawPerpPosition; | ||
| public int rawPerpVelocity; | ||
|
|
||
| public ThreeDeadWheelEncodersMessage(PositionVelocityPair par0, PositionVelocityPair par1, PositionVelocityPair perp) { | ||
| this.timestamp = System.nanoTime(); | ||
| this.rawPar0Position = par0.rawPosition; | ||
| this.rawPar0Velocity = par0.rawVelocity; | ||
| this.rawPar1Position = par1.rawPosition; | ||
| this.rawPar1Velocity = par1.rawVelocity; | ||
| this.rawPerpPosition = perp.rawPosition; | ||
| this.rawPerpVelocity = perp.rawVelocity; | ||
| } | ||
| } |
19 changes: 19 additions & 0 deletions
19
...de/src/main/java/org/firstinspires/ftc/teamcode/messages/TwoDeadWheelEncodersMessage.java
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,19 @@ | ||
| package org.firstinspires.ftc.teamcode.messages; | ||
|
|
||
| import com.acmerobotics.roadrunner.ftc.PositionVelocityPair; | ||
|
|
||
| public final class TwoDeadWheelEncodersMessage { | ||
| public long timestamp; | ||
| public int rawParPosition; | ||
| public int rawParVelocity; | ||
| public int rawPerpPosition; | ||
| public int rawPerpVelocity; | ||
|
|
||
| public TwoDeadWheelEncodersMessage(PositionVelocityPair par, PositionVelocityPair perp) { | ||
| this.timestamp = System.nanoTime(); | ||
| this.rawParPosition = par.rawPosition; | ||
| this.rawParVelocity = par.rawVelocity; | ||
| this.rawPerpPosition = perp.rawPosition; | ||
| this.rawPerpVelocity = perp.rawVelocity; | ||
| } | ||
| } |