frc-api/lib/driverstation.js

FRC Driver Station NodeJS API

/*global require, Buffer, console, module*/

var dgram = require('dgram');
var ev = require('events');
var crc32 = require('buffer-crc32');

var ESTOP_BIT = 0x40,
    ENABLED_BIT = 0x60;

function DriverStation() {
    'use strict';
	this.updateInterval = 20;
	this.isConnected = false;
	this.connection = new ev.EventEmitter();
	this.missed = 0;
    
    /**
     * Hex values are derived from the node-driverstation
     * npm module
     **/
	this.CONTROL = {
        packetIndex: 0x0,
		control: 0x44,
		dsDigitalIn: 0xff,
		teamID: 0x0,
		dsID_Alliance: 0x51,
		dsID_Position: 0x31,
		stick0Axes: null,
		stick0Buttons: null,
		stick1Axes: null,
		stick1Buttons: null,
		stick2Axes: null,
		stick2Buttons: null,
		stick3Axes: null,
		stick3Buttons: null,
		analog1: null,
		analog2: null,
		analog3: null,
		analog4: null
	};

	this.data = null;
}

DriverStation.prototype.enable = function () {
    'use strict';
	this.CONTROL.control = ENABLED_BIT;
};

DriverStation.prototype.estop = function () {
    'use strict';
	this.CONTROL.control = ESTOP_BIT;
};

DriverStation.prototype.waitForConnection = function () {
    'use strict';
    var self = this,
        server;
	this.findTimer = setInterval(function () {
		self.send();
	}, this.updateInterval * 50);
	
    //found connection! listen for updates
    
    server = dgram.createSocket("udp4");

	server.on("message", function (msg, rinfo) {
		if (this.findTimer) {
			clearInterval(this.findTimer);
			this.findTimer = null;
			this.connect();
			this.connection.emit("connected");
		}

		this.isConnected = true;
		this.connection.emit("data", this.parseData(msg));
		this.missed = 0;
	});

	server.bind(1150);
};

DriverStation.prototype.start = function (options) {
    'use strict';
    var t = String(options.teamID),
        first = t.substring(0, t.length - 2),
        second = t.substring(t.length - 2);
    
    first = (first === "") ? "0" : first;
    second = (second === "") ? "0" : second;
    
	this.ip = "10." + first + "." + second + ".2";
    
	this.alliance = options.alliance;
	this.position = options.position;

	this.CONTROL.teamID = options.teamID;

	this.client = dgram.createSocket("udp4");
	this.waitForConnection();
};

DriverStation.prototype.connect = function (cb) {
    'use strict';
    
	this.sendTimer = setInterval(function () {
		this.send();
		this.missed += 1;
	}, this.updateInterval);

	setTimeout(function () {
		if (!this.isConnected) {
			cb("No Robot Communication");
			this.disconnect();
		}
	}, this.updateInterval * 10); // 10 packets
};

DriverStation.prototype.disconnect = function (cb) {
    'use strict';
	clearInterval(this.sendTimer);
	this.isConnected = false;
};

DriverStation.prototype.parseData = function (data) {
    'use strict';
    //some of this is REALLY messy 
    var userDataHighLength = data.readInt32BE(0x21),
        userDataHigh = data.slice((0x21 + 0x4), (0x21 + 0x4) + userDataHighLength),
        userDataLow = data.slice(userDataHighLength + userDataHigh + 4,
                                 data.readInt32BE(userDataHighLength + userDataHigh + 4) + userDataHighLength + userDataHigh + 4);
	return {
		'control':            data.readUInt8(0x0),
		'batteryVolts':       12.00, /*not implemented yet*/
		'dsDigitalOut':       data.readUInt8(0x3),
		'teamID':             data.readUInt16BE(8),
		'macAddress':         data.toString('hex', 10, 16),
		'packetIndex':        data.readUInt16BE(30),
		'userDataHigh':       userDataHigh,
		'userDataLow':        userDataLow
	};
};

DriverStation.prototype.send = function () {
    'use strict';
	this.CONTROL.packetIndex += 1;

	var packet = new Buffer(1024);
	packet.fill(0x0);
    
    //based on bit values. any number of these may be wrong
    packet.writeUInt16BE(this.CONTROL.packetIndex, 0, 2);
	packet.writeUInt8(this.CONTROL.control, 2, 1);

	packet.writeUInt8(this.CONTROL.dsDigitalIn, 3, 1);
	packet.writeUInt16BE(this.CONTROL.teamID, 4, 2);

	packet.writeUInt8(this.CONTROL.dsID_Alliance, 6, 1);
	packet.writeUInt8(this.CONTROL.dsID_Position, 7, 1);

	packet.writeDoubleBE(this.CONTROL.stick0Axes, 8, 6);
	packet.writeUInt16BE(this.CONTROL.stick0Buttons, 14, 2);

	packet.writeDoubleBE(this.CONTROL.stick1Axes, 16, 6);
	packet.writeUInt16BE(this.CONTROL.stick1Buttons, 22, 2);

	packet.writeDoubleBE(this.CONTROL.stick2Axes, 24, 6);
	packet.writeUInt16BE(this.CONTROL.stick2Buttons, 30, 2);

	packet.writeDoubleBE(this.CONTROL.stick3Axes, 32, 6);
	packet.writeUInt16BE(this.CONTROL.stick3Buttons, 38, 2);

	packet.writeUInt16BE(this.CONTROL.analog1, 40, 2);
	packet.writeUInt16BE(this.CONTROL.analog2, 42, 2);
	packet.writeUInt16BE(this.CONTROL.analog3, 44, 2);
	packet.writeUInt16BE(this.CONTROL.analog4, 46, 2);

    this.client.send(packet, 0, packet.length, 1111, this.ip, function (err, bytes) {
		if (err) {
            console.err(err);
            this.disconnect();
            this.connection.emit('disconnected');
        }
	});

	if (this.missed > 10) {
		this.disconnect();
		this.missed = 0;

		this.connection.emit('disconnected');
		this.waitForConnection();
	}
};

module.exports = new DriverStation();