ConnectionDetection.c

#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Motor,  mtr_S1_C1_1,     motorA,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorB,        tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

void enterFailureMode()
{
  motor[motorA] = 0;
  motor[motorB] = 0;
}

void exitFailureMode()
{
}


bool kInFailureMode = false;

task checkConnectivity()
{
  int missedMessageCount = 0;
  long lastMessageCount = 0;
  
  while(true) {
    if (ntotalMessageCount == lastMessageCount) {
      if (++missedMessageCount > 500) {
        // The total message count hasn't incremented for more than five
        // iterations of the while loop.
        kInFailureMode = true;
        enterFailureMode();
      }
    }
    else { // The total message count changed, we have connection!
      if (kInFailureMode)
        exitFailureMode();
      
      kInFailureMode = false;
      missedMessageCount = 0;
    }
    
    lastMessageCount = ntotalMessageCount;
  }
}


void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  disableDiagnosticsDisplay();
  
  return;
}

task main()
{
  initializeRobot();

  waitForStart();   // wait for start of tele-op phase

  StartTask(checkConnectivity);
  
  while (true) {
	  getJoystickSettings(joystick);
	  
	  if (!kInFailureMode) {
  	  motor[motorA] = joystick.joy1_x1;
	    motor[motorB] = joystick.joy1_y1;
	  }
  }
}