00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include "surveyor_comms.h"
00026
00027 #include <errno.h>
00028 #include <assert.h>
00029 #include <fcntl.h>
00030 #include <sys/stat.h>
00031 #include <sys/ioctl.h>
00032 #include <termios.h>
00033 #include <math.h>
00034 #include <stdio.h>
00035 #include <unistd.h>
00036 #include <stdlib.h>
00037 #include <sys/time.h>
00038 #include <time.h>
00039 #include <string.h>
00040
00041 #include <iomanip>
00042 #include <iostream>
00043
00044 srv1_comm_t *
00045 srv1_create(const char *port)
00046 {
00047 srv1_comm_t *ret = (srv1_comm_t *) malloc(sizeof(srv1_comm_t));
00048
00049 ret->fd = -1;
00050 ret->image_mode = SRV1_IMAGE_OFF;
00051 ret->set_image_mode = SRV1_IMAGE_OFF;
00052 ret->need_ir = 0;
00053
00054 ret->frame_size = 0;
00055
00056 ret->vx = 0.0;
00057 ret->va = 0.0;
00058
00059 ret->frame = NULL;
00060
00061 strncpy(ret->port, port, sizeof(ret->port) - 1);
00062
00063 return ret;
00064 }
00065
00066
00067
00068
00069 int
00070 flip_nonblock(int fd)
00071 {
00072 int flags;
00073 if ((flags = fcntl(fd, F_GETFL)) < 0)
00074 {
00075 perror("surveyor_flip_nonblock():fcntl():");
00076 close(fd);
00077 return -1;
00078 }
00079
00080 if (fcntl(fd, F_SETFL, flags ^ O_NONBLOCK) < 0)
00081 {
00082 perror("surveyor_flip_nonblock():fcntl():");
00083 close(fd);
00084 return -1;
00085 }
00086
00087 return 0;
00088 }
00089
00090 int32_t
00091 msecsub(struct timeval a, struct timeval b)
00092 {
00093 return ((a.tv_sec - b.tv_sec) * 1000000) + (a.tv_usec - b.tv_usec);
00094 }
00095
00096
00097
00098
00099
00100 int
00101 read_limited(int fd, char *buf, int bytes, int microsecs)
00102 {
00103 flip_nonblock(fd);
00104
00105 struct timeval begin, now;
00106 gettimeofday(&begin, NULL);
00107
00108 int needtoread = bytes;
00109 int readresult;
00110
00111 for (;;)
00112 {
00113 gettimeofday(&now, NULL);
00114 if (msecsub(now, begin) > microsecs)
00115 {
00116 printf(
00117 "read_limited():Warning: CARLOS timed out (%d microsecs).\n",
00118 msecsub(now, begin));
00119 flip_nonblock(fd);
00120 return (bytes - needtoread);
00121 }
00122
00123 if ((readresult = read(fd, buf + (bytes - needtoread), needtoread))
00124 < 0)
00125 {
00126 if (errno != EAGAIN)
00127 {
00128 perror("read_limited():read()");
00129 flip_nonblock(fd);
00130 return -1;
00131 }
00132 }
00133 else
00134 {
00135 needtoread = needtoread - readresult;
00136 if (needtoread == 0)
00137 {
00138 break;
00139 }
00140 }
00141
00142 usleep(20);
00143 }
00144
00145 flip_nonblock(fd);
00146 return bytes;
00147 }
00148
00149
00150
00151
00152
00153 int
00154 srv1_flush_input(srv1_comm_t *x)
00155 {
00156 int res;
00157 ioctl(x->fd, TIOCINQ, (char *) &res);
00158 tcflush(x->fd, TCIFLUSH);
00159 return res;
00160 }
00161
00162 int
00163 srv1_open(srv1_comm_t *x)
00164 {
00165
00166 struct termios term;
00167
00168 int fd;
00169
00170 printf("Opening connection to Surveyor on %s...", x->port);
00171
00172
00173
00174 if ((fd = open(x->port, O_RDWR | O_NONBLOCK, 00644)) < 0)
00175 {
00176 perror("surveyor_open():open():");
00177 return 0;
00178 }
00179
00180 if (tcflush(fd, TCIFLUSH) < 0)
00181 {
00182 perror("surveyor_open():tcflush():");
00183 close(fd);
00184 return 0;
00185 }
00186
00187 if (tcgetattr(fd, &term) < 0)
00188 {
00189 perror("surveyor_open():tcgetattr():");
00190 close(fd);
00191 return 0;
00192 }
00193
00194 cfmakeraw(&term);
00195 cfsetispeed(&term, B115200);
00196 cfsetospeed(&term, B115200);
00197
00198 if (tcsetattr(fd, TCSAFLUSH, &term) < 0)
00199 {
00200 perror("surveyor_open():tcsetattr():");
00201 close(fd);
00202 return 0;
00203 }
00204
00205
00206 flip_nonblock(fd);
00207
00208 puts("Done.");
00209
00210 x->fd = fd;
00211
00212 return 1;
00213 }
00214
00215 void
00216 srv1_close(srv1_comm_t *x)
00217 {
00218 printf("\nNow closing: srv1_close()\n");
00219 srv1_set_speed(x, 0, 0);
00220
00221 close(x->fd);
00222 }
00223
00224 void
00225 srv1_destroy(srv1_comm_t *x)
00226 {
00227 if (x->frame != NULL)
00228 {
00229 free(x->frame);
00230 }
00231
00232 if (x->fd != -1)
00233 {
00234 srv1_close(x);
00235 }
00236
00237 free(x);
00238 return;
00239 }
00240
00241 int
00242 srv1_init(srv1_comm_t *x)
00243 {
00244 assert(x);
00245
00246 if (srv1_open(x) == 0)
00247 {
00248
00249 return 0;
00250 }
00251
00252
00253 char buf[256];
00254 if (write(x->fd, "V", 1) < 0)
00255 {
00256 printf("srv1_init(): can't write to port %s!\n", x->port);
00257 return 0;
00258 }
00259
00260 int spot = 0;
00261 memset(buf, 0, 256);
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283 while (buf[spot - 1] != '\n')
00284 {
00285 int num = read(x->fd, buf + spot, 1);
00286 if (num != 1)
00287 {
00288 printf("srv1_init(): Can't read a byte from surveyor!\n");
00289 return 0;
00290 }
00291 spot++;
00292 }
00293
00294 printf("srv1_init(): successful init. HW %s", buf + 2);
00295
00296 return 1;
00297 }
00298
00299 double
00300 calc_forward(signed char speed)
00301 {
00302 unsigned char speedg = (speed > 0 ? speed : -speed);
00303 if (speedg < 20)
00304 {
00305 return 0;
00306 }
00307 double i3 = 5.5277e-7;
00308 double j3 = -0.00016261;
00309 double k3 = 0.01628;
00310 double l3 = -0.26129;
00311
00312 double x = k3 * speedg;
00313 double y = j3 * speedg * speedg;
00314 double z = i3 * speedg * speedg * speedg;
00315 double result = l3 + x + y + z;
00316 printf("calc_forward(%d) = %f + %f + %f + %f = %f\n", speed, l3, x, y, z,
00317 result);
00318 return (speed > 0 ? result : -result);
00319 }
00320
00321 signed char
00322 calc_speed_hackish(double dx)
00323 {
00324 if (fabs(dx) > SRV1_MAX_VEL_X)
00325 {
00326 printf("srv1_set_speed(): warning: speed out of range.\n");
00327 return (dx > 0.0 ? 127 : -127);
00328 }
00329
00330 if (fabs(dx) < 0.05)
00331 {
00332 return 0;
00333 }
00334
00335 signed char current = 20;
00336
00337 for (;;)
00338 {
00339 current = current + 1;
00340 if (calc_forward(current) > fabs(dx))
00341 {
00342 return (dx < 0.0 ? -(current - 1) : current - 1);
00343 }
00344 }
00345 }
00346
00347 double
00348 calc_angular(int left, int right)
00349 {
00350 double basis = 0.234;
00351 printf("calc_angular(%d, %d) = %f\n", left, right, (calc_forward(right)
00352 - calc_forward(left)) / basis);
00353 return (calc_forward(right) - calc_forward(left)) / basis;
00354 }
00355
00356 void
00357 calc_rot_hackish(double dx, signed char *left, signed char *right)
00358 {
00359 int direction = 0;
00360 int l = *left;
00361 int r = *right;
00362 double angular = 0;
00363
00364 if (fabs(dx) < 0.05)
00365 {
00366 return;
00367 }
00368
00369 if (l == 0)
00370 {
00371 while ((l > -127) && (l < 127) && (r > -127) && (r < 127))
00372 {
00373 angular = calc_angular(l, r);
00374 if (angular < dx)
00375 {
00376 if (direction == -1)
00377 {
00378 break;
00379 }
00380 if (r < 20)
00381 {
00382 r = 20;
00383 l = -r;
00384 }
00385
00386 l--;
00387 r++;
00388 direction = 1;
00389 }
00390 if (angular > dx)
00391 {
00392 if (direction == 1)
00393 {
00394 break;
00395 }
00396 if (l < 20)
00397 {
00398 l = 20;
00399 r = -l;
00400 }
00401 l++;
00402 r--;
00403 direction = -1;
00404 }
00405 }
00406
00407 }
00408 else if (l > 0)
00409 {
00410 while ((l < 127) && (r < 127))
00411 {
00412 angular = calc_angular(l, r);
00413 if (angular < dx)
00414 {
00415 if (direction == -1)
00416 {
00417 break;
00418 }
00419
00420 r++;
00421 direction = 1;
00422 }
00423 if (angular > dx)
00424 {
00425 if (direction == 1)
00426 {
00427 break;
00428 }
00429 l++;
00430 direction = -1;
00431 }
00432 }
00433
00434 }
00435 else
00436 {
00437 while ((l > -127) && (r > -127))
00438 {
00439 angular = calc_angular(l, r);
00440 if (angular < dx)
00441 {
00442 if (direction == -1)
00443 {
00444 break;
00445 }
00446
00447 l--;
00448 direction = 1;
00449 }
00450 if (angular > dx)
00451 {
00452 if (direction == 1)
00453 {
00454 break;
00455 }
00456 r--;
00457 direction = -1;
00458 }
00459 }
00460 }
00461
00462 if (((r == 127) || (r == -127) || (l == 127) || (l == -127)) && (fabs(dx
00463 - calc_angular(l, r)) > 0.01))
00464 {
00465 printf(
00466 "srv1_set_speed(): warning: can't achieve %f rotation. got %f.\n",
00467 dx, calc_angular(l, r));
00468 }
00469
00470 *right = r;
00471 *left = l;
00472 }
00473
00474 int
00475 srv1_set_motors(srv1_comm_t *x, signed char l, signed char r, double t)
00476 {
00477 char cmdbuf[4];
00478
00479 char runtime;
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490 runtime = 0;
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504 cmdbuf[0] = 'M';
00505 cmdbuf[1] = l;
00506 cmdbuf[2] = r;
00507 cmdbuf[3] = runtime;
00508
00509 if (write(x->fd, cmdbuf, 4) < 0)
00510 {
00511
00512
00513 }
00514
00515
00516 if (read_limited(x->fd, cmdbuf, 2, 250000) == 2)
00517 {
00518 if (cmdbuf[0] == '#' && cmdbuf[1] == 'M')
00519 {
00520 return 1;
00521 }
00522 printf("srv1_set_speed(): warning: failed response: %c%c!!!\n",
00523 cmdbuf[0], cmdbuf[1]);
00524 int bytes = srv1_flush_input(x);
00525 printf("srv1: flushed %d bytes from input buffer\n", bytes);
00526
00527 }
00528
00529 return 0;
00530 }
00531
00532 int
00533 srv1_set_speed(srv1_comm_t *x, double dx, double dw)
00534 {
00535
00536 signed char leftspeed;
00537 signed char rightspeed;
00538
00539 printf("srv1_set_speed(): debug: dx: %2.2f dw: %2.2f\n", dx, dw);
00540 leftspeed = calc_speed_hackish(dx);
00541
00542 rightspeed = leftspeed;
00543
00544 x->vx = calc_forward(leftspeed);
00545
00546 calc_rot_hackish(dw, &leftspeed, &rightspeed);
00547
00548 x->va = calc_angular(leftspeed, rightspeed);
00549
00550
00551
00552 return srv1_set_motors(x, leftspeed, rightspeed, 0.0);
00553 }
00554
00555 int
00556 srv1_fill_image(srv1_comm_t *x)
00557 {
00558
00559
00560
00561
00562 char specbuf[10];
00563
00564 memset(specbuf, 0, 10);
00565
00566 if (x->set_image_mode != x->image_mode)
00567 {
00568 if (x->image_mode != SRV1_IMAGE_OFF)
00569 {
00570 printf("srv1_fill_image(): setting image mode '%c'\n",
00571 x->image_mode);
00572 if (write(x->fd, &(x->image_mode), 1) < 0)
00573 {
00574 return 0;
00575 }
00576
00577 int done = read_limited(x->fd, specbuf, 2, 500000);
00578
00579 if (done != 2)
00580 {
00581
00582 int btsdead = srv1_flush_input(x);
00583 printf("srv1_fill_image(): discarded %d bytes.\n", btsdead);
00584 return 0;
00585 }
00586
00587 if (specbuf[0] != '#' || specbuf[1] != x->image_mode)
00588 {
00589 printf(
00590 "srv1_fill_image(): didn't get correct response from image size set: %s\n",
00591 specbuf);
00592 int btsdead = srv1_flush_input(x);
00593 printf("srv1_fill_image(): discarded %d bytes.\n", btsdead);
00594 return 0;
00595 }
00596 else
00597 {
00598 x->set_image_mode = x->image_mode;
00599 }
00600 }
00601 else
00602 {
00603 x->set_image_mode = x->image_mode;
00604 }
00605 }
00606
00607 if (x->set_image_mode == SRV1_IMAGE_OFF)
00608 {
00609 return 1;
00610 }
00611 printf("srv1_fill_image(): Image Mode '%c'\n", x->set_image_mode);
00612 int tries = 1;
00613 for (;;)
00614 {
00615 if (write(x->fd, "I", 1) < 0)
00616 {
00617
00618 return 0;
00619 }
00620
00621 printf("srv1_fill_image(): getting spec.\n");
00622
00623 memset(specbuf, 0, 10);
00624 int done = read_limited(x->fd, specbuf, 10, 500000);
00625
00626 if (done != 10)
00627 {
00628 int btsdead = srv1_flush_input(x);
00629 if (tries < 10)
00630 {
00631 tries++;
00632
00633 continue;
00634 }
00635
00636 printf(
00637 "srv1_fill_image(): didn't get spec (got %d bytes: %s). flushed %d bytes.\n",
00638 done, specbuf, btsdead);
00639 return 0;
00640 }
00641 else
00642 {
00643 break;
00644 }
00645 }
00646
00647 if (strncmp("##IMJ", specbuf, 5) != 0)
00648 {
00649 int btsdead = srv1_flush_input(x);
00650 printf("srv1_fill_image(): incorrect response from I, flushed %d\n",
00651 btsdead);
00652 return 0;
00653 }
00654 int s0 = (unsigned char) specbuf[6];
00655 int s1 = (unsigned char) specbuf[7];
00656 int s2 = (unsigned char) specbuf[8];
00657 int s3 = (unsigned char) specbuf[9];
00658
00659 x->frame_size = s0 + (s1 * 256) + (s2 * 256 * 256) + (s3 * 256 * 256 * 256);
00660
00661 printf("srv1_fill_image(): specbuf: %c%c%c%c%c%c\n", specbuf[0], specbuf[1],
00662 specbuf[2], specbuf[3], specbuf[4], specbuf[5]);
00663 printf(
00664 "srv1_fill_image(): frame_size = %d + (%d * 256) + (%d * 256 * 256) + (%d * 256 * 256 * 256) = %d\n",
00665 s0, s1, s2, s3, x->frame_size);
00666
00667
00668
00669 if (x->frame == NULL)
00670 {
00671 x->frame = (char *) malloc(x->frame_size);
00672 }
00673 else
00674 {
00675 x->frame = (char *) realloc(x->frame, x->frame_size);
00676 }
00677
00678
00679 read_limited(x->fd, x->frame, x->frame_size, 1500000);
00680
00681
00682
00683
00684 return 1;
00685 }
00686
00687 int
00688 srv1_fill_ir(srv1_comm_t *x)
00689 {
00690 if (write(x->fd, "B", 1) < 0)
00691 {
00692 return 0;
00693 }
00694
00695 char buf[80];
00696 memset(buf, 0, 80);
00697 int done = read_limited(x->fd, buf, 46, 500000);
00698
00699 if (done != 46)
00700 {
00701
00702 int btsdead = srv1_flush_input(x);
00703 printf("srv1_fill_ir(): discarded %d bytes.\n", btsdead);
00704 return 0;
00705 }
00706
00707 if (strncmp(buf, "##BounceIR - ", 13) != 0)
00708 {
00709 int btsdead = srv1_flush_input(x);
00710 printf(
00711 "srv1_fill_ir(): IR return had wrong header '%s', discarded %d bytes.\n",
00712 buf, btsdead);
00713 return 0;
00714 }
00715
00716 sscanf(buf + 13, "%x", &(x->bouncedir[0]));
00717 sscanf(buf + 21, "%x", &(x->bouncedir[1]));
00718 sscanf(buf + 29, "%x", &(x->bouncedir[2]));
00719 sscanf(buf + 37, "%x", &(x->bouncedir[3]));
00720
00721 return 1;
00722 }
00723
00724 int
00725 srv1_read_sensors(srv1_comm_t *x)
00726 {
00727
00728
00729 srv1_fill_image(x);
00730
00731
00732 return 1;
00733 }
00734
00735 int
00736 srv1_reset_comms(srv1_comm_t *x)
00737 {
00738 return 1;
00739 }
00740
00741 double
00742 srv1_range_to_distance(int rangereading)
00743 {
00744
00745
00746
00747
00748 double a = -6.0333e-5 * rangereading * rangereading * rangereading;
00749 double b = 1.2986e-2 * rangereading * rangereading;
00750 double c = -9.6280e-1 * rangereading;
00751 double d = 4.3082e1;
00752
00753 return a + b + c + d;
00754 }
00755
00756
00757
00758 int
00759 saveNamedData(const char *name, char *data, int size)
00760 {
00761 int fd;
00762
00763 fd = open(name, O_TRUNC | O_CREAT | O_WRONLY, 00644);
00764 if (fd < 0)
00765 return 0;
00766
00767 if (write(fd, data, size) != size)
00768 {
00769 close(fd);
00770 return 0;
00771 }
00772 close(fd);
00773
00774 return 1;
00775 }
00776
00777 void
00778 savePhoto(const std::string aPrefix, char *data, int size, uint32_t aWidth)
00779 {
00780 static int nFrameNo = 0;
00781
00782 std::ostringstream filename;
00783 filename.imbue(std::locale(""));
00784 filename.fill('0');
00785
00786 filename << aPrefix << std::setw(aWidth) << nFrameNo++;
00787
00788 filename << ".jpg";
00789
00790
00791
00792
00793 saveNamedData(filename.str().c_str(), data, size);
00794 }
00795