int urg_reboot(urg_t *urg)
{
int expected_ret[][2] = {
{ 1, -1 },
{ 0, -1 },
};
int send_n;
int recv_n;
int i;
urg_laserOff(urg);
/* send RB twice */
for (i = 0; i < 2; ++i) {
send_n = scip_send(&urg->serial_, "RB\n");
if (send_n != 3) {
return SerialSendFail;
}
recv_n = scip_recv(&urg->serial_, "RB", NULL,
expected_ret[i], ScipTimeout);
if (recv_n < 0) {
return recv_n;
}
}
/* disconnect immediately */
urg_disconnect(urg);
return 0;
}
void urg_disconnect(urg_t *urg)
{
/* To stop MD/MS command */
urg_laserOff(urg);
serial_skip(&urg->serial_, ScipTimeout, EachTimeout);
/* Disconnect serial connection */
serial_disconnect(&urg->serial_);
}
/*! main */
int main(int argc, char *argv[])
{
enum {
Times = 10,
Urgs = 2,
};
urg_t urg[Urgs];
long *data[Urgs];
int data_max[Urgs];
int timestamp;
int ret;
int n;
int i;
int k;
#ifdef WINDOWS_OS
const char *devices[] = { "COM3", "COM4" }; /* For Windows */
#else
const char *devices[] = { "/dev/ttyACM0", "/dev/ttyACM1" }; /* For Linux */
#endif
/* Connection */
for (i = 0; i < Urgs; ++i) {
urg_initialize(&urg[i]);
ret = urg_connect(&urg[i], devices[i], 115200);
if (ret < 0) {
urg_exit(&urg[i], "urg_connect()");
}
/* To clear existing MD command*/
urg_laserOff(&urg[i]);
/* It will become easy if some frames are skipped. */
/* If specified skip is 2, then transferred data becomes half. */
/* urg_setSkipLines(&urg[i], 2); */
/* Reserve for receive buffer */
data_max[i] = urg_dataMax(&urg[i]);
data[i] = (long*)malloc(sizeof(long) * data_max[i]);
if (data[i] == NULL) {
perror("data buffer");
exit(1);
}
}
/* Request for MD data */
for (i = 0; i < Urgs; ++i) {
urg_setCaptureTimes(&urg[i], Times);
/* Request for data */
ret = urg_requestData(&urg[i], URG_MD, URG_FIRST, URG_LAST);
if (ret < 0) {
urg_exit(&urg[i], "urg_requestData()");
}
}
for (k = 0; k < Times; ++k) {
for (i = 0; i < Urgs; ++i) {
/* Ends when data reception is completed */
int remain_times = urg_remainCaptureTimes(&urg[i]);
printf(" %d: ", i);
printf("(%03d/%03d): ", remain_times, Times);
if (remain_times <= 0) {
printf("\n");
continue;
}
/* Reception */
n = urg_receiveData(&urg[i], data[i], data_max[i]);
if (n < 0) {
/* Continue processing, because there is chances of receiving the
data next time. */
printf("%s: %s\n", "urg_receiveData()", urg_error(urg));
} else {
/* Display */
timestamp = urg_recentTimestamp(&urg[i]);
printf("timestamp: %d, ", timestamp);
#if 0
{
int j;
for (j = 0; j < n; ++j) {
/* Neglect if distance data is less than urg_minDistance() */
printf("%d:%ld, ", j, data[i][j]);
}
printf("\n");
}
#endif
printf("\n");
}
}
}
/* Disconnect */
for (i = 0; i < Urgs; ++i) {
urg_disconnect(&urg[i]);
free(data[i]);
}
#ifdef MSC
getchar();
#endif
return 0;
}
/* Function: mdlTerminate =====================================================
* Abstract:
* In this function, you should perform any actions that are necessary
* at the termination of a simulation. For example, if memory was
* allocated in mdlStart, this is the place to free it.
*/
static void mdlTerminate(SimStruct *S)
{
urg_laserOff(&urg);
urg_disconnect(&urg);
}
