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 void reset(void)
{
int i;
int channel;
robot_name = robot_get_name();
char connector_name[10];
// Enable the Connectors
// connectors[0] = robot_get_device("con1");
// connectors[1] = robot_get_device("con2");
// connector_enable_presence(connectors[0], TIME_STEP);
// connector_enable_presence(connectors[1], TIME_STEP);
int piece_type = robot_name[1] - '0' -1;
max_connector_number = piece_type_nb_connectors[piece_type];
////robot_console_printf("======= %d, %d\n", piece_type, nb_connectors);
for (i = 0; i< max_connector_number; i++) {
// The first connector is the one that attaches to the robot arm
// Construct the names of connectors
//sprintf(connector_name, "%s_c%d", robot_name, 2*i+1);
sprintf(connector_name, "c%d", i+1);
//robot_console_printf("connector %d: %s \n", i+1, connector_name);
// Activate the connectors
connectors[i] = robot_get_device(connector_name);
connector_enable_presence(connectors[i], TIME_STEP);
// Internal state
connector_status[i] = UNLOCKED;
connector_presence[i] = 0;
count_badlock[i] = 0;
}
// Enable keyboard
robot_keyboard_enable(TIME_STEP);
// Enable the emitter/receiver ------------%
commEmitter = robot_get_device("rs232_out");
commReceiver = robot_get_device("rs232_in");
/* If the channel is not the good one, we change it. */
channel = emitter_get_channel(commEmitter);
if (channel != COMMUNICATION_CHANNEL) {
emitter_set_channel(commEmitter, COMMUNICATION_CHANNEL);
}
receiver_enable(commReceiver, TIME_STEP);
//robot_console_printf("The %s robot is reset, it uses %d connectors\n", robot_name, max_connector_number);
return;
}
static void reset(void)
{
int i,j;
int channel;
robot_name = robot_get_name();
robot_num = robot_name[1];
char connector_name[15];
float khepera3_matrix[9][2] =
{ {0, 0}, {-20000, 20000}, {-50000, 50000}, {-70001, 70000}, {70001, -70000},
{50000, -50000}, {20000, -20000}, {0, 0}, {0, 0} };
// { {-5000, -5000}, {-20000, 40000}, {-30000, 50000}, {-70000, 70000}, {70000, -60000},
// {50000, -40000}, {40000, -20000}, {-5000, -5000}, {-10000, -10000} };
char sensors_name[12];
// float (*temp_matrix)[2];
range = RANGE;
sensor_number = 9;
sprintf(sensors_name, "ds0");
range = 2000;
// Enable the Distance sensors and Braitenberg weights
for (i = 0; i < sensor_number; i++) {
sensors[i] = robot_get_device(sensors_name);
distance_sensor_enable(sensors[i], TIME_STEP);
if ((i + 1) >= 10) {
sensors_name[2] = '1';
sensors_name[3]++;
if ((i + 1) == 10) {
sensors_name[3] = '0';
sensors_name[4] = (char) '\0';
}
} else {
sensors_name[2]++;
}
for (j = 0; j < 2; j++) {
matrix[i][j] = khepera3_matrix[i][j];
}
}
// Enable the Connectors
for (i = 0; i< MAX_CONNECTOR_NUMBER; i++) {
// Construct the names of connectors
sprintf(connector_name, "con%d", i+1);
//robot_console_printf("connector %d: %s \n", i, connector_name);
// Activate the connectors
connectors[i] = robot_get_device(connector_name);
connector_enable_presence(connectors[i], TIME_STEP);
// Internal state
connector_status[i] = UNLOCKED;
}
//
// connectors[0] = robot_get_device("con1");
// connectors[1] = robot_get_device("con2");
// connector_enable_presence(connectors[0], TIME_STEP);
// connector_enable_presence(connectors[1], TIME_STEP);
//
// for (i = 0; i< MAX_CONNECTOR_NUMBER; i++) {
// connector_status[i] = UNLOCKED;
// }
// Basic speed at 0
speed_bias[0] = INITIAL_SPEED;
speed_bias[1] = INITIAL_SPEED;
// Enable keyboard
robot_keyboard_enable(TIME_STEP);
// Enable the Arm servo
arm_servo = robot_get_device("arm_servo");
servo_enable_position(arm_servo, TIME_STEP);
//robot_console_printf("The %s robot is reset, it uses %d sensors\n",robot_name, sensor_number);
// Enable the emitter/receiver, gps ------------%
commEmitter = robot_get_device("rs232_out");
commReceiver = robot_get_device("rs232_in");
gps = robot_get_device("gps");
/* If the channel is not the good one, we change it. */
channel = emitter_get_channel(commEmitter);
if (channel != COMMUNICATION_CHANNEL) {
emitter_set_channel(commEmitter, COMMUNICATION_CHANNEL);
}
receiver_enable(commReceiver, TIME_STEP);
gps_enable(gps, TIME_STEP);
found_robot = 0;
return;
}
