int main() {
double buffer[255];
double fit;
double *rbuffer;
int i;
wb_robot_init();
reset();
receiver_enable(rec,32);
differential_wheels_enable_encoders(64);
robot_step(64);
while (1) {
// Wait for data
while (receiver_get_queue_length(rec) == 0) {
robot_step(64);
}
rbuffer = (double *)wb_receiver_get_data(rec);
// Check for pre-programmed avoidance behavior
if (rbuffer[DATASIZE] == -1.0) {
fitfunc(gwaypoints,100);
// Otherwise, run provided controller
} else {
fit = fitfunc(rbuffer,rbuffer[DATASIZE]);
buffer[0] = fit;
wb_emitter_send(emitter,(void *)buffer,sizeof(double));
}
wb_receiver_next_packet(rec);
}
return 0;
}
static int run(int ms)
{
int i,j;
unsigned short sensors_value[MAX_SENSOR_NUMBER];
int connector_presence[MAX_CONNECTOR_NUMBER];
float speed[2];
float dxEmit, dzEmit;
char sendMessage[20]; // message that robot sends
char angleMessage[20]; // angle to turn piece
// Update the keyboard
keyboard();
// Update the distance sensors
for (i = 0; i < sensor_number; i++) {
sensors_value[i] = distance_sensor_get_value(sensors[i]);
}
// Update the connector presence sensors
for (i = 0; i < MAX_CONNECTOR_NUMBER; i++) {
connector_presence[i] = connector_get_presence(connectors[i]);
}
////robot_console_printf("Connectors : %d %d\n", connector_presence[0], connector_presence[1]);
// Lock automatically
if (!unlock && connector_presence[0] &&
!connector_status[0]) {
//Should lock
//robot_console_printf("Lock !\n");
connector_lock(connectors[0]);
connector_status[0] = LOCKED;
}
// Unlock requested
if (unlock) {
connector_unlock(connectors[0]);
connector_status[0] = UNLOCKED;
}
// Allow locking when out of presence
if (unlock && !connector_presence[0]) {
unlock= !unlock;
}
// Emitter/receiver
/* Send null-terminated robotID */
// const char *robotID = robot_get_name();
// robot_num = robotID[1];
sprintf(sendMessage, "%s%d%c%c", robot_name, state, pieceType, pieceNum);
// strcpy(sendMessage, robotID);
// strcat(sendMessage, state2char(state));
////robot_console_printf("State: %d\n", state);
// const char stateChar = state2char(state);
// const char *sendMessage = strcat(robotID, stateChar);
robot_console_printf("message: %s\n", sendMessage);
emitter_send_packet(commEmitter, sendMessage, strlen(sendMessage) + 1);
// default velocities
velBraitenberg(sensors_value, speed);
switch(state){
case SEARCH_FOR_PIECE:
// what if it picks up a piece by accident?
pieceType = '0';
pieceNum = '0';
if (receiver_get_queue_length(commReceiver) > 0) {
/* Read current packet's data */
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
// what if recMessage is less than 5 indices long?
if ( (recMessage[0] == 'p' && recMessage[4]=='0') ){
// it's an unclaimed piece on the floor
pieceType = recMessage[1];
pieceNum = recMessage[3];
robot_console_printf("Piece found-------\n");
robot_console_printf("Message: %s\n", recMessage);
speed[0] = 0;
speed[1] = 0;
abortCtr = 0;
waitCtr = 0;
state = ALIGN_WITH_PIECE;
}
receiver_next_packet(commReceiver);
}
break;
case ALIGN_WITH_PIECE:
if (receiver_get_queue_length(commReceiver)) {
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
if (recMessage[0] == 'p' &&
recMessage[1] == pieceType && recMessage[3] == pieceNum) {
direction = receiver_get_emitter_direction(commReceiver);
dxEmit = direction[0]; // x distance from emitter of piece
dzEmit = direction[2]+1;
//robot_console_printf("showMessage: %s\n", recMessage);
if (fabs(dxEmit) > EPS_X || fabs(dzEmit) > EPS_Z) {
//robot_console_printf("dxEmit: %f dzEmit: %f\n", fabs(dxEmit), fabs(dzEmit));
////robot_console_printf("Aligning with piece\n");
//velAlign(speed); // ALTERNATIVE
if (dxEmit >= 0) { // robot rotates about center point
speed[0] = 4000;
speed[1] = -4000;
}
else {
speed[0] = -4000;
speed[1] = 4000;
}
} else {
// move forward toward piece
state = APPROACH_PIECE;
}
}
receiver_next_packet(commReceiver);
}
else {
state = SEARCH_FOR_PIECE;
//robot_console_printf("ALIGN_WITH_PIECE: Communication broken !\n");
}
//speed[0] = 0;
//speed[1] = 0;
break;
case APPROACH_PIECE:
if (connector_status[0] == UNLOCKED) {
speed[0] = 5000;
speed[1] = 5000;
}
else {
state = ROTATE_PIECE;
}
break;
case ROTATE_PIECE:
if (receiver_get_queue_length(commReceiver) > 0) {
/* Read current packet's data */
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
//robot_console_printf("Piece message: %s\n", recMessage);
if (recMessage[1] == pieceType && recMessage[3] == pieceNum){
// the piece that it's carrying
if (recMessage[5] == 'A' && turnAngle == 0) {
sprintf(angleMessage, "%c%c%c%c%c", recMessage[6], recMessage[7], recMessage[8],
recMessage[9], recMessage[10]);
turnAngle = atof(angleMessage);
float arm_position = servo_get_position(arm_servo);
////robot_console_printf("servo : %f ", arm_position);
servo_set_position(arm_servo, arm_position+turnAngle);
//robot_console_printf("Angle message: %s\n", angleMessage);
}
if (recMessage[4] == '3') { // piece is now aligned
// NOTE: 3 is the piece state CARRIED
turnAngle = 0;
// turn a little more for certain pieces
/*if (pieceType == '3' || pieceType == '4') {
float arm_position = servo_get_position(arm_servo);
servo_set_position(arm_servo, arm_position-0.2);
} */
state = SEARCH_FOR_ROBOT;
}
}
receiver_next_packet(commReceiver);
}
speed[0] = 0;
speed[1] = 0;
break;
case SEARCH_FOR_ROBOT:
//velBraitenberg(sensors_value, speed);
/* Is there at least one packet in the receiver's queue ? */
if (receiver_get_queue_length(commReceiver) > 0) {
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
//robot_console_printf("Message: %s\n", recMessage);
if (recMessage[0] == 'r' && char2state(recMessage[2])==SEARCH_FOR_ROBOT) {
// specific assembly plan
if ( (pieceType == '1' && recMessage[3] == '2') ||
(pieceType == '2' && recMessage[3] == '1') ||
(pieceType == '3' && recMessage[3] == '4') ||
(pieceType == '4' && recMessage[3] == '3') ||
(pieceType == '5' && recMessage[3] == '6') ||
(pieceType == '6' && recMessage[3] == '5') ||
(pieceType == '2' && recMessage[3] == '7') ||
(pieceType == '7' && recMessage[3] == '2')) {
robot_console_printf("Robot nearby------\n");
speed[0] = 0;
speed[1] = 0;
if (pieceType == '2' && recMessage[3] == '7') {
// rotate piece 2 in new position to fit with piece 7
float arm_position = servo_get_position(arm_servo);
servo_set_position(arm_servo, arm_position-3.14);
}
state = ALIGN_WITH_ROBOT;
}
}
receiver_next_packet(commReceiver);
}
if (pieceType == '7') {
//robot_console_printf("Holding piece 7\n");
// speed[0] = 0;
// speed[1] = 0;
}
break;
case ALIGN_WITH_ROBOT:
if (receiver_get_queue_length(commReceiver) > 0) {
// other robot is still in communication range
/* Read current packet's data */
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
// MORE CONDITIONS
if (recMessage[0] == 'r'){ // it's a robot (not a piece)
if (robot_num < recMessage[1]) {
robot1 = robot_num;
robot2 = recMessage[1];
}
else {
robot1 = recMessage[1];
robot2 = robot_num;
}
/* print null-terminated robotID */
//robot_console_printf("Message from nearby robot: \"%s\"\n", recMessage);
////robot_console_printf("Other robot is in state: %d\n",char2state(recMessage[2]));
if (robot_num == robot2 && char2state(recMessage[2])==ALIGN_WITH_ROBOT) {
// robot 2 waits while robot 1 aligns
speed[0] = 0;
speed[1] = 0;
state = WAIT;
//robot_console_printf("Robot is R2, waiting\n");
}
if (robot_num == robot1 || (robot_num == robot2 && char2state(recMessage[2])==WAIT)) {
////robot_console_printf("Robot is R1, aligning\n");
direction = receiver_get_emitter_direction(commReceiver);
//robot_console_printf("Direction: [%f %f %f]\n", direction[0], direction[1], direction[2]);
dxEmit = direction[0]; // x distance from emitter of other robot
dzEmit = direction[2]+1;
// TO DO: replace this with more general code for multiple robots
if (fabs(dxEmit) > EPS_X || fabs(dzEmit) > EPS_Z) {
//robot_console_printf("dxEmit: %f dzEmit: %f\n", fabs(dxEmit), fabs(dzEmit));
if (dxEmit >= 0) {
speed[0] = 4000;
speed[1] = -4000;
}
else {
speed[0] = -4000;
speed[1] = 4000;
}
//velAlign(speed); // alternative
} else {
speed[0] = 0;
speed[1] = 0;
if (robot_num == robot1) {
state = WAIT; // wait for robot 2 to align
}
if (robot_num == robot2) {
state = APPROACH_ROBOT; // start approach to robot 1
}
}
}
}
/* fetch next packet */
receiver_next_packet(commReceiver);
}
else {
state = SEARCH_FOR_ROBOT;
//robot_console_printf("ALIGN_WITH_ROBOT: Communication broken !\n");
}
break;
case WAIT:
// what if it receives other robots' messages?
if (receiver_get_queue_length(commReceiver) > 0) {
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
speed[0] = 0;
speed[1] = 0;
if (robot_num == robot2 && char2state(recMessage[2])==WAIT) {
state = ALIGN_WITH_ROBOT; // robot 1 has finished aligning
}
if (robot_num == robot1 && char2state(recMessage[2])==APPROACH_ROBOT) {
state = APPROACH_ROBOT;
}
/* fetch next packet */
receiver_next_packet(commReceiver);
}
else {
state = SEARCH_FOR_ROBOT;
//robot_console_printf("WAIT: Communication broken !\n");
}
break;
case APPROACH_ROBOT:
speed[0] = 3000;
speed[1] = 3000;
if (receiver_get_queue_length(commReceiver) > 0) {
/* Read current packet's data */
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
// pieces have locked together; numbers are the resulting
// conglomerate piece states
if (strcmp(recMessage,"Lock5")==0 ||
strcmp(recMessage,"Lock6")==0 ||
strcmp(recMessage,"Lock7")==0 ||
strcmp(recMessage,"Lock8")==0 ){
// the piece that it's carrying
speed[0] = 0;
speed[1] = 0;
// pieceType of robot not carrying piece will later go back to '0'
pieceTypeNew = recMessage[4];
state = SEPARATE_ROBOTS;
}
receiver_next_packet(commReceiver);
}
break;
case SEPARATE_ROBOTS:
//speed[0] = 0;
//speed[1] = 0;
//if (receiver_get_queue_length(commReceiver) > 0) {
/* Read current packet's data */
// const void *buffer = receiver_get_data(commReceiver);
// const char *recMessage = (char*) buffer;
// if (recMessage[0] == 'r' &&
// char2state(recMessage[2])==SEPARATE_ROBOTS) {
if (ctr == 0 &&
(pieceType == '2' || pieceType == '3' || pieceType == '5')) {
// detach from magnet
robot_console_printf("Detach-------\n");
unlock = true;
}
if (ctr < 25) { //back up
speed[0] = -8000;
speed[1] = -8000;
ctr += 1;
}
else if (ctr >= 20 && ctr < 45) { // turn away from other robot
speed[0] = -8000;
speed[1] = 8000;
ctr += 1;
}
else {
ctr = 0;
if (pieceType == '1' || pieceType == '4') { // ||
//pieceType == '6') { // hold onto piece
pieceType = pieceTypeNew;
state = SEARCH_FOR_ROBOT;
}
else if (pieceType == '6') {
pieceType = pieceTypeNew;
float arm_position = servo_get_position(arm_servo);
servo_set_position(arm_servo, arm_position-1.57);
state = SEARCH_FOR_ROBOT;
}
else if (pieceType == '2' || pieceType == '3' ||
pieceType == '5') {
state = SEARCH_FOR_PIECE;
}
else if (pieceType == '7') { // puzzle is completed
unlock = true;
state = SEARCH_FOR_PIECE;
}
}
//}
//receiver_next_packet(commReceiver);
//}
break;
}
/* Set the motor speeds */
//robot_console_printf("Speeds: %d %d\n", (int) speed[0], (int) speed[1]);
differential_wheels_set_speed((int) speed[0], (int) speed[1]);
// Output the current metrics into the file
//file_output();
elapsed_time += (float)TIME_STEP / 1000.0;
return TIME_STEP; /* run for TIME_STEP ms */
}
static int run(int ms)
{
int i, j;
int robotNear;
char sendMessage[20]; // message that robot sends
char lockMessage[20];
char str[20];
float dxEmit, dzEmit;
//float next_position[2];
// Handle keyboard
keyboard();
// Connector status
for (i = 0; i < max_connector_number; i++) {
connector_presence[i] = connector_get_presence(connectors[i]);
}
////robot_console_printf("Connectors : %d %d\n", connector_presence[0], connector_presence[1]);
// Lock automatically
if (autolock) {
for (i = 0; i < max_connector_number; i++) {
//Should lock automatically
if (!unlock[i] && connector_presence[i] &&
!connector_status[i]) {
//robot_console_printf("Lock %d !\n", i);
connector_lock(connectors[i]);
connector_status[i] = LOCKED;
}
// Unlock requested
if (unlock[i]) {
// Unlocking event
if (connector_status[i]) {
connector_unlock(connectors[i]);
}
// Allow a new locking when out of presence
if (!connector_presence[i]
&& connector_status[i] || (count_unlock[i] > MAX_COUNT_UNLOCK)) {
count_unlock[i] = 0;
unlock[i]= !unlock[i];
connector_status[i] = UNLOCKED;
//robot_console_printf("Unlock %d..\n", i);
}
count_unlock[i] ++;
} else {
count_unlock[i] = 0;
}
// Unlock if bad position
if ((connector_status[i] == LOCKED && !connector_presence[i] )){
count_badlock[i] ++;
if (count_badlock[i] > MAX_COUNT_BADLOCK) {
count_badlock[i] = 0;
unlock[i] = true;
//robot_console_printf("Bad position %d %d\n", i, count_badlock[i]);
}
} else {
count_badlock[i] = 0;
}
// if (!unlock && connector_presence[2*i] && connector_presence[2*i+1] &&
// !connector_status[2*i] && !connector_status[2*i+1]) {
// //Should lock
// //robot_console_printf("Lock %d !\n", i);
// connector_lock(connectors[2*i]);
// connector_lock(connectors[2*i+1]);
// connector_status[2*i] = LOCKED;
// connector_status[2*i+1] = LOCKED;
// }
//
// // Unlock requested
// if (unlock && (connector_status[2*i] || connector_status[2*i+1])) {
// connector_unlock(connectors[2*i]);
// connector_unlock(connectors[2*i+1]);
// }
//
// // Allow locking when out of presence
// if (unlock && !connector_presence[2*i] && !connector_presence[2*i+1]
// && connector_status[2*i] && connector_status[2*i+1]) {
// unlock= !unlock;
// connector_status[2*i] = UNLOCKED;
// connector_status[2*i+1] = UNLOCKED;
// //robot_console_printf("Unlock %d..\n", i);
// }
//
// // Unlock if bad position
// if ((connector_status[2*i] == LOCKED && !connector_presence[2*i] )||
// (connector_status[2*i+1]== LOCKED && !connector_presence[2*i+1])){
// unlock = true;
// }
}
}
const char *robotID = robot_get_name();
int piece_type = robotID[1] - '0' -1;
strcpy(sendMessage, robotID);
strcat(sendMessage, state2char(state));
// robot_console_printf("message: %s\n", sendMessage);
// emitter_send_packet(commEmitter, sendMessage, strlen(sendMessage) + 1);
switch(state){
case FREE:
emitter_send_packet(commEmitter, sendMessage, strlen(sendMessage) + 1);
while (receiver_get_queue_length(commReceiver) > 0) {
/* Read current packet's data */
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
if (recMessage[0] == 'r' && (recMessage[2] == '1' ||
recMessage[2] == '2' || recMessage[2] == '3') &&
recMessage[3] == robot_name[1] && recMessage[4] == robot_name[3]){
// piece is claimed
// NOTE: '1' corresponds to robot state ALIGN_WITH_PIECE
// '2' corresponds to robot state APPROACH_PIECE
waitCtr = 0;
state = CLAIMED;
}
receiver_next_packet(commReceiver);
}
break;
case CLAIMED:
// robotNear = 0;
waitCtr += 1;
emitter_send_packet(commEmitter, sendMessage, strlen(sendMessage) + 1);
if (waitCtr > WAIT_TIME) {
waitCtr = 0;
state = FREE;
}
/*
while (receiver_get_queue_length(commReceiver) > 0) {
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
// TO DO: add more conditions
if (recMessage[0] == 'r') {
robotNear = 1;
// state = FREE;
}
receiver_next_packet(commReceiver);
}
if (receiver_get_queue_length(commReceiver) == 0) {
// robot went out of range
if (waitCtr > WAIT_TIME) {
state = FREE;
}
//state = FREE;
}
if (robotNear == 0) {
state = FREE;
} */
//if (receiver_get_queue_length(commReceiver) == 0) {
//}
if (connector_status[0] == LOCKED) {
// The first connector is the one that attaches to the robot arm
waitCtr = 0;
state = ROTATING;
}
break;
case ROTATING:
////robot_console_printf("Rotating--\n");
// wait until piece has first snapped into place
//if (rotate_ctr < 10) {
// rotate_ctr += 1;
// }
//j = piece_to_piece_connectors[piece_type-1];
if (receiver_get_queue_length(commReceiver) > 0) {
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
if (recMessage[0] == 'r' && recMessage[3] == robot_name[1] &&
recMessage[4] == robot_name[3]){
// state 3 for robot is ROTATE_PIECE
direction = receiver_get_emitter_direction(commReceiver);
float xEmit = direction[0];
float zEmit = direction[2];
float turnAngle = atan2(xEmit, zEmit); // angle piece has to turn
// convert angle to string
strcat(sendMessage, "A");
sprintf(str, "%.3g", turnAngle);
strcat(sendMessage, str);
//robot_console_printf("message: %s\n", sendMessage);
emitter_send_packet(commEmitter, sendMessage, strlen(sendMessage) + 1);
//dxEmit = direction[0]; // x distance from emitter of piece
// receiver z axis faces away from connector that should be
// attached to another connector
dzEmit = direction[2]-1;
////robot_console_printf("direction: %f %f %f\n", direction[0], direction[1], direction[2]);
//robot_console_printf("dzEmit: %f\n", fabs(dzEmit));
if (fabs(dzEmit) <= EPS_Z) {
//robot_console_printf("Lock before CARRIED\n");
//connector_lock(connectors[0]);
//if (recMessage[2] == '4') { // robot is is next state
state = CARRIED;
//}
}
}
receiver_next_packet(commReceiver);
}
// else?
if (connector_status[0] == UNLOCKED) {
state = FREE;
}
break;
case CARRIED:
while (receiver_get_queue_length(commReceiver) > 0) {
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
if (recMessage[0] == 'r' && recMessage[2] == '3' &&
recMessage[3] == robot_name[1] && recMessage[4] == robot_name[3]){
// only send state until robot recognizes that it can search for a robot
emitter_send_packet(commEmitter, sendMessage, strlen(sendMessage) + 1);
}
// add conditions?
// for piece 2 to distinguish which state to go into
if (recMessage[0] == 'r' && recMessage[2] == '7' && recMessage[3] == '1') {
otherPiece = 1;
}
if (recMessage[0] == 'r' && recMessage[2] == '7' && recMessage[3] == '7') {
otherPiece = 7;
}
receiver_next_packet(commReceiver);
}
if (connector_status[1] == LOCKED) {
// The second connector attaches to another piece
// should only bond with another appropriate piece--TO DO: check other piece?
// new states of 2-piece conglomerates
switch (piece_type+1) {
// don't want pieces unnecessarily broadcasting messages
case 1:
state = NULL_STATE; // PIECE_5;
break;
case 2:
if (otherPiece > 0) {
if (otherPiece == 1) {
state = PIECE_5;
}
if (otherPiece == 7) {
state = PIECE_8;
}
otherPiece = 0;
}
break;
case 3:
state = PIECE_6;
break;
case 4:
state = NULL_STATE; // PIECE_6;
break;
}
}
break;
case PIECE_5: // 5th type of piece: 1 and 2
while (receiver_get_queue_length(commReceiver) > 0) {
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
if (recMessage[0] == 'r' && recMessage[2] == '7' && state5Known == 0) {
if (recMessage[3] == '1' || recMessage[3] == '2') {
// only send state until robot recognizes that it can detach
// robot does not yet think its piece type is state 5
robot_console_printf("Nearby robot: %c\n", recMessage[1]);
sprintf(lockMessage, "Lock5%c", recMessage[1]); // send robot ID for uniqueness
emitter_send_packet(commEmitter, lockMessage, strlen(lockMessage) + 1);
}
else if (recMessage[3] == '5') {
robot_console_printf("STATE 5 KNOWN \n");
state5Known = 1;
}
}
receiver_next_packet(commReceiver);
}
if (piece_type+1 == 2 && connector_status[3] == LOCKED) {
// only piece 2 goes into this state
state = PIECE_7;
}
break;
case PIECE_6: // 6th type of piece: 3 and 4
while (receiver_get_queue_length(commReceiver) > 0) {
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
if (recMessage[0] == 'r' && recMessage[2] == '7' && state6Known == 0) {
if (recMessage[3] == '3' || recMessage[3] == '4'){
// only send state until robot recognizes that it can detach
// robot does not yet think its piece type is state 6
sprintf(lockMessage, "Lock6%c", recMessage[1]); // send robot ID for uniqueness
emitter_send_packet(commEmitter, lockMessage, strlen(lockMessage) + 1);
}
else if (recMessage[3] == '6') {
state6Known = 1;
}
}
receiver_next_packet(commReceiver);
}
if (piece_type+1 == 3 && connector_status[2] == LOCKED) {
// only piece 3 goes into this state
state = PIECE_7;
}
break;
case PIECE_7: // 7th type of piece: 1, 2, 3, 4
while (receiver_get_queue_length(commReceiver) > 0) {
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
if (recMessage[0] == 'r' && recMessage[2] == '7' && state7Known == 0) {
if (recMessage[3] == '5' || recMessage[3] == '6'){
// only send state until robot recognizes that it can detach
// robot does not yet think its piece type is state 7
sprintf(lockMessage, "Lock7%c", recMessage[1]); // send robot ID for uniqueness
emitter_send_packet(commEmitter, lockMessage, strlen(lockMessage) + 1);
}
else if (recMessage[3] == '7') {
state7Known = 1;
}
}
receiver_next_packet(commReceiver);
}
if (piece_type+1 == 2 && connector_status[2] == LOCKED) {
// only piece 2 goes into this state
state = PIECE_8;
}
break;
case PIECE_8:
while (receiver_get_queue_length(commReceiver) > 0) {
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
if (recMessage[0] == 'r' && recMessage[2] == '7' && state8Known == 0) {
if (recMessage[3] == '7' || recMessage[3] == '2'){
// only send state until robot recognizes that it can detach
// robot does not yet think its piece type is state 8
sprintf(lockMessage, "Lock8%c", recMessage[1]); // send robot ID for uniqueness
emitter_send_packet(commEmitter, lockMessage, strlen(lockMessage) + 1);
}
else if (recMessage[3] == '8') {
state8Known = 1;
}
}
receiver_next_packet(commReceiver);
}
break;
case NULL_STATE:
break;
}
return TIME_STEP; /* run for TIME_STEP ms */
}
