Troubleshooting

The robots have the bad habit of resetting.

Some commands that might be useful (I do not know what they do or if they are useful):

/* to avoid that the math trap causes a reset: */
void _trapISR_MathError(void)
{
   INTCON1bits.MATHERR = 0;	
}

Put this at the end of your Init function

if(RCONbits.POR) {	// reset if power on (some problem for few robots)
  RCONbits.POR=0;
  RESET();
}

Also, note that the robots do not have a fixed timestep (unlike the footbots). So timesteps can be shorter or longer according to what you are doing. You can use the agenda to overcome this problem, but I don't have any experience with that.