ewpratten.com/_drafts/2020-01-14-ZEBRA-Darts.md

layout	title	description	date	categories	redirect_from
post	Interpolating motor control commands from Dart data	And other weird things to do with XY datasets	2020-01-14 20:13:00	frc data	/post/eb3v140d/ /eb3v140d/

At some FRC events, ZEBRA Technologies places dart trackers for their MotionWorks 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. I recived the tracking data in the following CSV format:

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:

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:

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:

# 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.

To do this, all we need is a little math:

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'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