Using a DS to control a FRC robot

[Robototes2412]

Hello all.

Inspired by this project: http://roboticjourney.blogspot.com/, me and my team have decided to try and make a Nintendo DS act as a robot base station/control board. I was wondering the following things:


How do we route the video to the ds?
How do we get the robot to interpret the button presses on the ds as raw buttons on a "joystick"?


please reply :)

[EricVanWyk]

Oddly enough, the very first DS was a DS.

That is, the first demo we gave used a Nintendo DS as the Driver Station. The touch screen makes for some really fun driving.

[Robototes2412]

can i please have a copy of that code? it would make this project exponentially easier

[EricVanWyk]

This was back before NI officially joined the project, so it is a touch out of date :(

The protocol is completely different, so it wouldn't really help at all. Not even sure where it is archived.

Cool project though, it should be fun!

[Robototes2412]

/me sadface

do you know where the code for the driver station is? preferably in some !labview, but I can *try* to parse it if labview is the only road.

[Alan Anderson]

It is possible to do what you want, but you will not find the path an easy one.

The Driver Station source code is not published. The Driver Station-to-Robot communication protocol is private information. It might be easier to implement your own controller than to figure out how to emulate the FRC Driver Station.

[davidthefat]

Now, you can try to capture the packets being sent and recieved and reverse engineer from that. But honestly that seems like an awful lot of work to do something like that.

Use this to capture the packets:
http://www.ethereal.com/


The easier thing to do is make drivers for the DS on the computer to use as a controller

[Robototes2412]

yeah, i'll probably go back to my original idea of using a ds to control an ardubot. thanks for the advice though ;)

[EHaskins]

After the 2009 season I attempted to reverse engineer the ds-robot, ds-fms, and fms-ds communication. Its not complete, but, IIRC, I had a proof of concept ds which snet static data to the robot.

My work is at http://frc1103dashboard.codeplex.com.

You may find some of it useful.

[Greg McKaskle]

The protocol isn't that complex, and a soft DS has been written by several people. If you can get the DS to display a jpg, that part will fall into place. The only issue is that this is close to kickoff, but otherwise, a very doable project.

Greg McKaskle

[Robototes2412]

I have code that does exactly that, i just have to set the image size to QQVGA(?) (160x120). Where do i send data to get the image to come back?

[andreboos]

The contents of each packet are listed in the FRCCommonControlData struct in the NetworkCommunication/FRCComm.h file of the WPILib source. Wireshark (a.k.a. Ethereal) might help with disassembling the packets. I've looked at the packets with Wireshark, but it's hard to extract useful information without a disassembler plugin. Does anyone know why the protocol is supposedly private/closed-source? I would think that FIRST would encourage exactly this kind of innovation.

[Peter Johnson]

The contents of each packet are listed in the FRCCommonControlData struct in the NetworkCommunication/FRCComm.h file of the WPILib source. Wireshark (a.k.a. Ethereal) might help with disassembling the packets. I've looked at the packets with Wireshark, but it's hard to extract useful information without a disassembler plugin. Does anyone know why the protocol is supposedly private/closed-source? I would think that FIRST would encourage exactly this kind of innovation.

I've done this, including the Robot->DS direction (which is notably not documented in FRCComm.h), as part of an effort to build a robot side simulator in Python (related to my RobotPy work). So far I have complete functionality of basic robot operation running on Python on a normal PC interoperating with the official DS. Still to be done is enhanced IO, but much of this is documented in the EnhancedIO WPILib headers. I'm planning on completing this, along with implementing a simple non-GUI DS for testing purposes, in the next week or so.

I have not yet published this work, partly because I too am wondering why FIRST has kept the Robot/DS protocol secret, particularly given how easy it is to reverse engineer :confused:. I can understand the desire to keep the FMS protocol secret (due to competition network security issues), but not the rest.

[Greg McKaskle]

As far as I know, the protocol was not meant to be private, but it is not documented either. Perhaps this is to keep things flexible from year to year. Perhaps there was some concern about safety.

The system watchdog will shut down the robot if the communications halts, but if a badly written DS continues sending enabled packets and ignores the driver, the robot is essentially out of control. Something to keep in mind if building a DS for robots of this size and speed.

Greg McKaskle

[Robototes2412]

I'm gonna have it control a servobot over a custom udp protocol

do you know how to grab images from the robot camera over the wireless?

[Greg McKaskle]

There are two choices for getting the images.

What I'd recommend is to put the camera on the external switch beside the cRIO. With that, you can make direct requests to the camera requesting a JPEG. The Axis website documents the syntax of the request -- it is part of the VAPIX API.

The way the camera has been used the last few years is to put it behind the cRIO soft-switch. The command to the camera is then made on the cRIO, and the contents are retransmitted to the DS.

There is actually a third way, involving making a pass-thru for the cRIO TCP stack, but again, I'd suggest the first approach.

Greg McKaskle

[kamocat]

For the system disable, I would suggest a deadman using the L button, so the robot is only enabled while the L button is pressed.

[Robototes2412]

so something like this:

while(keysDown() & KEY_L) {
scanKeys();
//do the funky robot controlling stoofe
}

[kamocat]

No, not quite.
You still want to *communicate* with the robot regardless of its state. Just use the L key to determine whether the robot is enabled or not.

[Robototes2412]

or then:

while (1) { //in nds programming, while (1) loops are common
scanKeys();
if (lButton()) {
enabled = 1;
}
if (enabled && upDPad()) {
send(dir_FORWARD);
}
if (enabled && downDPad()) {
send(dir_BACKWARD);
}
}


all of this assuming that enabled is a bool and that send is a void

[kamocat]

Could it be that the driver station simply sends a flattened string of this datatype?
http://content.screencast.com/users/kamocat/folders/Jing/media/78d184fb-6f9e-44b6-9d37-1a4baaab6f67/00000088.png

[Peter Johnson]

Could it be that the driver station simply sends a flattened string of this datatype?

Essentially, yes. The first 80 bytes of the packet are indeed the FRCCommonControlData structure. The packet size is 1024 bytes, however; the last 4 bytes is a CRC32 of the entire packet. Most of the rest of the packet is normally 0, but last year dynamic blocks were added into this space for the DriverStationEnhancedIO stuff. The CRC32 is the standard one found in zlib, and is calculated over the entire packet with the CRC32 field equal to 0.

The packet needs to be sent to port 1110 on the robot; the standard DS does this every 20 ms.

The robot->DS packet is quite different. I'll post the full info once I've double-checked it in my Python implementation.

[kamocat]

Well, here's what I've guessed so far:

name – datatype – length (bytes)
battery – sgl – 4
dsDigitalOut – u8 – 1
updateNumber – u8 – 1
userDataHigh – string – 0<980*
userDataHighLength – i32 – 4
userDataLow – string – 0<980*
userDataLowLength – i32 – 4
errorData – string – 0<980*
errorDataLength – i32 – 4


subtotal: 18

* the length of userDataHigh + userDataLow + errorData may be no greater than 980 bytes.

total: 980 + subtotal = 998

I know the total SHOULD be 1024, so this leaves 26 bytes unaccounted for.

[Robototes2412]

as far as a protocol for controlling the robot, what would you suggest?

my thoughts are sending signals like "f" for forward, etc.

[kamocat]

Essentially, yes. The first 80 bytes of the packet are indeed the FRCCommonControlData structure. The packet size is 1024 bytes, however; the last 4 bytes is a CRC32 of the entire packet. Most of the rest of the packet is normally 0, but last year dynamic blocks were added into this space for the DriverStationEnhancedIO stuff. The CRC32 is the standard one found in zlib, and is calculated over the entire packet with the CRC32 field equal to 0.

The packet needs to be sent to port 1110 on the robot; the standard DS does this every 20 ms.

The robot->DS packet is quite different. I'll post the full info once I've double-checked it in my Python implementation.
I looked up the CRC32 in zlib, and found it's actually Adler-32 (http://en.wikipedia.org/wiki/Adler-32).
It's really easy to make. (I've attached a LabVIEW implementation)

