-
Notifications
You must be signed in to change notification settings - Fork 8
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
fixed some bugs when testing the odometry on real robot
now it's working, and also works for replay
- Loading branch information
Showing
9 changed files
with
160 additions
and
137 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 @@ | ||
| [] |
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
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
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
156 changes: 53 additions & 103 deletions
156
src/main/java/frc/robot/subsystems/drive/OdometryThread.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 |
|---|---|---|
| @@ -1,142 +1,92 @@ | ||
| // By 5516 Iron Maple https://github.com/Shenzhen-Robotics-Alliance/ | ||
| package frc.robot.subsystems.drive; | ||
|
|
||
| import com.ctre.phoenix6.BaseStatusSignal; | ||
| import com.ctre.phoenix6.StatusSignal; | ||
| import frc.robot.Constants; | ||
| import frc.robot.utils.MapleTimeUtils; | ||
| import frc.robot.Robot; | ||
| import org.littletonrobotics.junction.LogTable; | ||
| import org.littletonrobotics.junction.inputs.LoggableInputs; | ||
|
|
||
| import java.util.ArrayList; | ||
| import java.util.List; | ||
| import java.util.Queue; | ||
| import java.util.concurrent.ArrayBlockingQueue; | ||
| import java.util.concurrent.locks.Lock; | ||
| import java.util.concurrent.locks.ReentrantLock; | ||
| import java.util.function.Supplier; | ||
|
|
||
| public class OdometryThread extends Thread { | ||
| public static final class OdometryInput { | ||
| public interface OdometryThread { | ||
| class OdometryThreadInputs implements LoggableInputs { | ||
| public double[] measurementTimeStamps = new double[0]; | ||
|
|
||
| @Override | ||
| public void toLog(LogTable table) { | ||
| table.put("measurementTimeStamps", measurementTimeStamps); | ||
| } | ||
|
|
||
| @Override | ||
| public void fromLog(LogTable table) { | ||
| measurementTimeStamps = table.get("measurementTimeStamps", new double[0]); | ||
| } | ||
| } | ||
|
|
||
| class OdometryDoubleInput { | ||
| private final Supplier<Double> supplier; | ||
| private final Queue<Double> queue; | ||
| private Double[] valuesSincePreviousPeriod = new Double[0]; | ||
|
|
||
| public OdometryInput(Supplier<Double> signal) { | ||
| public OdometryDoubleInput(Supplier<Double> signal) { | ||
| this.supplier = signal; | ||
| this.queue = new ArrayBlockingQueue<>(Constants.ChassisConfigs.ODOMETRY_CACHE_CAPACITY); | ||
| } | ||
|
|
||
| public Double[] getValuesSincePreviousPeriod() { | ||
| return valuesSincePreviousPeriod; | ||
| } | ||
| } | ||
| private static final List<OdometryInput> registeredInputs = new ArrayList<>(); | ||
| private static final List<BaseStatusSignal> registeredStatusSignals = new ArrayList<>(); | ||
| public static OdometryInput registerInput(Supplier<Double> supplier) { | ||
| final OdometryInput odometryInput = new OdometryInput(supplier); | ||
| registeredInputs.add(odometryInput); | ||
| return odometryInput; | ||
| } | ||
| public static OdometryInput registerSignalInput(StatusSignal<Double> signal) { | ||
| signal.setUpdateFrequency(Constants.ChassisConfigs.ODOMETRY_FREQUENCY, Constants.ChassisConfigs.ODOMETRY_WAIT_TIMEOUT_SECONDS); | ||
| final OdometryInput odometryInput = new OdometryInput(signal.asSupplier()); | ||
| registeredStatusSignals.add(signal); | ||
| return odometryInput; | ||
| } | ||
|
|
||
| private static OdometryThread instance = null; | ||
| public static OdometryThread getInstance() { | ||
| if (instance == null) | ||
| instance = new OdometryThread( | ||
| registeredInputs.toArray(new OdometryInput[0]), | ||
| registeredStatusSignals.toArray(new BaseStatusSignal[0]) | ||
| ); | ||
| return instance; | ||
| } | ||
|
|
||
| private final OdometryInput[] odometryInputs; | ||
| private final BaseStatusSignal[] statusSignals; | ||
| private final Queue<Double> timeStampsQueue; | ||
| private Double[] timeStamps = new Double[0]; | ||
| private final Lock odometryLock = new ReentrantLock(); | ||
| public OdometryThread(OdometryInput[] odometryInputs, BaseStatusSignal[] statusSignals) { | ||
| this.timeStampsQueue = new ArrayBlockingQueue<>(Constants.ChassisConfigs.ODOMETRY_CACHE_CAPACITY); | ||
|
|
||
| this.odometryInputs = odometryInputs; | ||
| this.statusSignals = statusSignals; | ||
|
|
||
| setName("OdometryThread"); | ||
| setDaemon(true); | ||
| } | ||
| public void cacheInputToQueue() { | ||
| queue.offer(supplier.get()); | ||
| } | ||
|
|
||
| @Override | ||
| public synchronized void start() { | ||
| if (odometryInputs.length > 0) | ||
| super.start(); | ||
| public void fetchQueueToArray() { | ||
| valuesSincePreviousPeriod = mapQueueToArray(queue); | ||
| queue.clear(); | ||
| } | ||
| } | ||
|
|
||
| public static Double[] getOdometryTimeStamps() { | ||
| if (instance == null || !instance.isAlive()) | ||
| return new Double[0]; | ||
| return instance.getTimeStamps(); | ||
| List<OdometryDoubleInput> registeredInputs = new ArrayList<>(); | ||
| List<BaseStatusSignal> registeredStatusSignals = new ArrayList<>(); | ||
| static OdometryThread.OdometryDoubleInput registerSignalInput(StatusSignal<Double> signal) { | ||
| signal.setUpdateFrequency(Constants.ChassisConfigs.ODOMETRY_FREQUENCY, Constants.ChassisConfigs.ODOMETRY_WAIT_TIMEOUT_SECONDS); | ||
| registeredStatusSignals.add(signal); | ||
| return registerInput(signal.asSupplier()); | ||
| } | ||
| private Double[] getTimeStamps() { | ||
| return timeStamps; | ||
| static OdometryThread.OdometryDoubleInput registerInput(Supplier<Double> supplier) { | ||
| final OdometryThread.OdometryDoubleInput odometryDoubleInput = new OdometryThread.OdometryDoubleInput(supplier); | ||
| registeredInputs.add(odometryDoubleInput); | ||
| return odometryDoubleInput; | ||
| } | ||
|
|
||
| @Override | ||
| public void run() { | ||
| while (true) pollInputsInBackEnd(); | ||
| static double[] mapQueueToDoubleArray(Queue<Double> queue) { | ||
| return queue.stream().mapToDouble(value -> value).toArray(); | ||
| } | ||
|
|
||
| private void pollInputsInBackEnd() { | ||
| refreshSignalsAndBlockThread(); | ||
|
|
||
| odometryLock.lock(); | ||
| timeStampsQueue.offer(estimateAverageTimeStamps()); | ||
| for(OdometryInput odometryInput:odometryInputs) | ||
| odometryInput.queue.offer(odometryInput.supplier.get()); | ||
| odometryLock.unlock(); | ||
| } | ||
|
|
||
| private void refreshSignalsAndBlockThread() { | ||
| switch (Constants.ChassisConfigs.chassisType) { | ||
| case REV -> MapleTimeUtils.delay(1.0 / Constants.ChassisConfigs.ODOMETRY_FREQUENCY); | ||
| case CTRE_ON_RIO -> { | ||
| MapleTimeUtils.delay(1.0 / Constants.ChassisConfigs.ODOMETRY_FREQUENCY); | ||
| BaseStatusSignal.refreshAll(); | ||
| } | ||
| case CTRE_ON_CANIVORE -> BaseStatusSignal.waitForAll(Constants.ChassisConfigs.ODOMETRY_WAIT_TIMEOUT_SECONDS, statusSignals); | ||
| } | ||
| static Double[] mapQueueToArray(Queue<Double> queue) { | ||
| return queue.toArray(new Double[0]); | ||
| } | ||
|
|
||
| private double estimateAverageTimeStamps() { | ||
| double currentTime = MapleTimeUtils.getRealTimeSeconds(), totalLatency = 0; | ||
| for (BaseStatusSignal signal:statusSignals) | ||
| totalLatency += signal.getTimestamp().getLatency(); | ||
|
|
||
| if (statusSignals.length == 0) | ||
| return currentTime; | ||
| return currentTime - totalLatency / statusSignals.length; | ||
| } | ||
| static OdometryThread createInstance() { | ||
| if (Robot.CURRENT_ROBOT_MODE == Constants.RobotMode.REAL) | ||
| return new OdometryThreadReal( | ||
| registeredInputs.toArray(new OdometryThread.OdometryDoubleInput[0]), | ||
| registeredStatusSignals.toArray(new BaseStatusSignal[0]) | ||
| ); | ||
| return new OdometryThread() { | ||
| @Override public void updateInputs(OdometryThreadInputs inputs) {} | ||
| @Override public void start() {} | ||
| }; | ||
|
|
||
| public static void fetchOdometryDataSincePreviousRobotPeriod() { | ||
| if (instance != null && instance.isAlive()) | ||
| instance.fetchDataSincePreviousRobotPeriod(); | ||
| } | ||
| private void fetchDataSincePreviousRobotPeriod() { | ||
| odometryLock.lock(); | ||
| this.timeStamps = mapQueueToArray(timeStampsQueue); | ||
| timeStampsQueue.clear(); | ||
|
|
||
| for(OdometryInput odometryInput:odometryInputs) { | ||
| odometryInput.valuesSincePreviousPeriod = mapQueueToArray(odometryInput.queue); | ||
| odometryInput.queue.clear(); | ||
| } | ||
|
|
||
| odometryLock.unlock(); | ||
| } | ||
| void updateInputs(OdometryThreadInputs inputs); | ||
|
|
||
| private static Double[] mapQueueToArray(Queue<Double> queue) { | ||
| return queue.toArray(new Double[0]); | ||
| } | ||
| void start(); | ||
| } |
Oops, something went wrong.