int bro_server_fist (const bro_fist_t * input_fist, bro_fist_t * out_fist,
int scicos_sock, int spam_sock)
{
// Variable used to store the communications results.
int rc;
/* Check that the sockets are initialized correctly. It's a double check,
* really, because every connection initialization function has its own
* way to check if there were some problems. But in these cases it's a good
* idea to be sure that everything is ok.
*/
assert(scicos_sock >= 0 && spam_sock >= 0);
// Let's receive the Fists from the client (Scicos or whatever)
rc = recv(scicos_sock, input_fist, sizeof(bro_fist_t) * BUFFER_SIZE, 0);
if (rc < 0) {
perror("recv() failed");
return -1;
}
if (rc == 0 || rc < sizeof(bro_fist_t)) {
printf("The client closed the connection before all of the\n");
printf("data was sent\n");
return -1;
}
if (input_fist[0].operation != BRO_END_COMMUNICATION) {
// We are now sending the data to the SPAM and then receive the results...
bro_bt_client_fist (input_fist, out_fist, spam_sock);
} else {
printf("Received END COMMUNICATION packet!\n");
};
// Send the data back to the client with the results
rc = send(scicos_sock, out_fist, sizeof(bro_fist_t) * BUFFER_SIZE, 0);
if (rc < 0) {
perror("send() failed");
return -1;
}
return 0;
}
int
main (int argc, char *argv[])
{
int bro_server_socket = -1;
int bro_client_socket = -1;
int bro_spam_socket = -1;
bro_fist_t in_packet[BUFFER_SIZE];
bro_fist_t out_packet[BUFFER_SIZE];
int i, comm_res, packet_no;
bro_opts_t options;
memset (in_packet, 0, sizeof(bro_fist_t) * BUFFER_SIZE);
memset (out_packet, 0, sizeof(bro_fist_t) * BUFFER_SIZE);
srand ( time(NULL) );
if (bro_opts_parse (&options, argc, argv) == -1) {
return 0;
};
if(bro_bt_connect_device (&bro_spam_socket, options.mac)==0)
printf("Connection successful\n");
else
{
printf("Connection Failed: terminating\n");
return 0;
}
//prepare packets
for(i = 0; i < packets;i++)
{
in_packet[i].port = MOTOR_A;
in_packet[i].operation = SET_POWER;
if(armonicMeasure)
{;
//toDO chiediamo a Palopoli
}
else
{
in_packet[i].data = power;
in_packet[i].timestamp = 0;
}
}
//last packet to stop the motor
in_packet[packets-1].port = MOTOR_A;
in_packet[packets-1].operation = SET_POWER;
in_packet[packets-1].data = 0;
in_packet[packets-1].timestamp = 0;
//initializing CSV file
FILE* data = fopen(fileName, "w");
//fprintf(data,"Count, Time(ms)\n");
printf("Press ENTER to start");
getchar();
bro_bt_client_fist (in_packet, out_packet, bro_spam_socket);
//read the results
int t0 = out_packet[0].timestamp; //time starts when you turn on the brick
for(i = 0; i < packets ; i++)
{
//fprintf(data,"%d,",out_packet[i].port);
//fprintf(data,"%d,",out_packet[i].operation);
fprintf(data,"%f",out_packet[i].timestamp-t0);
fprintf(data,",");
fprintf(data,"%f",out_packet[i].data);
fprintf(data,"\n");
}
printf("\n\n Operation successful, data saved on: %s\n", fileName);
bro_bt_close_connection(bro_spam_socket);
return 0;
//experimental time set to 0
/*
int expTime = 0;
int t0 = 0;
while(expTime < duration)
{
// bro_start_server (&bro_server_socket, &bro_client_socket);
if (armonicMeasure)
in_packet[0].data = power * sin(expTime);
else
//send data to robot
bro_bt_client_fist (in_packet, out_packet, bro_spam_socket);
//write to csv
fprintf(data,"%f,",out_packet[0].data);
fprintf(data,"%f\n",out_packet[0].timestamp-t0);
//wait for sending the next packet
//usleep(samplingTime*1000); //microsecond here
expTime += samplingTime;
}
//stop the motor and close connection
in_packet[0].data = 0;
bro_bt_client_fist (in_packet, out_packet, bro_spam_socket);
bro_bt_close_connection(bro_spam_socket);
return 0;
}
packet_no = 0;
do {
// TODO: Fare il loop di comunicazione con, magari, una word per il blocco
// comm_res = bro_server_fist(in_packet, out_packet, bro_client_socket, bro_spam_socket);
packet_no++;
printf("Pacchetto numero: %i\n", packet_no);
for (i = 0; i < BUFFER_SIZE; i++) {
printf("Istruzione %i:\n", i+1);
printf("\tOperazione: %i\n", in_packet[i].operation);
printf("\tPorta: %i\n", in_packet[i].port);
printf("\tDati: %.2f\n", in_packet[i].data);
printf("\tDati settati: %.2f\n\n", out_packet[i].data);
}
} while ((in_packet[0].operation != BRO_END_COMMUNICATION) && (comm_res
>= 0));
printf("Exited communication loop after %i packets!\n", packet_no);
bro_bt_close_connection(bro_spam_socket);
bro_stop_server(bro_server_socket, bro_client_socket);
return 0;
*/
}
main (int argc, char *argv[])
{
int bro_server_socket = -1;
int bro_client_socket = -1;
int bro_spam_socket = -1;
bro_fist_t in_packet[BUFFER_SIZE];
bro_fist_t out_packet[BUFFER_SIZE];
char file_name[200];
sprintf(file_name, "/home/ubuntu/NXT/AppliedRobotics/motor_data/p%i", (int) MOTOR_POWER);
int i, comm_res, packet_no;
bro_opts_t options;
memset (in_packet, 0, sizeof(bro_fist_t) * BUFFER_SIZE);
memset (out_packet, 0, sizeof(bro_fist_t) * BUFFER_SIZE);
srand ( time(NULL) );
if (bro_opts_parse (&options, argc, argv) == -1) {
return 0;
};
printf("Connecting to the device...\n");
int conect_status = bro_bt_connect_device (&bro_spam_socket, options.mac);;
if (conect_status == 0) {
printf("Connected. Press RUN>.\n");
} else {
printf("Can`t connect to brick.\n");
exit(1);
}
printf("Open File: %s", file_name);
FILE *f = fopen(file_name, "w");
if (f == NULL)
{
printf("Error opening file!\n");
exit(1);
}
fprintf(f, "TIME, TACHO_COUNT\n");
packet_no = 0;
int time = 0 , elapsed_time, start_time, distance;
int time_setted = 0;
do {
bro_fist_t countFist;
countFist.operation = TACHO_COUNT;
countFist.port = MOTOR_B;
in_packet[0] = countFist;
if(packet_no == 0) {
bro_fist_t startFist;
startFist.operation = SET_POWER;
startFist.port = MOTOR_B;
startFist.data = MOTOR_POWER;
in_packet[1] = startFist;
} else {
in_packet[1] = countFist;
}
comm_res = bro_bt_client_fist(in_packet, out_packet, bro_spam_socket);
if (comm_res < 0) {
printf("Conection closed by brick\n");
break;
}
for (i = 0; i < BUFFER_SIZE; i++) {
if(out_packet[i].operation == 6) {
distance = (int) out_packet[i].data;
time = out_packet[i].timestamp;
}
}
if (!time_setted && time != 0) {
start_time = time;
time_setted = 1;
}
elapsed_time = time - start_time;
fprintf(f, "%i,%i\n", elapsed_time, distance);
if (WRITE_PACKETS_TO_STD) {
printf("TIME: %i, TACHO_COUNT: %i\n", elapsed_time, distance);
}
packet_no++;
//usleep(READ_PERIOD * 1000);
} while ((time - start_time) < MEASURE_TIME);
printf("Exited communication loop after %i packets!\n", packet_no);
printf("closing connection...");
bro_bt_close_connection(bro_spam_socket);
fclose(f);
bro_stop_server(bro_server_socket, bro_client_socket);
return 0;
}
