MSG_ACTION
MSG_ACK
MSG_LEAD
MSG_START
MSG_STOP
MSG_WAIT
MSG_CUSTOM
0
1
2
3
4
5
6
#define fois_forward 5
#define fois_circle 12
#define reposer Wait(500);
#define SEARCH 23
#define forward_speed 50
#define forward_distance 60 //cm
#define Turn_LeftNinety()
(d - 90+360) % 360;
#define Turn_RightNinety()
(d + 90) % 360;
#define Forward_Forty()
#define Turn_RightThirty()
= (d + 30) % 360;
byte
byte
int
int
previous = 0xFF;
next = 0xFF;
myID = 8;
IDnum = 0;
// angle
actionMsg[7] = dist;
// distance
actionMsg[8] = speed;
actionMsg[9] = 0x00;
// speed
byte ackMsg[];
j = 0;
while(j<3)
{
sendMessage (actionMsg);
TextOut(0, LCD_LINE3, "send msg");
i = 0;
while((!getMessage(ackMsg))&&(i<50))
{
Wait(100);
i++;
}
TextOut(0, LCD_LINE3, "receive ack");
if((ackMsg[4] == MSG_ACK)&&(ackMsg[3] == myID))
{
break;
}
j++;
}
IDnum++;
}
void Go_Back(int i)
{
repeat(i)
{
Turn_RightThirty();
reposer;
}
if(d == 0)
{
//Turn_RightNinety();
//Turn_RightNinety();
RotateMotorEx(OUT_AC, 50, 480,
d = (d + 90) % 360;
reposer;
RotateMotorEx(OUT_AC, 50, 480,
d = (d + 90) % 360;
} else if(d == 90)
{
//Turn_RightNinety();
RotateMotorEx(OUT_AC, 50, 480,
d = (d + 90) % 360;
} else if(d == 270)
{
Turn_LeftNinety();
}
repeat(y)
{
Forward_Forty();
Send_Msg_Ack(forward_distance,
}
forward_speed);
if(x<=0)
{
Turn_LeftNinety();
repeat(-x){
Forward_Forty();
Send_Msg_Ack(forward_distance, forward_speed);
}
}else
{
RotateMotorEx(OUT_AC, 50, 420, -100, true, true);
d = (d + 90) % 360;
repeat(x)
{
Forward_Forty();
Send_Msg_Ack(forward_distance, forward_speed);
}
}
RotateMotor(OUT_AC, forward_speed, 600);
byte leadMsg[];
ArrayInit(leadMsg, 0, 5);
leadMsg[0] = 0x00;
leadMsg[1] = 0x00;
leadMsg[2] = myID;
// ID of the my team
leadMsg[3] = next;
// ID of the destination
leadMsg[4] = 2;
sendMessage (leadMsg);
while(true)
{
TextOut(20, LCD_LINE3, "return!");
}
}
i++;
}
Turn_RightNinety();
//(40, 350, 180)
reposer;
repeat(fois_forward)
{
Forward_Forty();
y--;
Send_Msg_Ack(forward_distance, forward_speed);
i = 0;
repeat(fois_circle)
{
Turn_RightThirty_Check(fois_circle, i);
i++;
}
}
//(40, 0 180)
reposer;
Turn_RightNinety();
Forward_Forty();
x--;
Send_Msg_Ack(forward_distance, forward_speed);
Forward_Forty();
x--;
Send_Msg_Ack(forward_distance, forward_speed);
i = 0;
repeat(fois_circle)
{
Turn_RightThirty_Check(fois_circle, i);
i++;
}
Turn_RightNinety();
Send_Msg_Ack(0, 0);
//(-40, 0, 0)
reposer;
repeat(fois_forward)
{
Forward_Forty();
y++;
Send_Msg_Ack(forward_distance, forward_speed);
i = 0;
repeat(fois_circle)
{
Turn_RightThirty_Check(fois_circle, i);
i++;
}
}
//(-40, 350, 0)
reposer;
Turn_RightNinety();
Forward_Forty();
x++;
Send_Msg_Ack(forward_distance, forward_speed);
repeat(fois_circle)
{
Turn_RightThirty_Check(fois_circle, i);
i++;
}
Turn_RightThirty();
Send_Msg_Ack(0, 0);
//(0, 350, 180)
reposer;
repeat(fois_forward)
{
Forward_Forty();
y--;
Send_Msg_Ack(forward_distance, forward_speed);
i = 0;
repeat(fois_circle)
{
Turn_RightThirty_Check(fois_circle, i);
i++;
}
}
//back to (0,0,180)
}
}
void MovFollow()
{
byte msg[];
byte ackMsg[];
ArrayInit(ackMsg,0,8);
ackMsg[0] = IDnum;
ackMsg[1] = 0x00;
ackMsg[2] = myID;
ackMsg[3] = previous; //
ackMsg[4] = MSG_ACK;
ackMsg[5] = 90;
ackMsg[6] = 0x00;
ackMsg[7] = 0;
// ok for 0
int angle = 0;
int dist = 0;
int speed = 0;
while(true)
{
until (getMessage (msg))
{
Wait (100);
}
if(msg[4] == 0) //action message
{
TextOut(0, LCD_LINE2, "Receive Success! ");
angle = msg[5];
dist = msg[7]; // needs to transform
speed = msg[8];
dist = dist*1800/60;
NumOut(0, LCD_LINE1, (angle-d+360)%360);
"Waiting Msg...");
");