Sokoban

Zadanie nášho projektu

Cieľom nášho projektu Sokoban bolo naprogramovať robota Tatrabot tak, aby zvládol nájsť správne riešenie zadanej hracej plochy viď obr. 1 hry Sokoban k dispozícii sme mali 4 hracie plochy ktoré sú na obr. 1 a následne týmto nájdeným riešením odohrať hru do úspešného konca, pričom hracia plocha pozostávala z navzájom kolmých čiar vytvárajúcich mriežku rôznych tvarov obsahujúcu priesečníky reprezentujúce kocky potrebné k odohratiu hry, teda ich dopraveniu na vopred určené miesta zadanej hracej plochy mriežky.

obr. 1

Náš projekt mal 2 fázy. Zatiaľ máme dokončenú prvú fázu kde sme sa zoznámili s robotom TATRABOT vyskúšali sme jeho funkčnosť a naučili sme ho jazdiť po vygenerovanej trase. Druhú časť ktorá generuje trasu sa nám ešte nepodarilo spraviť.

Hardware

Pre náš projekt sme použili robota TATRABOT.

200px

Software

Tento program obsahuje pohybovú riadiacu časť teda prvú časť nášho projektu.

main()
{
#include "tatrabot.h"
#include "demo.h"

//char smer[256] = "DPDPDVPPPDDVDDPDPDPDVDLDLDDLPLDLPLDLDVPDVLDDVPPPDDVDLDLDVDPLDPDLDVLLLDDDVPPPD";
char smer[255] = "DPDPDVPPP";               //predom zadaná trasa ktorú má robot prejsť v dokončenej verzii bude trasu generovať tiež program
 
void rt(void) {       //zabočiť do prava
  //rightTurn
  setMotor(2, -4000);
  setMotor(1, 4000);
}
 
void lt(void) {       //zabočiť do ľava
  //leftTurn
  setMotor(1, -4000);
  setMotor(2, 4000);
}  
void bd(void) {       //cúvanie
  //backward
  setMotor(3, -4000);
}
 
void fd(int distance) {
  unsigned int endA = countA + distance;
  unsigned int endB = countB + distance;
  while (countA <= endA && countB <= endB) {
    uint8_t ln = senseLine();
    uint8_t led1 = (ln >> 3) & 1;
    uint8_t led2 = (ln >> 2) & 1;
    uint8_t ledx = ln & 1;
 
    if (led2 == 1 || ledx == 1) {
      led2 = 1;
    }
    else {
     led2 = 0;
    }
    uint8_t led3 = (ln >> 1) & 1;
    if (led1 == 1 && led2 == 1 && led3 == 0) {
      lt();
    }
    if (led1 == 0 && led2 == 1 && led3 == 1) {
      rt();
    }
    setMotor(3, 4000);
  }
  motorsOff();
}
void back(int distance) {
  unsigned int endA = countA + distance;
  unsigned int endB = countB + distance;
  while (countA <= endA && countB <= endB) {
    //chprintf(BT, "cA = %5d, eA = %5d, CB = %5d, eB = %5d\n\r", countA, endA, countB, endB);
    uint8_t ln = senseLine();
    uint8_t led1 = (ln >> 3) & 1;
    uint8_t led2 = (ln >> 2) & 1;
    uint8_t ledx = ln & 1;
 
    if (led2 == 1 || ledx == 1) {
      led2 = 1;
    }
    else {
      led2 = 0;
    }
    uint8_t led3 = (ln >> 1) & 1;
    if (led1 == 1 && led2 == 1 && led3 == 0) {
      //lt
      rt();
    }
    if (led1 == 0 && led2 == 1 && led3 == 1) {
      //rt
      lt();
    }
    //chprintf(BT, "dopredu...\r\n");
    bd();
  }
  motorsOff();
}
void turn(const char s) {
  int d = 11, dis = 3;
  if (s == 'v') {
    dis = dis + 5;
  }
  if (s == 'v') {
    back(d);
  }
  else {
   fd(d);
  }
  unsigned int endA = countA + dis;
  unsigned int endB = countB + dis;
  while (countA <= endA && countB <= endB) {
    if (s == 'l' || s == 'v') {
      lt();
    }
    if (s == 'r') {
      rt();
    }
  }
 
  while (true) {
    uint8_t ln = senseLine();
    uint8_t led1 = (ln >> 3) & 1;
    uint8_t led2 = (ln >> 2) & 1;
    uint8_t ledx = ln & 1;
    if (led2 == 1 || ledx == 1) {
      led2 = 1;
    }
    else {
      led2 = 0;
    }
    uint8_t led3 = (ln >> 1) & 1;
    chprintf(BT, "L1 = %5d, L2 = %5d, L3 = %5d\n\r", led1, led2, led3);
    if (led1 == 0 && led2 == 1 && led3 == 0) {
      break;
    }
    if (s == 'l' || s == 'v') {
      lt();
    }
    if (s == 'r') {
      rt();
    }
  }
  if (s == 'v') {
    back(d);
  }
  else {
    fd(d);
  }
}
void chod(void) {
  int i = 0;
  while (smer[i] != '\0') {
    if (smer[i] == 'D') {
      fd(22);
    }
    if (smer[i] == 'P') {
      turn('r');
    }
    if (smer[i] == 'L') {
      turn('l');
    }
    chprintf(BT, "==============i++=============");
    i++;
  }
}
void chodd(void) {
  while (true) {
    chprintf(BT, "zadajte pismeno: \r\n");
    uint8_t choice = chSequentialStreamGet(BT);
    switch (choice) {
    case 'd':
      fd(22);
      break;
    case 'p':
      turn('r');
      break;
    case 'l':
      turn('l');
      break;
    case 'v':
      turn('v');
      break;
    }
  }
}
int main(void) {
  tatrabotInit();
  //demo();
  chod();
  /*
   while(true){
   chprintf(BT, "cA = %5d,  eB = %5d\n\r", countA, countB);
   }
   */
  chprintf(BT, "end");
  motorsOff();
}
 } 

Záver

Robot fungoval primeranek podmienkam. Pri rôznych svetelných podmienkach na dráhe

nastalo že robot nie vždy vedel sledovať čiaru. Ale po nekalibrovaní senzorov funguje správne.