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;
}
}
