task sendMessages() { while(true){ nxtDisplayTextLine(1,"RUP = %d",GyroUp[0]); nxtDisplayTextLine(2,"RRND = %d",GyroUp[1]); cCmdMessageWriteToBluetooth(1,GyroUp,2,mailbox1); wait1Msec(1); cCmdMessageWriteToBluetooth(2,GyroRnd,2,mailbox1); wait1Msec(1); cCmdMessageWriteToBluetooth(3,Nunchuck,8,mailbox1); wait1Msec(1); } return; }
void sndOver(ubyte x) { ubyte xmitBuffer[kMaxMsgSize]; TFileIOResult nErrStatus; memset(xmitBuffer,0, kMaxMsgSize); nxtDisplayTextLine(3, "Attempt to send %d!", x); xmitBuffer[0] = x; // cCmdMessageWriteToBluetooth(x, 1, Queue1); <--- this works too if you really just want to send the byte x // the following prepare you the structure in case you want to send more than 1 byte nErrStatus = cCmdMessageWriteToBluetooth(xmitBuffer, 1, Queue1); switch (nErrStatus){ case ioRsltSuccess: case ioRsltCommPending: nxtDisplayTextLine(3, "Send %d Success !", xmitBuffer); break; case ioRsltCommChannelBad: nxtDisplayString(3, "Bad Channel!...."); break; default: nxtDisplayString(3, "Send Failed!...."); break; } }
bool readAndSend(TMailboxIDs incomingMailbox, TMailboxIDs outGoingMailbox) { TFileIOResult nBTCmdErrorStatus; int nSizeOfMessage; // Check to see if a message is available // waitBtIdle(); nSizeOfMessage = cCmdMessageGetSize(incomingMailbox); while (nSizeOfMessage == 0) { nSizeOfMessage = cCmdMessageGetSize(incomingMailbox); wait1Msec(50); } // PlaySound(soundLowBuzz); if (nSizeOfMessage > dataSize) nSizeOfMessage = dataSize; waitBtIdle(); nBTCmdErrorStatus = cCmdMessageRead(dataReceived, nSizeOfMessage, incomingMailbox); waitBtIdle(); if(dataReceived[0]==1) { // TFileIOResult res = cCmdBTCheckStatus(0); prepareDataToSend(); waitBtIdle(); nBTCmdErrorStatus = cCmdMessageWriteToBluetooth(0, dataToSend, dataSize, outGoingMailbox); waitBtIdle(); count++; } return false; }
task main(){ SensorType[S4] = sensorSONAR; while(true){ if(totalY == 0) totalY = 1; if(totalX == 0) totalX = 1; float rot = atan(totalX / totalY) * 180 / PI; if(rot < 0){ rot = 359+rot; } nxtDisplayTextLine(2, "Rot: %f", rot); float dist = sqrt(totalX*totalX + totalY*totalY); nxtDisplayTextLine(3, "Dist: %f", dist); nxtDisplayTextLine(4, "TotalX: %f", totalX); nxtDisplayTextLine(5, "TotalY: %f", totalY); checkForRecieve(); ubyte data[2]; data[0] = detectsWall(); data[1] = nMotorEncoder[motorB]; cCmdMessageWriteToBluetooth(data, 2, mailbox19); } }
task main() { SensorType[S1] = sensorSONAR; motor[motorA] = 40; while(SensorValue[S1]>8){}//moves forward until 8cm away from base motor[motorA] = 0; wait1Msec(60000); //1 minute prep time for spraying glasses ubyte valueToSend[1]; valueToSend[0] = 1; cCmdMessageWriteToBluetooth(valueToSend,1,2); //sends 1 to base to indicate it can continue cleaning }
task sendMessages() { const int kMinTimeBetweenMsg = 50; long nInterMsgTime; static long nLastSendTime = 0; while (true) { // // Delay between messages. // if ((nPgmTime - nLastSendTime) < kMinTimeBetweenMsg) { wait1Msec(6); continue; } if (bBTBusy) { ++nSendBusy; wait1Msec(5); continue; } ++nNearEndSent; ++nMsgXmit; //if (nMsgXmit == 0) nMsgXmit = 1; // Don't send illegal value cCmdMessageWriteToBluetooth(3,chuck1dat,4,mailbox1); wait1Msec(1); cCmdMessageWriteToBluetooth(2,chuck2dat,2,mailbox1); wait1Msec(1); cCmdMessageWriteToBluetooth(1,buttondat,2,mailbox1); nLastSendTime = nPgmTime; } return; }
task main() { //InitRS485(); nxtEnableHSPort(); //Enable High Speed Port #4 nxtSetHSBaudRate(9600); //Xbee Default Speed nxtHS_Mode = hsRawMode; //Set to Raw Mode (vs. Master/Slave Mode) //char messagereceive,messagesend='f'; string my_message; string xRec,yRec,xSend,ySend; int turnA,count,newcount,stopsignal1=0; float travelled,theta=0,x=60,y=60,newx,newy,angle; int num,rotation,distance; int times=0; //ubyte send,incomingData; // turn - 1 -> left, -1->right while(SensorValue[lightSensor] < 35 && stopsignal1==0) { times=times+1; wait1Msec(1000); cCmdMessageRead(my_message,1,1); if(my_message == "f") { stopsignal1=1; break; } num=abs(rand()%100+1); rotation=abs(rand()%180+1); distance=abs(rand()%360); if (num<=50) turnA=1; else turnA=-1; count=turn(rotation,turnA); travelled=move(distance);//* 2*PI*2.8; travelled=travelled*2*PI*2.8/360; if (times==1) theta=90+count/2; else theta=theta+count/2; if(theta>180) theta=theta-360; else if (theta<-180) theta=360+theta; else theta=theta; x+=travelled*cosDegrees(theta); y+=travelled*sinDegrees(theta); nxtDisplayTextLine(1, "Angle %f",theta); // Display the text on line number 4 of 8 on the LCD nxtDisplayTextLine(2, "X-loca %f",x); // Display the text on line number 4 of 8 on the LCD nxtDisplayTextLine(3, "Y- loca %f",y); // Display the text on line number 4 of 8 on the LCD } if(stopsignal1==1) { eraseDisplay(); nxtDisplayTextLine(6,"%s",my_message); //wait1Msec(1000); while(1) { cCmdMessageRead(xRec,10,2); // wait1Msec(1000); cCmdMessageRead(yRec,10,1); newx=atof(xRec); newy=atof(yRec); if (newx!=0 && newy!=0) break; } // newx = messageParm[0]; // newy = messageParm[1]; nxtDisplayTextLine(1,"Received"); nxtDisplayTextLine(2, "New x: %f", newx); nxtDisplayTextLine(3, "New y: %f", newy); distance=sqrt((x-newx)*(x-newx) + (y-newy)*(y-newy)); distance=abs(distance*360/(2*PI*2.8)); angle=atan((y-newy)/(x-newx))/(2*PI) * 360; nxtDisplayTextLine(4, "Angle: %f", angle); if(x<newx) { newcount=abs((0-theta)*2); if (theta<0) turnA=1; else turnA=-1; count=turn(newcount,turnA); if(y>newy) { turnA=-1; count=turn(abs(angle)*2,turnA); } else { turnA=+1; count=turn(abs(angle)*2,turnA); } } else { if(theta<0) { newcount=abs((-180-theta)*2); turnA=-1; } else { newcount=abs((180-theta)*2); turnA=1; } count=turn(newcount,turnA); if(y>newy) { turnA=+1; count=turn(abs(angle)*2,turnA); } else { turnA=-1; count=turn(abs(angle)*2,turnA); } } nxtDisplayTextLine(5, "distance: %d", distance); travelled=move(distance); wait1Msec(5000); } else { eraseDisplay(); //wait1Msec(3000); my_message='f'; cCmdMessageWriteToBluetooth(my_message,strlen(my_message),1); // sendMessageWithParm(x,y); //sendMessage(y); sprintf(xSend,"%f",x); sprintf(ySend,"%f",y); wait1Msec(3000); cCmdMessageWriteToBluetooth(xSend,strlen(xSend),2); nxtDisplayTextLine(2, "New x: %s", xSend); wait1Msec(500); cCmdMessageWriteToBluetooth(ySend,strlen(ySend),1); nxtDisplayTextLine(3, "New y: %s", ySend); } wait1Msec(3000); nxtDisableHSPort(); //Disable HS Port #4 }
task main() { int first = 0; ubyte dataToSend[3] = {1,2,3}; while (true) { wait1Msec(400); // Don’t send messages too frequently. //if (first){ /* cCmdMessageWriteToBluetooth(1, dataToSend, 3, mailbox1); wait1Msec(400); sendMessageWithParm(SensorValue[S1],0, 0); first = 1; // } // else // { cCmdMessageWriteToBluetooth(2, dataToSend, 3, mailbox2); wait1Msec(400);*/ // sendMessageWithParm(SensorValue[S1],SensorValue[S1], 0); // wait10Msec(20); // sendMessageWithParm(SensorValue[S1],SensorValue[S1], 0); first = nBTCurrentStreamIndex; cCmdMessageWriteToBluetooth(2, dataToSend, 3, mailbox1); first = nBTCurrentStreamIndex; wait10Msec(20); while (true) { sendMessageWithParm(SensorValue[S1],SensorValue[S1], 0); cCmdMessageWriteToBluetooth(2, dataToSend, 3, mailbox1); wait10Msec(5); } first = nBTCurrentStreamIndex; cCmdMessageWriteToBluetooth(1, dataToSend, 3, mailbox1); first = nBTCurrentStreamIndex; wait10Msec(20); // wait10Msec(20); // // first = 0; // } /* cCmdMessageWriteToBluetooth(nStream, pData, nLength, nQueueID); Writes a Bluetooth message to 'nQueueID'. The message will be written to the specified 'port' or 'nStream' which should be in the range of 0 to 3. This command is only useful when multiple ports are simultaneously open on the NXT; a configuration that is not recommended because of the much slower communications when multiple ports are in use. The message data is taken from the buffer at 'pData'. 'nLength' is the number of bytes in the message; the maximum length is 58 bytes. */ } }
task sendMessages() { const int kMinTimeBetweenMsg = 50; static long nLastSendTime = 0; while (true) { // // Delay between messages. // checkBTLinkConnected(); if ((nPgmTime - nLastSendTime) < kMinTimeBetweenMsg) { wait1Msec(6); continue; } if (bBTBusy) { ++nSendBusy; wait1Msec(5); continue; } ++nNearEndSent; ++nMsgXmit; //if (nMsgXmit == 0) nMsgXmit = 1; // Don't send illegal value ubyte krik[4]={0,0,0,0}; if(cmps==0){ krik[0] = 0; krik[1] = 0; } if(cmps>0){ krik[0] = cmps; krik[1] = 0; } if(cmps<0){ krik[1] = -cmps; krik[0] = 0; } NunchukGetParamter(S3, 0x00); if (sizeof(outbuf)==6) { if((outbuf1[5]&0x02)!=0){ krik[2] = 1; } if((outbuf1[5]&0x01)!=0){ krik[3] = 1; } } cCmdMessageWriteToBluetooth(3,krik,4,mailbox1); wait10Msec(1); nLastSendTime = nPgmTime; } return; }
task main (){ checkBTLinkConnected(); LftAccumulatorRound=0; LftHeadingRound = 0; RgtAccumulatorRound=0; RgtHeadingRound = 0; time = 0; nTime = 0; cTime = 0; LftSendFin = 0; RgtSendFin = 0; LstLftSendRound = 0; LstRgtSendRound = 0; HTGYROstartCal(LFTGYROROUND); HTGYROstartCal(RGTGYROROUND); while(true){ wait1Msec(1); LftAccumulatorRound+=HTGYROreadRot(LFTGYROROUND); RgtAccumulatorRound+=HTGYROreadRot(RGTGYROROUND); if(time == 10){ eraseDisplay(); LftHeadingRound += LftAccumulatorRound; RgtHeadingRound += RgtAccumulatorRound; if(LftHeadingRound > 80000) LftHeadingRound = 80000; if(LftHeadingRound < -80000) LftHeadingRound = -80000; if(RgtHeadingRound < -80000) RgtHeadingRound = -80000; if(RgtHeadingRound > 80000) RgtHeadingRound = 80000; LftSendRound = (LftHeadingRound*0.0001)+8; RgtSendRound = (RgtHeadingRound*0.0001); RgtSendRound = 0-RgtSendRound+8; if(LftSendRound <= 16 && LftSendRound > 0) LftSendFin[0] = LftSendRound; if(RgtSendRound <= 16 && RgtSendRound > 0) RgtSendFin[0] = RgtSendRound; FinSendfin[0] = LftSendFin[0]; FinSendfin[1] = RgtSendFin[0]; if(nTime == 10){ cCmdMessageWriteToBluetooth(1,FinSendfin,2,mailbox1); wait1Msec(1); nTime = 0; } /*if(cTime == 200){ if(LftSendRound == LstLftSendRound){ PlaySound(soundException); HTGYROstartCal(LFTGYROROUND); } else LstLftSendRound = LftSendRound; if(RgtSendRound == LstRgtSendRound){ PlaySound(soundException); HTGYROstartCal(RGTGYROROUND); } else LstRgtSendRound = RgtSendRound; cTime = 0; }*/ nxtDisplayTextLine(1,"RND = %4d, %4d",FinSendfin[0],FinSendfin[1]); nxtDisplayTextLine(3,"Press [] to reset"); LftAccumulatorRound = 0; RgtAccumulatorRound = 0; time = 0; ++nTime; ++cTime; } if(nNxtButtonPressed == kEnterButton) { LftHeadingRound = 0; HTGYROstartCal(LFTGYROROUND); RgtHeadingRound = 0; HTGYROstartCal(RGTGYROROUND); } ++time; } }