Our tradition has been to release the source code for our robot every year, and so here it is: Robot code for the KwarQs (#2423) 2011 Robot, used in the Boston Regional.
This year we wrote our code in Python, running on the RobotPy python interpreter on the cRio. Compatible with the kitbot chassis, the robot uses CANJaguar for the arm motors, which has an elevator and a grabber arm that can be raised or lowered by the operator. Our autonomous mode is consistent -- but mechanical problems got in the way at the competition.
Probably the most interesting part of this is that a test harness is included (test.py) which tests some of the basic functionality of the robot code and runs through the steps for autonomous mode, using a 'fake' wpilib that allows the code to run on a normal windows machine with python 3 installed on it. Might be able to give you some ideas on how to test your own RobotPy code in python.
Link:
http://www.virtualroadside.com/FRC/#Kwarqs2011