[kamocat]

as far as a protocol for controlling the robot, what would you suggest?

my thoughts are sending signals like "f" for forward, etc.
For controlling the robot, I would use the touchscreen. Send to the robot the location on the touchscreen. However, scale it to be like axis 1 and 2 of Joystick 1.

[Robototes2412]

ok, can you please include the code in line-based format? i cannot read labview files or code

[kamocat]

It's right on the wikipedia page (http://en.wikipedia.org/wiki/Adler-32).
#define MOD_ADLER 65521

unsigned long adler32(unsigned char *data, int len) /* where data is the location of the data in physical memory and len is the length of the data in bytes */
{
unsigned long a = 1, b = 0;
int index;

/* Process each byte of the data in order */
for (index = 0; index < len; ++index)
{
a = (a + data[index]) % MOD_ADLER;
b = (b + a) % MOD_ADLER;
}

return (b << 16) | a;
}

[Peter Johnson]

Well, here's what I've guessed so far:

<SNIP>

I know the total SHOULD be 1024, so this leaves 26 bytes unaccounted for.

Robot -> DS direction:
DS Dst Port 1150
1024 bytes (1152 if setUserDsLcdData() called)
Sent immediately after DS packet received (in response)

0x0: current control byte (echoed copy of FRCCommonControlData.control)
0x1: Battery voltage (integer portion in BCD)
0x37 if no code
0x2: Battery voltage (fractional portion in BCD)
0x37 if no code
0x3: DS Digital outputs
0x4-0x7: ?
0x8-0x9: teamID (big endian integer)
0xa-0xf: cRIO MAC address, in order, as bytes (e.g. 00,80,2f,11,4d,ac)
0x10-0x17: FPGA version? "06300800"
0x18-0x1d: ?
0x1e-0x1f: current command packet index (echoed copy of FRCCommonControlData.packetIndex)
0x20: User Data update number (from setStatusData())
0x21-0x24: User string length (32-bit big endian)
0x25 - X: User string - setStatusData() userDataHigh
X+1 - X+4: Error string length (32-bit big endian)
X+5 - Y: Error string - setErrorData()
Y+1 - Y+4: User data length (32-bit big endian)
Y+5 - 0x3d7: User data - setStatusData() userDataLow
0x3d8 - 0x3f7: overrideIOConfig() data (DSEIO status_block_t), followed by 0s
0x3f8 - 0x3fb: 0
0x3fc - 0x3ff: big endian CRC32 of 0-0x3ff (with 0 in CRC field)

