// move:
void move(int order){
if(order == BALL1);
if(order == BALL2){
findLine();
turnLeftLine();
moveLine();
}
if(order == BALL3){
moveBack1();
turnLeft();
}
if(order == BALL4){
moveBack2();
turnBack();
}
if(order == STORAGE1){
turnBack();
moveBack();
turnLeft();
}
if(order == STORAGE2){
findLine();
turnRight();
}
}
int main() // Main function
{
low(26);
low(27);
int irLeft, irRight;
// Add startup code here.
int leftDist, rightDist;
while(1)
{
drive_setRampStep(10);
drive_ramp(64, 64);
while(1)
{
irLeft = checkLeft();
irRight = checkRight();
irRight = ping_cm(8);
print ("left = %d, right = %d \n", irLeft, irRight);
if(irLeft == 1)
{
high(26);
}
else
{
low(26);
}
if(ping_cm(8)<=10)
{
high(27);
}
else
{
low(27);
}
if(irLeft == 1)
{
drive_ramp(0, 0);
turnLeftH();
print (" Left empty hall detected");
break;
}
if (ping_cm(8)<=9)
{
drive_ramp(0, 0);
turnBack();
print (" End of the line");
break;
}
}
}
}
/* 向下走一步 */
void StepDown()
{
int dir = getDirection();
switch (dir)
{
case DIR_RIGHT:
turnRight();
break;
case DIR_LEFT:
turnLeft();
break;
case DIR_DOWN:
break;
case DIR_UP:
turnBack();
break;
default:
return ;
}
AdvStep();
return ;
}
void change_direction_to(char dir)
{
if (bot_dir == N)
{
switch (dir)
{
case N:
break;
case S:
{
turnBack();
bot_dir = S;
break;
}
case E:
{
turnRight();
bot_dir = E;
break;
}
case W:
{
turnLeft();
bot_dir = W;
break;
}
}
}
else if (bot_dir == S)
{
switch (dir)
{
case N:
{
turnBack();
bot_dir = N;
break;
}
case S:
{
break;
}
case E:
{
turnLeft();
bot_dir = E;
break;
}
case W:
{
turnRight();
bot_dir = W;
break;
}
}
}
else if (bot_dir == W)
{
switch (dir)
{
case W:
break;
case E:
{
turnBack();
bot_dir = E;
break;
}
case N:
{
turnRight();
bot_dir = N;
break;
}
case S:
{
turnLeft();
bot_dir = S;
break;
}
}
}
else if (bot_dir == E)
{
switch (dir)
{
case E:
break;
case W:
{
turnBack();
bot_dir = W;
break;
}
case S:
{
turnRight();
bot_dir = S;
break;
}
case N:
{
turnLeft();
bot_dir = N;
break;
}
}
}
}
void dispersion()
{
to_begin=1;
lcd_cursor(1,1);
lcd_string("DISPE");
velocity(VELOCITY_MAX,VELOCITY_MAX);
forward();
while(1)
{
follow();
lcd_print(2,16,bot_dir,1);
if(isPlus())
{
follow();
while(isPlus());
if(value == 0 && end == 0)
initial_direction(bot_id);
if(value == 0 && end == 1)
{
turnRight();
change_dirn(bot_dir);
}
value++;
}
if(isEnd())
{
USART_Transmit('*');
USART_Transmit(bot_id);
switch(bot_id)
{
case 'A': flag[0]++;break;
case 'B': flag[1]++;break;
case 'C': flag[2]++;break;
case 'D': flag[3]++;break;
}
if(end==0)
{
turnBack();
value = 0;
}
if(end==1)
{
turnBack();
value = 0;
}
if(end ==2)
{
length =value-1;
lcd_print(1,7,length,1);
turnBack();
stop();
break;
}
end++;
}
}
}
void proceduralDFS() {
int found = false;
int depth = 0;
int branch = 0;
int pathChosen[50][2];
int nodeFound;
int i;
while (!found) {
//find a node
depth++;
displayTextLine(depth,"Finding for node..");
nodeFound = moveToColor();
//if got blue.. then success
if (nodeFound == blue) {
displayTextLine(depth,"Success",depth);
turnBack();
found = true;
}
//if got red, do backtrack
else if (nodeFound == red) {
displayTextLine(depth,"Failure..");
depth--;
//Cari Branch Selanjutnya
backToPreviousNode();
displayTextLine(depth+1,"");
branch = alignToBranch(pathChosen[depth-1][0]);
//Jika tak ada, backtrack ke node sebelumnya
while (branch == 4) {
depth--;
displayTextLine(depth+1,"");
moveToColor();
moveForward(300);
branch = alignToBranch(pathChosen[depth-1][0]);
}
//Pilih jalan
displayTextLine(depth,"lv%d: Branching to %d",depth,branch);
pathChosen[depth-1][0] = branch;
if (branch <0) {
pathChosen[depth-1][0] *= -1;
pathChosen[depth-1][1] = 1;
}
else {
pathChosen[depth-1][1] = 0;
}
}
//if got green, prepare for deeper depth..
else if (nodeFound == green){
moveForward(300);
branch = alignToBranch(0);
displayTextLine(depth,"lv%d: Branching to %d",depth,branch);
pathChosen[depth-1][0] = branch;
if (branch <0) {
pathChosen[depth-1][0] *= -1;
pathChosen[depth-1][1] = 1;
}
else {
pathChosen[depth-1][1] = 0;
}
}
}
//Tampilkan rute ditemukan
depth--;
for (i=0; i<depth ; i++) {
displayTextLine(i+1,"path chosen -> %d",pathChosen[i][0]);
}
displayTextLine(i+9,"Mission Accomplished..");
//Kembali pulang berdasarkan rute tercatat
for (int i=depth-1 ; i>=0 ; i--) {
moveToColor();
moveForward(300);
switch(pathChosen[i][0]) {
case 3 : turnRight();
moveForward(200);
break;
case 2 : if (pathChosen[i][1] == 1) turnRight();
moveForward(400);
break;
case 1 : turnLeft();
moveForward(200);
break;
}
moveToColor();
}
}
void backToPreviousNode(){
turnBack();
moveToColor();
moveForward(300);
}
