#pragma config(Sensor, S1,     Left,           sensorSONAR)
#pragma config(Sensor, S2,     Center,         sensorSONAR)
#pragma config(Sensor, S3,     Right,          sensorSONAR)
#pragma config(Sensor, S4,     Light,          sensorLightInactive)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
int L=0;
int C=0;
int R=0;
int Lht=0;
task main()
{
  while(true)
  {
    L=SensorValue(Left);
    C=SensorValue(Center);
    R=SensorValue(Right);
    Lht=SensorValue(Light);
  ubyte valueToSend[4];
      valueToSend[0] = L;
      valueToSend[1] = C;
      valueToSend[2] = R;
      valueToSend[3] = Lht;
      cCmdMessageWriteToBluetooth(valueToSend, 4,mailbox1);
   }
}

task BroadcastPose()
{
    nSchedulePriority = 5;  // lower than 'default'
    ubyte xmitBuff[12];
    while (true)
    {
        // live data logging
        int x;
        memcpy(xmitBuff[0], "PO", 2);
        x = time1[T1]; memcpy(xmitBuff[2], x, 2);
        x = nMotorEncoder[LEFT]; memcpy(xmitBuff[4], x, 2);
        x = nMotorEncoder[RIGHT]; memcpy(xmitBuff[6], x, 2);
        x = HTMCreadHeading(COMPASS); memcpy(xmitBuff[8], x, 2);
        x = 0; memcpy(xmitBuff[10], x, 2); // reserved for IMU heading
        while (bBTBusy) wait1Msec(10);
        //??cCmdMessageAddToQueue(0, xmitBuff, sizeof(xmitBuff));
        cCmdMessageWriteToBluetooth(xmitBuff, sizeof(xmitBuff), mailbox1);
        while (nBluetoothCmdStatus == ioRsltCommPending) wait1Msec(2);

        wait1Msec(1000);
    }
}