void ram(void)
{
int c=0, pos=0,del=0;
struct pos_s tail[MAX_SNAKE_LEN];
snake.tail = tail;
// load the highscore
highscore = highscore_get();
// initially reset everything
reset();
while (1) {
if(!(++c % snake.speed)) {
handle_input();
pos = (snake.t_start+1) % MAX_SNAKE_LEN;
snake.tail[pos].x = snake.tail[snake.t_start].x;
snake.tail[pos].y = snake.tail[snake.t_start].y;
if(snake.dir == 0)
snake.tail[pos].x++;
else if(snake.dir == 1)
snake.tail[pos].y++;
else if(snake.dir == 2)
snake.tail[pos].x--;
else if(snake.dir == 3)
snake.tail[pos].y--;
snake.t_start = pos;
if (pos < snake.len) {
del = MAX_SNAKE_LEN - (snake.len - pos);
} else
del = pos - snake.len;
// remove last, add first line
draw_block(snake.tail[del].x, snake.tail[del].y, 0xFF);
draw_block(snake.tail[pos].x, snake.tail[pos].y, 0b00011000);
// check for obstacle hit..
if (hitWall() || hitSelf()) {
death_anim();
if (showHighscore())
break;
reset();
} else if (hitFood())
next_level();
lcdDisplay();
}
#ifdef SIMULATOR
delayms(50);
#else
delayms(3);
#endif
}
}
void startSlam(int v)
{
FileStorage fs("config.yml", FileStorage::READ);
Mat cam, dist, rpos, q, s;
fs["cameraMatrix"] >> cam;
fs["distMatrix"] >> dist;
fs["sensorPos"] >> rpos;
fs["q"] >> q;
fs["s"] >> s;
fs.release();
ml = new MarkerLocator(cam, dist, rpos, 100.);
// Mat q = (Mat_<double>(2,1) << .01, .1*M_PI/180);
// Mat s = (Mat_<double>(2,1) << 10, .5*M_PI/180);
slam = new EkfSlam(&scan, q, s);
#ifdef GUI
namedWindow("EKFSlam");
disp = new MapDisplay("Map");
waitKey();
#endif
Mat u = (Mat_<double>(2,1) << 0, 0);
int fd = openPort();
oiStart();
setSafeMode();
readDist();
readAngle();
VideoCapture cap(0);
if(!cap.isOpened()) {
printf("Failed capture\n");
return;
}
VideoWriter record("/tmp/demo.avi", CV_FOURCC('D','I','V','X'), 30, Size(1340,600), true);
if(!record.isOpened()) {
printf("Failed writer\n");
return;
}
clock_t t;
time_t stime;
while(1) {
t = clock();
setDrive(v,v);
double mindist = 99999;
while(msElap(t) < 100 && !hitWall()) {
u.at<double>(0) = (double)readDist();
u.at<double>(1) = M_PI*(double)readAngle()/180;
mindist = slamStep(u, cap, record);
}
// backup before turn when running into something
if(hitWall() || mindist < 700) {
if(hitWall()) {
// backup
setDrive(-15,-15);
t = clock();
while(msElap(t) < 500) {
u.at<double>(0) = (double)readDist();
u.at<double>(1) = M_PI*(double)readAngle()/180;
slamStep(u, cap, record);
}
}
// turn
if(time(&stime)%60 < 30) {
setDrive(-5, 5);
}
else
setDrive(5,-5);
t = clock();
while(msElap(t) < 250) {
u.at<double>(0) = 0;
u.at<double>(1) = M_PI*(double)readAngle()/180;
slamStep(u, cap, record);
}
}
if(waitKey(10) == 27)
break;
}
setDrive(0,0);
imwrite("lastFrame.png", g_outFrame);
close(fd);
delete ml;
delete slam;
#ifdef GUI
delete disp;
#endif
}
