///////////////////////////////////////////////////////////////////////////////////////
//
//    Program for the RoboDog
//		This is a modified program of the 'Motor Synchronization' demo program in RbotC
//
///////////////////////////////////////////////////////////////////////////////////////

int object        = 0;

task LOOK_FOR_OBJECT()
{
	while(true)
	{
		while(SensorValue[S4] > 50){}
		object = 1;
		while(object == 1){}
	}

}



task main()
{

	SensorType[S4] = sensorSONAR9V;
	StartTask(LOOK_FOR_OBJECT);
  int nMoveSize     = 1200;    // One full step
  int nSpeed        = 100;
  int nDelay        = 1800;
  int nTurnRatio    = 100;

  const TSynchedMotors kSyncType     = synchAC;
  const tMotor         kPrimaryMotor = motorA;
  //
  // Setup the motor configuration
  //
  bFloatDuringInactiveMotorPWM = false;

  nMotorEncoder[kPrimaryMotor]       = 0;
  nMotorEncoderTarget[kPrimaryMotor] = 0;

  //
  // Enable synchronization between two motors.
  //     -- motor C will be the 'primary' motor
  //     -- motor A will be the slave motor
  //     -- Turn ratio +100% -- same direction and same speed as primary
  //                      0  -- stopped
  //                   -100% -- reversed direct and same speed as primary
  //
  nSyncedMotors            = kSyncType;  // "C" will be synchronized to "A".
  nSyncedTurnRatio         = nTurnRatio;
  nPidUpdateInterval       = 10;   // Best performance if we do really frequent updates
                                     // (i.e. calculation) on the motor speeds to correct
                                     // for errors.



while(true)

{

	   nSpeed        = 100;
  	 nTurnRatio    = 100;


  	nSyncedMotors            = synchCA;  // Reverse: "C" will be synchronized to "A".
  	nSyncedTurnRatio         = nTurnRatio;

    nMotorEncoderTarget[motorC] 	= nMoveSize; // incremental amount to move motor
    motor[motorC]                 = nSpeed;    // motor speed
    wait1Msec(nDelay);                       // wait for action to complete

    nSyncedMotors            = kSyncType;  // Reset: "C" will be synchronized to "A".
	  nSyncedTurnRatio         = nTurnRatio;

  while(object == 0)
  {

    nMotorEncoderTarget[kPrimaryMotor] = nMoveSize; // incremental amount to move motor
    motor[kPrimaryMotor]               = nSpeed;    // motor speed
    wait1Msec(nDelay);                       // wait for action to complete

  	nSyncedMotors            = synchCA;
  	nSyncedTurnRatio         = nTurnRatio;

    nMotorEncoderTarget[motorC] 	= nMoveSize;
    motor[motorC]                 = nSpeed;
    wait1Msec(nDelay);

    nSyncedMotors            = kSyncType;
	  nSyncedTurnRatio         = nTurnRatio;
  }

		nSpeed        = -100;

repeat(3)// Go backwards
  {

    nMotorEncoderTarget[kPrimaryMotor] = nMoveSize;
    motor[kPrimaryMotor]               = nSpeed;
    wait1Msec(nDelay);

  	nSyncedMotors            = synchCA;
  	nSyncedTurnRatio         = nTurnRatio;

    nMotorEncoderTarget[motorC] 	= nMoveSize;
    motor[motorC]                 = nSpeed;
    wait1Msec(nDelay);

    nSyncedMotors            = kSyncType;
	  nSyncedTurnRatio         = nTurnRatio;
  }

nSpeed        = 100;
nTurnRatio    = 0;


repeat(4)//Turn
{
  	nSyncedMotors            = synchCA;
  	nSyncedTurnRatio         = nTurnRatio;

    nMotorEncoderTarget[motorC] 	= nMoveSize;
    motor[motorC]                 = nSpeed;
    wait1Msec(1900);

    nSyncedMotors            = kSyncType;
	  nSyncedTurnRatio         = nTurnRatio;
}

		object = 0;
		SensorValue[S4] = 255;
}



}