task main()
{
clearDebugStream();
writeDebugStreamLine("xbee5");
time1[T1] = 0;
nxtEnableHSPort(); //Enable High Speed Port #4
nxtSetHSBaudRate(115200); //Xbee Default Speed
nxtHS_Mode = hsRawMode; //Set to Raw Mode (vs. Master/Slave Mode)
while (true)
{
int t = time1[T1];
int ax, ay, az, tx, ty, tz, gx, gy, gz;
MSIMUreadAccelAxes(AIMU, ax, ay, az);
MSIMUreadTiltAxes(AIMU, tx, ty, tz);
MSIMUreadGyroAxes(AIMU, gx, gy, gz);
dataLog(7, t, (az / 100.0));
dataLog(8, t, (tz / 1000.0));
dataLog(9, t, gz);
wait1Msec(50);
}
}
void setupHighSpeedLink(const bool bMaster)
{
// Initialize port S$ to "high speed" mode.
nxtEnableHSPort();
int i = nxtSetHSBaudRate(9600);
nxtHS_Mode = hsMsgModeMaster;
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
}
