90 lines
3.0 KiB
Markdown
90 lines
3.0 KiB
Markdown
---
|
|
layout: post
|
|
title: "Interpolating motor control commands from Dart data"
|
|
description: "And other weird things to do with XY datasets"
|
|
date: 2020-01-14 20:13:00
|
|
categories: frc data
|
|
redirect_from:
|
|
- /post/eb3v140d/
|
|
- /eb3v140d/
|
|
---
|
|
|
|
At some FRC events, [ZEBRA Technologies](https://www.zebra.com/us/en.html) places dart trackers for their [MotionWorks](https://www.zebra.com/us/en/solutions/intelligent-edge-solutions/rtls.html) system on robots. They then provide real-time motion tracking data for all bots on the field. I got my hands on a few data archives from various events in 2019 that used the darts. Here is a little post about what I have been able to do with this data.
|
|
|
|
## The data
|
|
For this post, I will be working with data from [Chezy Champs 2019](https://chezychamps.com/). I recived the tracking data in the following CSV format:
|
|
|
|
```csv
|
|
X position (Feet), Y position (Feet), UNIX epoch time (UTC)
|
|
```
|
|
|
|
## Data analysis
|
|
|
|
I wrote a little parser that converts the CSV data to pose data. A pose is a vector containing the following field-absolute components:
|
|
|
|
```c++
|
|
struct Pose {
|
|
double x;
|
|
double y;
|
|
double theta;
|
|
}
|
|
```
|
|
|
|
While we are at it, Ill define a "Transpose", which is like a pose, but relative to other poses, and a "ChassisSpeed" vector:
|
|
|
|
```c++
|
|
struct ChassisSpeed {
|
|
double left;
|
|
double right;
|
|
}
|
|
|
|
class Transpose {
|
|
private:
|
|
double x;
|
|
double y;
|
|
double theta;
|
|
|
|
public:
|
|
ChassisSpeed fromTranspose();
|
|
}
|
|
```
|
|
|
|
### Converting a difference to a Transpose
|
|
|
|
The only data we can directly read from the Dart data is poses over time (all poses have an angle of 0 degrees). To convert these to Transposes, we need to do a little math. Basically, we can take two poses, then calculate their differences:
|
|
|
|
```python
|
|
# Find the differences in coords
|
|
dx: float = now.x - last.x
|
|
dy: float = now.y - last.y
|
|
|
|
# Calculate a heading from translation
|
|
theta: float = atan2(dx, dy)
|
|
```
|
|
|
|
Now, with this data, we can build a Transpose out of any pair of data points.
|
|
|
|
### Converting a transpose to a ChassisSpeed
|
|
|
|
To be completely honest, I originally had no idea what I was going to use this data for, but ended up deciding to try to reconstruct motor data from the position data. This is just a form of [Inverse Kinematics](https://en.wikipedia.org/wiki/Inverse_kinematics).
|
|
|
|
To do this, all we need is a little math:
|
|
|
|
```java
|
|
ChassisSpeed fromTranspose(){
|
|
double delta = TRACK_WIDTH * this.theta / 2 * 1.0469745223;
|
|
return (new ChassisSpeed(this.x - deltaV, this.x + deltaV);
|
|
}
|
|
```
|
|
|
|
This will calculate the heading difference between 0,0 and the transpose, then apply that to the X vector as a wheel velocity. `TRACK_WIDTH` is the width of the robot drivebase in inches (I used `25.4` because that is team [254](https://www.thebluealliance.com/team/254)'s track width)
|
|
|
|
#### Chassis speeds over time
|
|
|
|
Here is a little graph of the calculated chassis speeds over time. (Keeping in mind that inaccuracies in the Dart system cause some "drift")
|
|
|
|
|
|
|
|
### Extrapolating further
|
|
|