bool Bluetooth_ReadMessage(int* One, int* Two, int* Three)
{
Bluetooth_Initialize();
if(bQueuedMsgAvailable())
{
eraseDisplay();
nxtDisplayString(1, "%i", messageParm[0]);
nxtDisplayString(2, "%i", messageParm[1]);
nxtDisplayString(3, "%i", messageParm[2]);
*One = messageParm[0];
*Two = messageParm[1];
*Three = messageParm[2];
ClearMessage();
return true;
}
else
{
return false;
}
}
void Bluetooth_Initialize()
{
if(!Bluetooth_Initialized)
{
Bluetooth_Initialized = true;
while(bQueuedMsgAvailable())
{
ClearMessage();
}
}
}
task main(){
RgtMUp = 0;
RgtMRnd = 0;
LftMUp = 0;
LftMRnd = 0;
checkBTLinkConnected();
while(true){
wait1Msec(1);
while (bQueuedMsgAvailable()){
ClearMessage();
LftRec = messageParm[0];
RgtRec = messageParm[1];
}
/*if(message == 0){
wait1Msec(5);
nxtDisplayTextLine(2,"no message");
continue;
}*/
nxtDisplayTextLine(1,"message[0] = %d",LftRec);
nxtDisplayTextLine(2,"message[1] = %d",RgtRec);
LftGUp = LftRec/100;
LftGRnd = LftRec%100;
RgtGUp = RgtRec/100;
RgtGRnd = RgtRec%100;
RgtMUp = RgtGUp * 16;
RgtMRnd = (RgtGRnd * 16);
LftMUp = LftGUp * 16;
LftMRnd = (LftGRnd * 16);
nxtDisplayTextLine(3,"RGU = %d",RgtGUp);
nxtDisplayTextLine(4,"RGR = %d",RgtGRnd);
nxtDisplayTextLine(5,"LGU = %d",LftGUp);
nxtDisplayTextLine(6,"LGR = %d",LftGRnd);
servo[srvo_S1_C2_1] = RgtMUp;
servo[srvo_S1_C2_2] = RgtMRnd;
servo[srvo_S1_C2_3] = LftMUp;
servo[srvo_S1_C2_4] = LftMRnd;
}
}
bool Bluetooth_MessageAvailable()
{
return bQueuedMsgAvailable();
}
