bool run(void *p)
{
uint8_t slaveAddr = 0x1E;
char transmit = '0';
LPC_I2C2->I2ADR0 = slaveAddr;
if(xSemaphoreTake(dataRx,portMAX_DELAY))
{
//Transmit Master motion and direction data to the Bot
transmit = motion_detect();
uart3_TransferByte(transmit);
}
return true;
}
int
qcam_main( int argc, char ** argv )
{
struct qcam_softc *qs = NULL;
u_int options = 0;
int scan_size = QC_S80x60;
char *infile = "/dev/stdin";
char *outfile = "/dev/stdout";
FILE *infilef = NULL;
FILE *outfilef = NULL;
int ch, sizemult=0;
int pixel_threshold=0, frame_threshold=0;
int sleeptime = 0;
progname = argv[0];
signal( SIGBUS, sigbus_handler );
if (argc == 1)
usage();
while ((ch = getopt(argc, argv, "qm:os:pM:d:twx123z:")) != -1)
switch (ch) {
case 'q':
SET(CAMERA_IN);
break;
case 'm':
SET(MOVIE_IN);
infile = optarg;
break;
case 'o':
SET(ONE_IN);
break;
case 'p':
SET(PREVIEW_ONE);
break;
case 'M':
SET(MOVIE_OUT);
outfile = optarg;
break;
case 't':
SET(TTY_OUT);
break;
case 'w':
SET(TTY_OUT);
SET(TTY_WEB_OUT);
break;
case 'x':
SET(XWIN_OUT);
break;
case 's':
sleeptime = atoi(optarg);
break;
case 'd':
{
int d = atoi(optarg);
if (d == 1)
SET(CAM_DEBUG1);
else if (d == 2)
SET(CAM_DEBUG2);
else if (d != 0)
usage();
break;
}
case '1':
if (!sizemult)
sizemult=1;
scan_size = QC_S80x60;
break;
case '2':
if (sizemult)
sizemult=2;
else {
sizemult=1;
scan_size = QC_S160x120;
}
break;
case '3':
if (sizemult)
sizemult=3;
else {
sizemult=1;
scan_size = QC_S320x240;
}
break;
case 'z':
pixel_threshold = atoi(optarg);
frame_threshold = atoi(argv[optind]);
optind++;
SET(MOTION_DET);
break;
default:
usage();
}
argc -= optind;
argv += optind;
if (ISSET(MOTION_DET))
{
motion_detect(options, ISSET(MOVIE_OUT) ? outfile : NULL,
pixel_threshold, frame_threshold);
return 0;
}
if (!(ISSET(CAMERA_IN) || ISSET(MOVIE_IN)))
errx(1, "need input device");
if (!(ISSET(MOVIE_OUT) || ISSET(TTY_OUT) || ISSET(XWIN_OUT)))
errx(1, "need output device");
if (ISSET(MOVIE_IN) && ISSET(MOVIE_OUT))
errx(1, "file input and file output are mutually exclusive");
if (ISSET(PREVIEW_ONE) &&
!(ISSET(TTY_OUT) || ISSET(XWIN_OUT)))
errx(1, "need tty_out or xwin_out for preview");
if (ISSET(PREVIEW_ONE) && !(ISSET(MOVIE_OUT)))
errx(1, "need a file name to output snapshot to");
if (ISSET(CAMERA_IN))
{
qs = qcam_open(QCAM);
if (!qs)
errx(1, "cannot access quickcam");
qcam_setsize(qs, scan_size);
if (ISSET(CAM_DEBUG1))
qcam_debug = 1;
if (ISSET(CAM_DEBUG2))
qcam_debug = 2;
}
setuid(getuid());
if (ISSET(MOVIE_IN))
{
int l = strlen(infile);
qs = qcam_new();
if (strcmp(infile, "-") == 0)
infilef = stdin;
else if ((l > 3 && strcmp(infile+l-3, ".gz") == 0) ||
(l > 4 && strcmp(infile+l-4, ".bz2") == 0))
{
int fd, pid, pipefds[2];
pipe(pipefds);
fd = open(infile, O_RDONLY);
if (fd < 0)
goto error_opening;
pid = fork();
if (pid > 0)
{
/* in parent */
close(fd);
close(pipefds[1]);
infilef = fdopen(pipefds[0], "r");
}
if (pid == 0)
{
/* in child */
close(pipefds[0]);
if (pipefds[1] != 1)
{
dup2(pipefds[1], 1);
close(pipefds[1]);
}
if (fd != 0)
{
dup2(fd, 0);
close(fd);
}
if (strcmp(infile+l-3, ".gz") == 0)
execl("/usr/bin/gzip", "gzip", "-dc", NULL);
else
execl("/usr/bin/bzip2", "bzip2", "-dc", NULL);
close(0);
close(1);
errx(1, "cannot exec");
}
if (pid < 0)
{
/* busted */
errx(1, "cannot fork");
}
}
else
infilef = fopen(infile, "r");
if (!infilef)
error_opening:
errx(1, "cannot open input file");
pgm_readhdr(qs, infilef);
}
if (ISSET(MOVIE_OUT))
{
if (strcmp(outfile, "-") == 0)
outfilef = stdout;
else
outfilef = fopen(outfile, "w");
if (!outfilef)
errx(1, "cannot open output file");
pgm_writehdr(qs, outfilef);
}
if (ISSET(TTY_OUT))
ttydisp_init(qs,ISSET(TTY_WEB_OUT));
if (ISSET(XWIN_OUT) && !ISSET(PREVIEW_ONE))
{
if (!sizemult)
sizemult=1;
xwindisp_init(qs, sizemult);
}
if (ISSET(ONE_IN) && ISSET(PREVIEW_ONE))
{
qcam_setsize(qs, QC_S80x60);
if (ISSET(XWIN_OUT))
xwindisp_init(qs, 1);
do {
qcam_scan(qs);
qcam_adjustpic(qs);
if (ISSET(TTY_OUT))
ttydisp(qs,ISSET(TTY_WEB_OUT));
if (ISSET(XWIN_OUT))
xwindisp(qs);
} while (!keyinput());
qcam_setsize(qs, scan_size);
qcam_scan(qs);
pgm_write(qs, outfilef);
} else if (ISSET(ONE_IN) && !ISSET(MOVIE_IN))
{
qcam_setsize(qs, QC_S80x60);
do {
qcam_scan(qs);
} while (qcam_adjustpic(qs));
qcam_setsize(qs, scan_size);
qcam_scan(qs);
if (ISSET(MOVIE_OUT))
pgm_write(qs, outfilef);
if (ISSET(TTY_OUT))
ttydisp(qs,ISSET(TTY_WEB_OUT));
} else {
int done = 0;
if (ISSET(CAMERA_IN) && ISSET(MOVIE_OUT))
{
qcam_setsize(qs, QC_S80x60);
do {
qcam_scan(qs);
} while (qcam_adjustpic(qs));
qcam_setsize(qs, scan_size);
}
while (!done)
{
if ( sleeptime != 0 )
usleep( sleeptime );
if (ISSET(MOVIE_IN))
{
pgm_read(qs, infilef);
if (feof(infilef))
done++;
}
if (ISSET(CAMERA_IN))
{
qcam_scan(qs);
qcam_adjustpic(qs);
}
if (ISSET(MOVIE_OUT))
pgm_write(qs, outfilef);
if (ISSET(TTY_OUT))
{
ttydisp(qs,ISSET(TTY_WEB_OUT));
if (ISSET(MOVIE_IN))
usleep(200000);
}
if (ISSET(XWIN_OUT))
{
xwindisp(qs);
if (ISSET(MOVIE_IN))
usleep(200000);
}
if (ISSET(ONE_IN))
{
char dummy;
done++;
if (ISSET(MOVIE_IN))
read(0, &dummy, 1);
}
if (!ISSET(MOVIE_IN) && keyinput())
done++;
}
}
if (ISSET(MOVIE_IN))
fclose(infilef);
if (ISSET(MOVIE_OUT))
fclose(outfilef);
if (ISSET(XWIN_OUT))
xwindisp_close();
if (ISSET(CAMERA_IN))
qcam_close(qs);
return 0;
}
void logic_loop()
{
printf("f_logic = %d\n", f_logic);
if (f_logic){
struct timeval start;
gettimeofday(&start, 0);
printf("logic is enabled\n");
int width = 640;
int height = 480;
if (!pic1){
pic1 = malloc(sizeof(BMP));
memset(pic1, 0, sizeof(BMP));
BMP_init(pic1, width, height);
}
if (!pic2){
pic2 = malloc(sizeof(BMP));
memset(pic2, 0, sizeof(BMP));
BMP_init(pic2, width, height);
}
if (take_both) {
take_pic(pic1);
take_pic(pic2);
take_both = 0;
} else {
BMP* bmp = pic2;
pic2 = pic1;
take_pic(bmp);
pic1 = bmp;
}
DetectionDiff* diff = motion_detect(pic1, pic2, 50, 5);
float angles[2];
if (calc_rotation(diff, angles)) {
struct timeval end;
gettimeofday(&end, 0);
long tdiff = end.tv_sec*1e3 + end.tv_usec/1e3 - start.tv_sec*1e3 - start.tv_usec/1e3;
printf("cycle length in milliseconds: %ld\n", tdiff);
/*
motion_detect_mark_frame(pic1, diff);
motion_detect_mark_frame(pic2, diff);
motion_detect_mark_frame(diff->first_pass, diff);
motion_detect_mark_frame(diff->second_pass, diff);
char name1[256] = {0};
char name2[256] = {0};
char name3[256] = {0};
char name4[256] = {0};
snprintf(name1, 256, "%d_pic1.bmp", itid);
snprintf(name2, 256, "%d_pic2.bmp", itid);
snprintf(name3, 256, "%d_first.bmp", itid);
snprintf(name4, 256, "%d_second.bmp", itid);
++itid;
BMP_write(pic1, name1);
BMP_write(pic2, name2);
BMP_write(diff->first_pass, name3);
BMP_write(diff->second_pass, name4);
*/
printf("detected motion, horizontal rotation = %f radians\n", angles[0]);
printf("detected motion, vertical rotation = %f radians\n", angles[1]);
if (!is_playing()) {
play_random_sample();
}
if (is_playing()) {
int slept = 0;
int max_sleep = 3000;
while (slept < max_sleep && is_playing()) {
delayms(100);
slept += 100;
}
if (is_playing()) {
kill(0, SIGINT);
}
}
rotate_x(angles[0]);
take_both = 1;
}
else{
struct timeval end;
gettimeofday(&end, 0);
long tdiff = end.tv_sec*1e3 + end.tv_usec/1e3 - start.tv_sec*1e3 - start.tv_usec/1e3;
printf("cycle length in milliseconds: %ld\n", tdiff);
}
BMP_free(diff->first_pass);
BMP_free(diff->second_pass);
free(diff);
} else {
printf("logic is disabled\n");
sleep(1);
}
}