Test code for FRC robot