If setUserDsLcdData() called:
0x400: 0x9f
0x401: 0xff
0x402 - 0x47f: LCD contents, padded with spaces (0x20)
split into 6 lines of 21 chars, top-to-bottom, left-to-right

This is copy and paste from my work in progress doc on the subject.

[kamocat]

For the DS->robot message, I'm uncertain about what to do with the last several elements in the FRCCommonControlData.
EDIT: to answer my own question, here are the values for the v20 image:



cRIO checksum (u64) - 0
FPGA checksum1 (u64) - 0
FPGA checksum2 (u64) - 0
vMonth1 (u8) - 49
vMonth2 (u8) - 48
vDay1 (u8) - 48
vDay2 (u8) - 50
vYear1 (u8) - 48
vYear2 (u8) - 56
vRev1 (u8) - 48
vRev2 (u8) - 48

[kamocat]

Essentially, yes. The first 80 bytes of the packet are indeed the FRCCommonControlData structure. The packet size is 1024 bytes, however; the last 4 bytes is a CRC32 of the entire packet. Most of the rest of the packet is normally 0, but last year dynamic blocks were added into this space for the DriverStationEnhancedIO stuff. The CRC32 is the standard one found in zlib, and is calculated over the entire packet with the CRC32 field equal to 0.

The packet needs to be sent to port 1110 on the robot; the standard DS does this every 20 ms.

