// Initials the game with map indication: 0 for road, 1 for rock, 2 for the tank.
// Prints out the map once it has been created
void playGame() {
//map: Matrix (8 * 8) Start: (7, 0)
carRow = 7;
carCol = 0;
direction = 0;
int row;
for (row = 0; row < 8; row++) {
int col;
for (col = 0; col <8; col++) {
map[row][col] = 0;
}
}
map[carRow][carCol] = 2;
unsigned char start[] = "Game Start! Avoid the booms!";
info(start);
sleep(1);
//Set the position to be 1 (aka bomb)
int i;
for (i = 0; i < 15; i++) {
//start from bottom left corner
int randRow = 7;
int randCol = 0;
while (((randRow > 5) && (randCol < 2 )) || ((randRow > 5 && randCol > 5))) {
randRow = rand() % 8;
randCol = rand() % 8;
}
map[randRow][randCol] = 1;
}
showMap();
}
void GameSession::run(LevelDefinitionBase* level) {
_levelDefinition = level;
startLevel();
const Pad& pad = Hardware::getPad();
while (isRunning()) {
checkLevelComplete();
animate();
timer();
move();
if (pad.isStartNewPress()) {
pause();
} else if (pad.isSelectNewPress() && _isMapAvailable) {
showMap();
} else if (pad.isLHeld() && pad.isRHeld()) {
commitSuicide();
}
Hardware::waitForVBlank();
}
}
void ModelContainerView::showMap(int pMapId, int x, int y) {
MapTree* mt = iVMapManager->getInstanceMapTree(pMapId);
std::string dirFileName = iVMapManager->getDirFileName(pMapId);
if(!mt->hasDirFile(dirFileName)) {
dirFileName = iVMapManager->getDirFileName(pMapId, x, y);
}
showMap(mt,dirFileName);
iInstanceId = pMapId;
}
int main (int argc, char **argv)
{
srand(time(0));
MAP map;
setDefaults(&map);
PLAYER hero;
startPlayer(&hero);
getargs(&map, &hero, argc, argv);
startMap(&map);
hero.face = map.elements.hero.face;
hero.pos = putElement(&map, hero.face);
GHOSTS ghosts;
ghosts.face = GHOST;
ghosts.count = 0;
startGhosts(&map, &ghosts);
fflush(stdout);
initscr();
nodelay(stdscr, TRUE);
noecho();
keypad(stdscr, TRUE);
if (map.props.hascolor)
startColors();
struct timespec tim, tim2;
tim.tv_sec = 0;
tim.tv_nsec = map.props.speed;
do {
nanosleep(&tim, &tim2);
getCommand(&hero);
walk(&map, &hero, &ghosts);
showMap(&map, &hero, &ghosts);
repopMap(&map);
upGhosts(&map, &ghosts);
walkGhosts(&map, &ghosts);
} while (!isDead(&hero));
if (!noRecords)
writeRecords(&hero, &ghosts);
finalize(&map, &ghosts);
finalText(&map, &hero, &ghosts);
exit(0);
}
MapDisplay::MapDisplay()
: Display()
, manual_object_( NULL )
, loaded_( false )
, resolution_( 0.0f )
, width_( 0 )
, height_( 0 )
{
connect(this, SIGNAL( mapUpdated() ), this, SLOT( showMap() ));
topic_property_ = new RosTopicProperty( "Topic", "",
QString::fromStdString( ros::message_traits::datatype<nav_msgs::OccupancyGrid>() ),
"nav_msgs::OccupancyGrid topic to subscribe to.",
this, SLOT( updateTopic() ));
alpha_property_ = new FloatProperty( "Alpha", 0.7,
"Amount of transparency to apply to the map.",
this, SLOT( updateAlpha() ));
alpha_property_->setMin( 0 );
alpha_property_->setMax( 1 );
color_scheme_property_ = new EnumProperty( "Color Scheme", "map", "How to color the occupancy values.",
this, SLOT( updatePalette() ));
// Option values here must correspond to indices in palette_textures_ array in onInitialize() below.
color_scheme_property_->addOption( "map", 0 );
color_scheme_property_->addOption( "costmap", 1 );
draw_under_property_ = new Property( "Draw Behind", false,
"Rendering option, controls whether or not the map is always"
" drawn behind everything else.",
this, SLOT( updateDrawUnder() ));
resolution_property_ = new FloatProperty( "Resolution", 0,
"Resolution of the map. (not editable)", this );
resolution_property_->setReadOnly( true );
width_property_ = new IntProperty( "Width", 0,
"Width of the map, in meters. (not editable)", this );
width_property_->setReadOnly( true );
height_property_ = new IntProperty( "Height", 0,
"Height of the map, in meters. (not editable)", this );
height_property_->setReadOnly( true );
position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO,
"Position of the bottom left corner of the map, in meters. (not editable)",
this );
position_property_->setReadOnly( true );
orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY,
"Orientation of the map. (not editable)",
this );
orientation_property_->setReadOnly( true );
}
bool ModelContainerView::loadAndShowTile(int pMapId, int x, int y) {
char buffer[500];
sprintf(buffer, "%s/vmaps",gDataDir);
bool result = false;
//if(pMapId == 1) return true; //+++
int val = iVMapManager->loadMap(buffer,(unsigned int) pMapId, x,y);
if(val == VMAP_LOAD_RESULT_OK) {
result = true;
showMap(pMapId,x,y);
} else {
printf("Unable to load %s\n", buffer);
}
return(result);
}
bool ChooseMapState::onNextMap(const CEGUI::EventArgs& e)
{
static MapManager *mapMgr = MapManager::getSingletonPtr();
if (mCurrentMapIndex + 1 < mMaxMapNumber)
mCurrentMapIndex++;
AudioManager::play(AUDIO_MOUSE_CLICK, 0.5f, false, false);
int maxsize = mapMgr->getMapPreSize();
if (maxsize>mCurrentMapIndex)
{
showMap(mapMgr->getMapPreByIndex(mCurrentMapIndex));
}
return true;
}
bool ModelContainerView::loadAndShowTile(int pMapId) {
char buffer[500];
sprintf(buffer, "%s/vmaps",gDataDir);
bool result = false;
int val = iVMapManager->loadMap(buffer, (unsigned int) pMapId,-1,-1);
if(val == VMAP_LOAD_RESULT_OK) {
result = true;
MapTree* mt = iVMapManager->getInstanceMapTree(pMapId);
std::string dirFileName = iVMapManager->getDirFileName(pMapId);
iTriVarTable = Table<std::string , VAR*>();
iTriIndexTable = Table<std::string , Array<int> >(); // reset table
iInstanceId = pMapId;
showMap(mt,dirFileName);
}
return(result);
}
// enter status
void ChooseMapState::enter( void )
{
mSceneMgr = GameApp::getSingletonPtr()->getAideSceneMgr();
mRenderWindow = GameApp::getSingletonPtr()->getRenderWindow();
GUIManager::loadWindow("ChooseMap");
GUIManager::setCursorType(CURSOR_NORMAL);
createGUIEvent();
createCamera();
mMaxMapNumber = MapManager::getSingletonPtr()->getMapPreSize();
if (mMaxMapNumber>mCurrentMapIndex && mCurrentMapIndex>=0)
{
showMap(MapManager::getSingletonPtr()->getMapPreByIndex(mCurrentMapIndex));
}
}
void runMainLoop()
{
while(nh_.ok())
{
//cv::imshow(MAP_WINDOW_NAME, map_img_pos_curr_);
int key = cv::waitKey(CV_WAIT_KEY_RATE);
if(key >= 0)
state_ = waitKeyJudge(key);
if(state_ == QUIT)
break;
else
showMap();
ros::spinOnce();
rate_.sleep();
}
}
int main(int argc, const char * argv[]) {
while (1) {
system("clear");
// 1. 根据map数据打印出地图
showMap();
// 2. 接收输入小人的前进方向
char dir = enterDirection();
// 3. 根据小人前进的方向，来移动小人或者推箱子
if(dir == 'q'){
printf("Game Over ~ ");
return 0;
}else{
movePerson(dir);
}
}
return 0;
}
int main(){
int i = 0;
int mapCount = 0, clearMapCount = 0, dumpCount=0;
int revFrameCount = 0;
#ifdef USE_NORTH
targetsGPS[maxTargets].lat = ADVANCED5LAT;
targetsGPS[maxTargets].lon = ADVANCED5LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED6LAT;
targetsGPS[maxTargets].lon = ADVANCED6LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED7LAT;
targetsGPS[maxTargets].lon = ADVANCED7LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED8LAT;
targetsGPS[maxTargets].lon = ADVANCED8LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED2LAT;
targetsGPS[maxTargets].lon = ADVANCED2LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED1LAT;
targetsGPS[maxTargets].lon = ADVANCED1LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED3LAT;
targetsGPS[maxTargets].lon = ADVANCED3LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED12LAT;
targetsGPS[maxTargets].lon = ADVANCED12LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED4LAT;
targetsGPS[maxTargets].lon = ADVANCED4LON;
maxTargets++;
#else
targetsGPS[maxTargets].lat = ADVANCED4LAT;
targetsGPS[maxTargets].lon = ADVANCED4LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED1LAT;
targetsGPS[maxTargets].lon = ADVANCED1LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED2LAT;
targetsGPS[maxTargets].lon = ADVANCED2LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED3LAT;
targetsGPS[maxTargets].lon = ADVANCED3LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED11LAT;
targetsGPS[maxTargets].lon = ADVANCED11LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED8LAT;
targetsGPS[maxTargets].lon = ADVANCED8LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED7LAT;
targetsGPS[maxTargets].lon = ADVANCED7LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED6LAT;
targetsGPS[maxTargets].lon = ADVANCED6LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED11LAT;
targetsGPS[maxTargets].lon = ADVANCED11LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED5LAT;
targetsGPS[maxTargets].lon = ADVANCED5LON;
maxTargets++;
#endif
maxTargetIndex=maxTargets-1;
for(i=0;i<maxTargets;i++){// this is converting all GPS point data to XY data.
targetListXY[i].x = GPSX(targetsGPS[i].lon, startLongitude);
targetListXY[i].y = GPSY(targetsGPS[i].lat, startLatitude);
}
currentXY.x = GPSX(gpsvar.longitude,startLongitude);// converts current robot X location compared to start longitude
currentXY.y = GPSY(gpsvar.latitude,startLatitude);// converts current robot Y location compared to start latitude
targetXY = targetListXY[currentTargetIndex];//sets first target GPS point
nextTargetIndex = (currentTargetIndex + 1)%maxTargets;//sets next target GPS point
nextXY = targetListXY[nextTargetIndex];// ??
previousXY.x = GPSX(startLongitude, startLongitude);// why?
previousXY.y = GPSY(startLatitude, startLatitude);//Why?
initRoboteq(); /* Initialize roboteq */
initGuide();//what is guide?
#ifdef USE_VISION // if USE_vision is defined, then initialize vision.
initVision();
#endif //USE_VISION
#ifdef USE_GPS// if USE_GPS is defined, then initialize GPS.
initGPS();
initParser();
#endif //USE_GPS
#ifdef USE_LIDAR// if USE_LIDAR is defined, then initialize LIDAR.
initObjects();
initSICK();
#endif //USE_LIDAR
#ifdef DEBUG_VISUALIZER// if defined, then use visualizer.
initVisualizer();
#endif //DEBUG_VISUALIZER
#ifdef USE_MAP//////>>>>>>>>>>>????
initMap(0,0,0);
#endif //USE_MAP
#ifdef DUMP_GPS// dump GPS data into file
FILE *fp;
fp = fopen("gpsdump.txt", "w");
#endif // DUMP_GPS
while(1){
double dir = 1.0;
double speed = 0.0, turn = 0.0;
static double turnBoost = 0.750;//Multiplier for turn. Adjust to smooth jerky motions. Usually < 1.0
static int lSpeed = 0, rSpeed = 0;//Wheel Speed Variables
if (joystick() != 0) {// is joystick is connected
if (joy0.buttons & LB_BTN) {// deadman switch, but what does joy0.buttons do?????????????????????????????????
speed = -joy0.axis[1]; //Up is negative on joystick negate so positive when going forward
turn = joy0.axis[0];
lSpeed = (int)((speed + turnBoost*turn)*maxSpeed);//send left motor speed
rSpeed = (int)((speed - turnBoost*turn)*maxSpeed);//send right motor speed
}else{ //stop the robot
rSpeed=lSpeed=0;
}
if(((joy0.buttons & B_BTN)||autoOn)&& (saveImage==0)){//what is the single & ???????????????????
saveImage =DEBOUNCE_FOR_SAVE_IMAGE;//save each image the camera takes, save image is an int declared in vision_nav.h
}else{
if (saveImage) saveImage--; // turn off if button wasn't pressed?
}
if(joy0.buttons & RB_BTN){//turn on autonmous mode if start??? button is pressed
autoOn = 1;
mode=1;
}
if(joy0.buttons & Y_BTN){ // turn off autonomous mode
autoOn = 0;
mode =0;
}
lastButtons = joy0.buttons;//is this just updating buttons?
} else{
// printf("No Joystick Found!\n");
rSpeed=lSpeed=0;
}
//
// printf("3: %f %f\n",BASIC3LAT,BASIC3LON);
// printf("4: %f %f\n",BASIC4LAT,BASIC4LON);
// printf("5: %f %f\n",BASIC5LAT,BASIC5LON);
// getchar();
#ifdef AUTO_SWAP//what is this
if((currentTargetIndex>1&&targetIndexMem!=currentTargetIndex)||!autoOn||!mode==3){
startTime=currentTime=(float)(clock()/CLOCKS_PER_SEC);
targetIndexMem = currentTargetIndex;
}else{
currentTime=(float)(clock()/CLOCKS_PER_SEC);
}
totalTime = currentTime-startTime;
if(totalTime>=SWAPTIME&&autoOn){
swap();
targetIndexMem = 0;
}
#endif //AUTO_SWAP
#ifdef USE_GPS
readGPS();
currentXY.x = GPSX(gpsvar.longitude,startLongitude);
currentXY.y = GPSY(gpsvar.latitude,startLatitude);
robotTheta = ADJUST_RADIANS(DEG2RAD(gpsvar.course));
#else
currentXY.x = 0.0;
currentXY.y = 0.0;
robotTheta = 0.0;
#endif //USE_GPS
if(autoOn&&!flagPointSet){//this whole thing?????
flagXY.x=currentXY.x+FLAG_X_ADJUST;
flagXY.y=currentXY.y;
flagPointSet=1;
startAutoTime=currentAutoTime=(float)(clock()/CLOCKS_PER_SEC);
}
if(autoOn){
currentAutoTime=(float)(clock()/CLOCKS_PER_SEC);
totalAutoTime = currentAutoTime-startAutoTime;
if(totalAutoTime>=MODE2DELAY){
mode1TimeUp=1;//what is mode1 time up?
}
printf("TIMEING\n");
}
// if(currentTargetIndex <= OPEN_FIELD_INDEX || currentTargetIndex >= maxTargetIndex){
if(currentTargetIndex <= OPEN_FIELD_INDEX){//if you are on your last target, then set approaching thresh, and dest thresh to larger values?
//OPEN_FIELD_INDEX is set to 0 above...?
approachingThresh=4.0;
destinationThresh=3.0;
}else{//otherwise set your thresholds to a bit closer.
// destinationThresh=1.0;
destinationThresh=0.75;
approachingThresh=2.5;
}
//mode1 = lane tracking and obstacle avoidance. mode 2 = vision, lane tracking, but guide to gps. its not primary focus.
//mode3= gps mode in open field, but vision is toned down to not get distracted by random grass.
//mode 4= flag tracking
if(guide(currentXY, targetXY, previousXY, nextXY, robotTheta, robotWidth, 1)&& !allTargetsReached){//If target reached and and not all targets reached
printf("REACHED TARGET\n");
initGuide();// reset PID control stuff. problably resets all control variables.
previousXY = targetXY;//update last target
if(currentTargetIndex == maxTargetIndex){ //seeing if you are done with all targets.
allTargetsReached = 1;
}else{//otherwise update all the target information
currentTargetIndex = (currentTargetIndex + 1);
nextTargetIndex = (currentTargetIndex + 1)% maxTargets;
targetXY = targetListXY[currentTargetIndex];
nextXY = targetListXY[nextTargetIndex];
}
}
if((autoOn&&(currentTargetIndex == 0&&!approachingTarget&&!mode1TimeUp))||allTargetsReached){
//if autonomous, and on first target, and not not approaching target, and not mode 1 time up, or reached last target.
mode =1;//wtf is mode
distanceMultiplier = 50;//wthis is how heavily to rely on vision
} else if((autoOn&¤tTargetIndex == 0&&mode1TimeUp)||(autoOn&&approachingTarget&&(currentTargetIndex<=OPEN_FIELD_INDEX||currentTargetIndex>=maxTargetIndex-END_LANE_INDEX))){
mode =2;
distanceMultiplier = 50;
} else if((autoOn&¤tTargetIndex!=0)){
mode =3;
distanceMultiplier = 12;
}
flagPointDistance = D((currentXY.x-flagXY.x),(currentXY.y-flagXY.y));// basically the distance formula, but to what? what flags GPS point?
if(allTargetsReached&&flagPointDistance<FLAG_DIST_THRESH){
mode =4;// what is mode
}
#ifdef FLAG_TESTING
/*FLAG TESTING*/
mode=4;
#endif //FLAG_TESTING
/*Current Target Heading PID Control Adjustment*/
cvar.lookAhead = 0.00;//?
cvar.kP = 0.20; cvar.kI = 0.000; cvar.kD = 0.15;
turn = cvar.turn;
int bestVisGpsMask = 99;
int h = 0;
double minVisGpsTurn = 9999;
for(h=0;h<11;h++){
if(fabs((cvar.turn-turn_angle[h]))<minVisGpsTurn){
minVisGpsTurn=fabs((cvar.turn-turn_angle[h]));
bestVisGpsMask = h;
}
}
bestGpsMask = bestVisGpsMask;
// printf("bvg: %d \n", bestVisGpsMask);
// printf("vgt: %f cv3: %f\n", minVisGpsTurn,cvar3.turn);
#ifdef USE_VISION
// double visTurnBoost = 0.50;
double visTurnBoost = 1.0;
if(imageProc(mode) == -1) break;
if(mode==1||mode==2){
turn = turn_angle[bestmask];
turn *= visTurnBoost;
}else if(mode==3 && fabs(turn_angle[bestmask])>0.70){
turn = turn_angle[bestmask];
turn *= visTurnBoost;
}
#endif //USE_VISION
#ifdef USE_LIDAR
updateSick();
// findObjects();
#endif //USE_LIDAR
#ifdef USE_COMBINED_BUFFER//??????????
#define WORSTTHRESH 10
#define BESTTHRESH 3
if(mode==4){
#ifdef USE_NORTH
turn = (0.5*turn_angle[bestBlueMask]+0.5*turn_angle[bestRedMask]);
#else
turn = (0.65*turn_angle[bestBlueMask]+0.35*turn_angle[bestRedMask]);
#endif
turn *= 0.75;
}
combinedTargDist = cvar.targdist;
if(((approachingTarget||inLastTarget)&¤tTargetIndex>OPEN_FIELD_INDEX
&¤tTargetIndex<maxTargetIndex-END_LANE_INDEX)||(MAG(howbad[worstmask]-howbad[bestmask]))<BESTTHRESH||mode==4){
getCombinedBufferAngles(0,0);//Don't Use Vision Radar Data
}else{
getCombinedBufferAngles(0,1);//Use Vision Radar Data
}
if(combinedBufferAngles.left != 0 || combinedBufferAngles.right !=0){
if(mode == 1 || mode==2 || mode==3 || mode==4){
// if(mode == 1 || mode==2 || mode==3){
// if(mode==2 || mode==3){
// if(mode==3){
if(fabs(combinedBufferAngles.right)==fabs(combinedBufferAngles.left)){
double revTurn;
double revDistLeft, revDistRight;
int revIdx;
if(fabs(turn)<0.10) dir = -1.0;
if(fabs(combinedBufferAngles.left)>1.25) dir = -1.0;
if(dir<0){
revIdx = 540-RAD2DEG(combinedBufferAngles.left)*4;
revIdx = MIN(revIdx,1080);
revIdx = MAX(revIdx,0);
revDistLeft = LMSdata[revIdx];
revIdx = 540-RAD2DEG(combinedBufferAngles.right)*4;
revIdx = MIN(revIdx,1080);
revIdx = MAX(revIdx,0);
revDistRight = LMSdata[revIdx];
if(revDistLeft>=revDistRight){
revTurn = combinedBufferAngles.left;
}else {
revTurn = combinedBufferAngles.right;
}
turn = revTurn;
}else{
turn = turn_angle[bestmask];
}
} else if(fabs(combinedBufferAngles.right-turn)<fabs(combinedBufferAngles.left-turn)){
// } else if(turn<=0){
turn = combinedBufferAngles.right;
}else {
turn = combinedBufferAngles.left;
}
}
}
#endif //USE_COMBINED_BUFFER
if(dir<0||revFrameCount!=0){
dir = -1.0;
revFrameCount = (revFrameCount+1)%REVFRAMES;
}
// turn *= dir;
turn = SIGN(turn) * MIN(fabs(turn), 1.0);
speed = 1.0/(1.0+1.0*fabs(turn))*dir;
speed = SIGN(speed) * MIN(fabs(speed), 1.0);
if(!autoOn){
maxSpeed = 60;
targetIndexMem = 0;
}else if(dir<0){
maxSpeed = 30;
}else if(mode<=2||(mode==3 && fabs(turn_angle[bestmask])>0.25)){
maxSpeed = 60 - 25*fabs(turn);
// maxSpeed = 70 - 35*fabs(turn);
// maxSpeed = 90 - 50*fabs(turn);
// maxSpeed = 100 - 65*fabs(turn);
}else if(mode==4){
maxSpeed = 45-20*fabs(turn);
}else{
maxSpeed = 85 - 50*fabs(turn);
// maxSpeed = 100 - 65*fabs(turn);
// maxSpeed = 110 - 70*fabs(turn);
// maxSpeed = 120 - 85*fabs(turn);
}
if(autoOn){
lSpeed = (speed + turnBoost*turn) * maxSpeed;
rSpeed = (speed - turnBoost*turn) * maxSpeed;
}
#ifdef DEBUG_MAIN
printf("s:%.4f t: %.4f m: %d vt:%f dir:%f tmr: %f\n", speed, turn, mode, turn_angle[bestmask], flagPointDistance, totalAutoTime);
#endif //DEBUG_MAIN
#ifdef DUMP_GPS
if(dumpCount==0){
if (fp != NULL) {
fprintf(fp, "%f %f %f %f %f\n",gpsvar.latitude,gpsvar.longitude, gpsvar.course, gpsvar.speed, gpsvar.time);
}
}
dumpCount = dumpCount+1%DUMPGPSDELAY;
#endif //DUMP_GPS
#ifdef DEBUG_TARGET
debugTarget();
#endif //DEBUG_TARGET
#ifdef DEBUG_GUIDE
debugGuide();
#endif //DEBUG_GUIDE
#ifdef DEBUG_GPS
debugGPS();
#endif //DEBUG_GPS
#ifdef DEBUG_LIDAR
debugSICK();
#endif //DEBUG_LIDAR
#ifdef DEBUG_BUFFER
debugCombinedBufferAngles();
#endif //DEBUG_BUFFE
#ifdef DEBUG_VISUALIZER
robotX = currentXY.x;
robotY = currentXY.y;
robotTheta = robotTheta;//redundant I know....
targetX = targetXY.x;
targetY = targetXY.y;
// should probably pass the above to the function...
paintPathPlanner(robotX,robotY,robotTheta);
showPlot();
#endif //VISUALIZER
#ifdef USE_MAP
if(mapCount==0){
// mapRobot(currentXY.x,currentXY.y,robotTheta);
if(clearMapCount==0) clearMapSection(currentXY.x,currentXY.y,robotTheta);
else clearMapCount = (clearMapCount+1)%CLEARMAPDELAY;
mapVSICK(currentXY.x,currentXY.y,robotTheta);
// mapVSICK(0,0,0);
#ifdef USE_LIDAR
mapSICK(currentXY.x,currentXY.y,robotTheta);
#endif
showMap();
// printf("MAPPING\n");
}
mapCount= (mapCount+1)%MAPDELAY;
#endif //USE_MAP
sendSpeed(lSpeed,rSpeed);
Sleep(5);
}
#ifdef DUMP_GPS
fclose(fp);
#endif
return 0;
}
// Moves backwards in current direction for 0.5 second.
// The direction variable for the game will also be updated
// Possible displays on LCD
// 1. Current position will be printed out on LCD
// 2. If the tank goes out of the range of internal game map,
// the movement will be rejected and tank will display "Out of board"
// 3. If the tank hit a rock on the game map, LCD displays "!!!Boom!!!"
// 4. If the tank is too close to obstacle in REAL world, LCD displays "Blocked!!"
void moveBackwards() {
if(direction == 0 ){
carRow++;
} else if (direction == 1){
carCol--;
} else if (direction == 2){
carRow--;
} else {
carCol++;
}
unsigned char table[100];
if((carRow > 7) || (carRow < 0) || (carCol > 7) || (carCol < 0)){
sprintf(table, "%s", "Out of board");
if(direction == 0 ){
carRow--;
} else if (direction == 1){
carCol++;
} else if (direction == 2){
carRow++;
} else {
carCol--;
}
} else if(map[carRow][carCol] == 1) {
sprintf(table, "%s", "!!!Boom!!!");
info(table);
gameOver();
} else if((carRow == 7) && (carCol == 7)){
win();
} else {
if(direction == 0 ){
map[carRow-1][carCol] = 0;
} else if (direction == 1){
map[carRow][carCol+1] = 0;
} else if (direction == 2){
map[carRow+1][carCol] = 0;
} else {
map[carRow][carCol-1] = 0;
}
int n = 0;
while( n < 100) {
if (sensorDistance(path4) < 3000){
sprintf(table, "%s","Position: ");
char row[15];
sprintf(row, "%d", carRow);
strcat(table, row);
strcat(table, ", ");
char col[15];
sprintf(col, "%d", carCol);
strcat(table, col);
controlLeft(1, 0);
controlRight(0, 1);
usleep(10000);
} else {
sprintf(table, "%s", "Blocked!!");
}
n++;
}
map[carRow][carCol] = 2;
}
info(table);
showMap();
usleep(500000);
stop();
}
void doGridLearning() {
int i, j, k, moves, oldx, oldy, reward, ret;
int count = 0;
int up, down, left, right;
for( count; count < EPISODES; count++ ) {
episodeNumber++;
//randomly choose features phi0 to phi2
/* for( i = 0; i < 3; i++ ) {
if( ( (float)random() / (float)RAND_MAX ) < 0.5 ) {
phi[i] = 1;
} else {
phi[i] = 0;
}
}
*/ if( ((float)random() / (float)RAND_MAX) < 0.7 ) {
phi[0] = 1;
} else {
phi[0] = 0;
}
if( ((float)random() / (float)RAND_MAX) < 0.3 ) {
phi[1] = 1;
} else {
phi[1] = 0;
}
if( ((float)random() / (float)RAND_MAX) < 0.5 ) {
phi[2] = 1;
} else {
phi[2] = 0;
}
if( ( phi[0] == 1 && phi[1] == 0 ) || ( phi[0] == 0 && phi[1] == 1 )) {
rewardx = rewardAx;
rewardy = rewardAy;
} else {
rewardx = rewardBx;
rewardy = rewardBy;
}
for( k = 0; k < 3; k++ ) {
phiVal[rewardAx][rewardAy][k] = 0;
phiVal[rewardBx][rewardBy][k] = 0;
phiVal[rewardx][rewardy][k] = REWARD;
}
val[rewardAx][rewardAy] = 0;
val[rewardBx][rewardBy] = 0;
grid[rewardAx][rewardAy] = FLOOR;
grid[rewardBx][rewardBy] = FLOOR;
grid[rewardx][rewardy] = REWARD;
val[rewardx][rewardy] = REWARD;
//randomly chose starting position
do{
i = random() % MAXX;
j = random() % MAXY;
}while( grid[i][j] == WALL || (i == rewardx && j == rewardy) );
xloc = i;
yloc = j;
step = 0;
do{
step++;
//reset freedom of movement
for( moves = 0; moves < 4; moves++ ) {
canmove[moves] = 0;
}
//determine freedom of movement
if( xloc > 0 ) {
if( grid[xloc-1][yloc] != WALL ) {
canmove[LEFT] = 1;
}
}
if( xloc < (MAXX-1) ) {
if( grid[xloc+1][yloc] != WALL ) {
canmove[RIGHT] = 1;
}
}
if( yloc > 0 ) {
if( grid[xloc][yloc-1] != WALL ) {
canmove[UP] = 1;
}
}
if( yloc < (MAXY-1) ) {
if( grid[xloc][yloc+1] != WALL ) {
canmove[DOWN] = 1;
}
}
oldx = xloc;
oldy = yloc;
do{
ret = doMove(choseMove());
}while( ret == -1 );
reward = getReward();
val[oldx][oldy] += ALPHA*(reward+(GAMMA*val[xloc][yloc])-val[oldx][oldy]);
for( k = 0; k < 3; k ++ ) {
if( phi[k] == 1 ) {
phiVal[oldx][oldy][k] += ALPHA*(reward+(GAMMA*phiVal[xloc][yloc][k])-phiVal[oldx][oldy][k]);
}
}
if( episodeNumber % 1000 == 0 && step == 1 ) {
showMap();
usleep((useconds_t)SLEEPTIME);
}
}while( step < MAXSTEPS && ( xloc != rewardx || yloc != rewardy ));
}
return;
}
int main(int argc, char* argv[]) {
int i, j, k, n;
int wallposition, wallopening;
int counter;
char temp;
srandom(time(NULL));
xloc = 0;
yloc = 0;
episodeNumber = 0;
myScreen = initscr();
refresh();
//create gridworld
for( i = 0; i < MAXX; i++ ) {
for( j = 0; j < MAXY; j++) {
grid[i][j] = FLOOR;
val[i][j] = 0;
for( k = 0; k < 3; k++ ) {
phiVal[i][j][k] = 0;
}
}
}
//randomly place wall with single "slit" opening
if( random() % 2 == 0 ) { //vertical wall
wallposition = random() % (MAXX-2);
wallopening = random() % MAXY;
for( i = 0; i < MAXY; i++ ) {
grid[wallposition+1][i] = WALL;
}
grid[wallposition+1][wallopening] = FLOOR;
}else { //horizontal wall
wallposition = random() % (MAXY-2);
wallopening = random() % MAXX;
for( i = 0; i < MAXX; i++ ) {
grid[i][wallposition+1] = WALL;
}
grid[wallopening][wallposition+1] = FLOOR;
}
//randomly place rewards A and B
do{
i = random() % MAXX;
j = random() % MAXY;
}while( grid[i][j] == WALL );
//grid[i][j] = REWARD;
rewardAx = i;
rewardAy = j;
do{
i = random() % MAXX;
j = random() % MAXY;
}while( grid[i][j] == WALL || ( rewardAx == i && rewardAy == j ) );
rewardBx = i;
rewardBy = j;
//val[i][j] = REWARD;
showMap();
doGridLearning();
//leave window open until keypress
temp = getchar();
endwin();
return 0;
}
/* MapPreviewCanvas::draw
* Draws the map
*******************************************************************/
void MapPreviewCanvas::draw()
{
// Setup colours
wxColour wxc;
wxc.Set(map_view_col_background); rgba_t col_view_background(wxc.Red(), wxc.Green(), wxc.Blue(), 255);
wxc.Set(map_view_col_line_1s); rgba_t col_view_line_1s(wxc.Red(), wxc.Green(), wxc.Blue(), 255);
wxc.Set(map_view_col_line_2s); rgba_t col_view_line_2s(wxc.Red(), wxc.Green(), wxc.Blue(), 255);
wxc.Set(map_view_col_line_special); rgba_t col_view_line_special(wxc.Red(), wxc.Green(), wxc.Blue(), 255);
wxc.Set(map_view_col_line_macro); rgba_t col_view_line_macro(wxc.Red(), wxc.Green(), wxc.Blue(), 255);
// Setup the viewport
glViewport(0, 0, GetSize().x, GetSize().y);
// Setup the screen projection
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(0, GetSize().x, 0, GetSize().y, -1, 1);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// Clear
glClearColor(((double)col_view_background.r)/255.f, ((double)col_view_background.g)/255.f,
((double)col_view_background.b)/255.f, ((double)col_view_background.a)/255.f);
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
// Translate to inside of pixel (otherwise inaccuracies can occur on certain gl implementations)
if (OpenGL::accuracyTweak())
glTranslatef(0.375f, 0.375f, 0);
// Zoom/offset to show full map
showMap();
// Translate to middle of canvas
glTranslated(GetSize().x * 0.5, GetSize().y * 0.5, 0);
// Zoom
glScaled(zoom, zoom, 1);
// Translate to offset
glTranslated(-offset_x, -offset_y, 0);
// Setup drawing
glDisable(GL_TEXTURE_2D);
glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
glLineWidth(1.5f);
glEnable(GL_LINE_SMOOTH);
// Draw lines
for (unsigned a = 0; a < lines.size(); a++)
{
mep_line_t line = lines[a];
// Check ends
if (line.v1 >= verts.size() || line.v2 >= verts.size())
continue;
// Get vertices
mep_vertex_t v1 = verts[lines[a].v1];
mep_vertex_t v2 = verts[lines[a].v2];
// Set colour
if (line.special)
OpenGL::setColour(col_view_line_special);
else if (line.macro)
OpenGL::setColour(col_view_line_macro);
else if (line.twosided)
OpenGL::setColour(col_view_line_2s);
else
OpenGL::setColour(col_view_line_1s);
// Draw line
glBegin(GL_LINES);
glVertex2d(v1.x, v1.y);
glVertex2d(v2.x, v2.y);
glEnd();
}
glLineWidth(1.0f);
glDisable(GL_LINE_SMOOTH);
// Swap buffers (ie show what was drawn)
SwapBuffers();
}
// -----------------------------------------------------------------------------
// Draws the map
// -----------------------------------------------------------------------------
void MapPreviewCanvas::draw()
{
// Setup colours
auto col_view_background = ColourConfiguration::colour("map_view_background");
auto col_view_line_1s = ColourConfiguration::colour("map_view_line_1s");
auto col_view_line_2s = ColourConfiguration::colour("map_view_line_2s");
auto col_view_line_special = ColourConfiguration::colour("map_view_line_special");
auto col_view_line_macro = ColourConfiguration::colour("map_view_line_macro");
auto col_view_thing = ColourConfiguration::colour("map_view_thing");
// Setup the viewport
glViewport(0, 0, GetSize().x, GetSize().y);
// Setup the screen projection
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(0, GetSize().x, 0, GetSize().y, -1, 1);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// Clear
glClearColor(
((double)col_view_background.r) / 255.f,
((double)col_view_background.g) / 255.f,
((double)col_view_background.b) / 255.f,
((double)col_view_background.a) / 255.f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Translate to inside of pixel (otherwise inaccuracies can occur on certain gl implementations)
if (OpenGL::accuracyTweak())
glTranslatef(0.375f, 0.375f, 0);
// Zoom/offset to show full map
showMap();
// Translate to middle of canvas
glTranslated(GetSize().x * 0.5, GetSize().y * 0.5, 0);
// Zoom
glScaled(zoom_, zoom_, 1);
// Translate to offset
glTranslated(-offset_.x, -offset_.y, 0);
// Setup drawing
glDisable(GL_TEXTURE_2D);
glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
glLineWidth(1.5f);
glEnable(GL_LINE_SMOOTH);
// Draw lines
for (auto& line : lines_)
{
// Check ends
if (line.v1 >= verts_.size() || line.v2 >= verts_.size())
continue;
// Get vertices
auto v1 = verts_[line.v1];
auto v2 = verts_[line.v2];
// Set colour
if (line.special)
OpenGL::setColour(col_view_line_special);
else if (line.macro)
OpenGL::setColour(col_view_line_macro);
else if (line.twosided)
OpenGL::setColour(col_view_line_2s);
else
OpenGL::setColour(col_view_line_1s);
// Draw line
glBegin(GL_LINES);
glVertex2d(v1.x, v1.y);
glVertex2d(v2.x, v2.y);
glEnd();
}
// Load thing texture if needed
if (!tex_loaded_)
{
// Load thing texture
SImage image;
auto entry = App::archiveManager().programResourceArchive()->entryAtPath("images/thing/normal_n.png");
if (entry)
{
image.open(entry->data());
tex_thing_ = OpenGL::Texture::createFromImage(image, nullptr, OpenGL::TexFilter::Mipmap);
}
else
tex_thing_ = 0;
tex_loaded_ = true;
}
// Draw things
if (map_view_things)
{
OpenGL::setColour(col_view_thing);
if (tex_thing_)
{
double radius = 20;
glEnable(GL_TEXTURE_2D);
OpenGL::Texture::bind(tex_thing_);
for (auto& thing : things_)
{
glPushMatrix();
glTranslated(thing.x, thing.y, 0);
glBegin(GL_QUADS);
glTexCoord2f(0.0f, 0.0f);
glVertex2d(-radius, -radius);
glTexCoord2f(0.0f, 1.0f);
glVertex2d(-radius, radius);
glTexCoord2f(1.0f, 1.0f);
glVertex2d(radius, radius);
glTexCoord2f(1.0f, 0.0f);
glVertex2d(radius, -radius);
glEnd();
glPopMatrix();
}
}
else
{
glEnable(GL_POINT_SMOOTH);
glPointSize(8.0f);
glBegin(GL_POINTS);
for (auto& thing : things_)
glVertex2d(thing.x, thing.y);
glEnd();
}
}
glLineWidth(1.0f);
glDisable(GL_LINE_SMOOTH);
// Swap buffers (ie show what was drawn)
SwapBuffers();
}
/* MapPreviewCanvas::draw
* Draws the map
*******************************************************************/
void MapPreviewCanvas::draw()
{
// Setup colours
rgba_t col_view_background = ColourConfiguration::getColour("map_view_background");
rgba_t col_view_line_1s = ColourConfiguration::getColour("map_view_line_1s");
rgba_t col_view_line_2s = ColourConfiguration::getColour("map_view_line_2s");
rgba_t col_view_line_special = ColourConfiguration::getColour("map_view_line_special");
rgba_t col_view_line_macro = ColourConfiguration::getColour("map_view_line_macro");
rgba_t col_view_thing = ColourConfiguration::getColour("map_view_thing");
// Setup the viewport
glViewport(0, 0, GetSize().x, GetSize().y);
// Setup the screen projection
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(0, GetSize().x, 0, GetSize().y, -1, 1);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// Clear
glClearColor(((double)col_view_background.r)/255.f, ((double)col_view_background.g)/255.f,
((double)col_view_background.b)/255.f, ((double)col_view_background.a)/255.f);
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
// Translate to inside of pixel (otherwise inaccuracies can occur on certain gl implementations)
if (OpenGL::accuracyTweak())
glTranslatef(0.375f, 0.375f, 0);
// Zoom/offset to show full map
showMap();
// Translate to middle of canvas
glTranslated(GetSize().x * 0.5, GetSize().y * 0.5, 0);
// Zoom
glScaled(zoom, zoom, 1);
// Translate to offset
glTranslated(-offset_x, -offset_y, 0);
// Setup drawing
glDisable(GL_TEXTURE_2D);
glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
glLineWidth(1.5f);
glEnable(GL_LINE_SMOOTH);
// Draw lines
for (unsigned a = 0; a < lines.size(); a++)
{
mep_line_t line = lines[a];
// Check ends
if (line.v1 >= verts.size() || line.v2 >= verts.size())
continue;
// Get vertices
mep_vertex_t v1 = verts[lines[a].v1];
mep_vertex_t v2 = verts[lines[a].v2];
// Set colour
if (line.special)
OpenGL::setColour(col_view_line_special);
else if (line.macro)
OpenGL::setColour(col_view_line_macro);
else if (line.twosided)
OpenGL::setColour(col_view_line_2s);
else
OpenGL::setColour(col_view_line_1s);
// Draw line
glBegin(GL_LINES);
glVertex2d(v1.x, v1.y);
glVertex2d(v2.x, v2.y);
glEnd();
}
// Load thing texture if needed
if (!tex_loaded)
{
// Load thing texture
SImage image;
ArchiveEntry* entry = theArchiveManager->programResourceArchive()->entryAtPath("images/thing/normal_n.png");
if (entry)
{
image.open(entry->getMCData());
tex_thing = new GLTexture(false);
tex_thing->setFilter(GLTexture::MIPMAP);
tex_thing->loadImage(&image);
}
else
tex_thing = NULL;
tex_loaded = true;
}
// Draw things
if (map_view_things)
{
OpenGL::setColour(col_view_thing);
if (tex_thing)
{
double radius = 20;
glEnable(GL_TEXTURE_2D);
tex_thing->bind();
for (unsigned a = 0; a < things.size(); a++)
{
glPushMatrix();
glTranslated(things[a].x, things[a].y, 0);
glBegin(GL_QUADS);
glTexCoord2f(0.0f, 0.0f); glVertex2d(-radius, -radius);
glTexCoord2f(0.0f, 1.0f); glVertex2d(-radius, radius);
glTexCoord2f(1.0f, 1.0f); glVertex2d(radius, radius);
glTexCoord2f(1.0f, 0.0f); glVertex2d(radius, -radius);
glEnd();
glPopMatrix();
}
}
else
{
glEnable(GL_POINT_SMOOTH);
glPointSize(8.0f);
glBegin(GL_POINTS);
for (unsigned a = 0; a < things.size(); a++)
glVertex2d(things[a].x, things[a].y);
glEnd();
}
}
glLineWidth(1.0f);
glDisable(GL_LINE_SMOOTH);
// Swap buffers (ie show what was drawn)
SwapBuffers();
}