The robot->DS packet is quite different. I'll post the full info once I've double-checked it in my Python implementation.

Well, I've tried this out. I can send and receive messages, but something isn't working.
I know it's receiving them, because I receive messages at exactly the rate I chose to send them. (This works down to 1 millisecond)

I think I'm missing the system watchdog.
I remember that the system watchdog works off TCP communication. That means if I want the robot to be enabled, I have to send another message in addition to these UDP datagrams. Do you have any information on that TCP message?

[Peter Johnson]

I think I'm missing the system watchdog.
I remember that the system watchdog works off TCP communication. That means if I want the robot to be enabled, I have to send another message in addition to these UDP datagrams. Do you have any information on that TCP message?

I don't see any TCP traffic in my testing so far. However, I do see occasional (every 3 seconds) UDP traffic between port 750 on the DS and port 17185 on the robot. 64 bytes are sent from the DS to the robot, and the robot responds with 36 bytes. I have not yet decoded the contents of these packets.

[kamocat]

Hmmm.
Perhaps we could check with the owner of this thread (http://www.chiefdelphi.com/forums/showthread.php?t=75431&highlight=software+driver+station).

[kamocat]

Well, I've tried this out. I can send and receive messages, but something isn't working.
I know it's receiving them, because I receive messages at exactly the rate I chose to send them. (This works down to 1 millisecond)

I think I'm missing the system watchdog.
I remember that the system watchdog works off TCP communication. That means if I want the robot to be enabled, I have to send another message in addition to these UDP datagrams. Do you have any information on that TCP message?
I tried decoding the messages I receive. I'm pretty sure I'm not sending the correct message, since I'm getting almost empty data in reply.
Here's the nonzero data that I get:

control bit: binary 01000000 (this does not change whether it's enabled or disabled, teleop or autonomous)
battery (integer): decimal 55 (same as 0x37)
battery (fractional): decimal 55
mac address: 0x00802F118478
FPGA version: 3474020444623351856
packetIndex: 33680 (this does not change with the packet number I send)


I'm assuming you've gotten this to work yourself.

So, what I'm going to do is upload a message that I might send, and ask you to check if it is valid by recreating the message using the settings I provide:
Alliance Position: Red1
Control state: Teleop disabled
Team ID: 8557
all joystick axis and buttons are zero.

[AustinSchuh]

Having gotten it working myself, I'd recommend trying to use your code that fills up a packet to recreate a packet that you get back from the bot. If you can recreate the packet from the inputs that the packet is begin given entirely from "scratch", it should work. I found that I wasn't checksumming right, and that was causing problems.

There is no TCP traffic.

[kamocat]

Having gotten it working myself, I'd recommend trying to use your code that fills up a packet to recreate a packet that you get back from the bot. If you can recreate the packet from the inputs that the packet is begin given entirely from "scratch", it should work. I found that I wasn't checksumming right, and that was causing problems.

There is no TCP traffic.
Perhaps I misunderstand you.
The robot sends back a different packet than it receives, using information other than that in the packet that is received.

Are you saying using this returned packet to fix my checksumming?

I suppose I could also open up the "Start DS Communication" VI and see if my packets are being received properly.

[AustinSchuh]

I'm saying that you should try to regenerate a packet that you snoop from the DS. This would be an "offline" test. Snoop packet A, deduce what inputs were used to generate A (through either code or by hand), feed those to your code, and then check to see if A = what you generated. This will let you know if you can generate a valid packet from known inputs. And valid is easy to check, since you know what the packet should be if you do all your math right.

[kamocat]

I'm saying that you should try to regenerate a packet that you snoop from the DS. This would be an "offline" test. Snoop packet A, deduce what inputs were used to generate A (through either code or by hand), feed those to your code, and then check to see if A = what you generated. This will let you know if you can generate a valid packet from known inputs. And valid is easy to check, since you know what the packet should be if you do all your math right.
Oh, I see. That's a good idea.

[kamocat]

So, I got everything in the packet correct, except for the last four bytes.
I think somethings wrong with my Alder-32 algorithm. (The checksum doesn't give me any hints to my error).
http://content.screencast.com/users/kamocat/folders/Jing/media/0dd15bed-efb2-4b8f-bf95-69033213ac0f/00000089.png
I've tested it with very short samples, but I don't have anything large enough to roll over the divisor.

Or, perhaps I'm wrong, and the DS actually uses CRC32.

[AustinSchuh]

Run your checksum including the last 4 bytes with the last 4 bytes set to 0.

[kamocat]

I've double-checked, and I am doing that.
Alder32 IS the correct checksum, isn't it?

[Peter Johnson]

I've uploaded my Python version to the RobotPy repo. http://git.tortall.net/cgit.cgi/RobotPy.git/tree/sim

The netcomm directory contains all the heavy lifting Python source:

frccomm.py: Packet data structures and packetizing/depacketizing.
robotcomm.py: Threaded UDP server implementation equivalent to the NetworkCommunication module on the cRIO.
dscomm.py: Threaded UDP client/server implementation equivalent to the Driver Station.


The dspy directory implements a very basic driver station with tkinter GUI.

Running robotcomm.py directly creates a very basic robot emulator (for testing).

I've done basic (enable/disable, etc) interoperability testing between these and the official DS and cRIO. However, a number of things are still untested/unimplemented, such as enhanced IO, resynchronization (switching between official DS and dspy), etc.

[kamocat]

Okay, guys.
I've checked my Adler32 algorithm and it functions correctly.
I've successfully made the robot think my computer is running the Driver Station by sending it a series of DS I've saved. (This confirms that I'm saving the data correctly)
I don't think I'm manipulating the data incorrectly, since it is such a simple manipulation.

This leaves me with the conclusion that the DS does not use Adler32. (Adler32 is the checksum used by zlib)
Then what does it use?
It's been referred to as a Cyclic Redundancy Check. CRC32 would make sense. Is it the IEEE standard, or is it something else?

[AustinSchuh]

If I'm reading my code right, it's a CRC with the polynomial = 0xEDB88320L. I'm not nearly as certain about that as I am about other parts.

[Peter Johnson]

This leaves me with the conclusion that the DS does not use Adler32. (Adler32 is the checksum used by zlib)
Then what does it use?
It's been referred to as a Cyclic Redundancy Check. CRC32 would make sense. Is it the IEEE standard, or is it something else?

No, it's not Adler32. It's CRC32. In Python I use zlib.crc32(). The zlib implementation can be found at http://www.stillhq.com/gpg/source-modified-1.0.3/zlib/crc32.html.

I believe http://www.libpng.org/pub/png/spec/1.2/PNG-CRCAppendix.html is a non-table-based version of the same algorithm, but I've not tested that implementation.

[kamocat]

Okay, so it's CRC-32-IEEE. Cool.
AustinSchuh, this means the polynomial is 0x4C11DB7.

Now I just need to fix my CRC32 algorithm.

[AustinSchuh]

Yes.

Austin Schuh

[Robototes2412]

I sincerely apologize for necroing this thread, but I have made progress on the robot control via a DS.

I have made a pygtk gui for Team 2-something's arduino robot interface.

NOTE:
I have not had the time to test this on any non debian-based os. I am assuming it will work where pygtk will though.

I have "broken ground" on a DS Gui for this, but have not included it in this prototype.

Please comment on any bugs, as I have tried to fix every glaring problem, but just realized i neglected to comment pretty much at all while making this beta.

Thank you all :)