Path::Path(Point s, Point e):start(s),end(e),edge(start, end){
prev = next = NULL;
vec = end - start;
vec = vec.normalize();
to_next = get_orientation(vec);
to_prev = get_orientation(start-end);
set_angel();
}
void main (void)
{
int reflection,LDs_count,omron_count,angel;
int old_fix,fix,left,right;
unsigned char reflect,status,k;
char old_flag,flag;
WDTCN = 0xDE; // disable watchdog timer
WDTCN = 0xAD;
SYSCLK_Init (); // Initialize system clock to 16MHz
ExtCrystalOsc_Init (); // switch system clock
Port_Init (); // Initialize crossbar and GPIO
SPI0_Init();
LED_Init(); // Initialize OLED
Sensors_Init();
Rudder_Init();
Motor_Init();
Measurement_Init();
Test_Helper_Init();
EA = 1; // Enable global interrupts
CR = 1; // start to output PWM
P31 = 1;
PUT_LINE("OMRON:",omron_count);
PUT_LINE("reflection:",reflection);
PUT_LINE("LDs_count:",LDs_count);
PUT_LINE("Tunnel1:",tunnel_length[0]);
PUT_LINE("Tunnel2:",0);
PUT_LINE("Tunnel3:",0);
//PUT_LINE("angel:",angel);
//PUT_LINE("fix:",fix);
while (1) { // Spin forever
DIRECTION = 0;
PCA0CPH1 = 0xE0;
flag = 0;
while (1) {
// dectecting
reflect = get_LD_reflection();
old_flag = flag;
flag = 0;
// conner emergency
if(reflect < 25){
if(!LEFT3 && RIGHT3)
flag = 1;
if(!RIGHT3 && LEFT3)
flag = -1;
if(flag)
break;
}
// go straight forward:
if(!LEFT1 || !LEFT2 || !LEFT3)
flag = 1;
if(!RIGHT1 && LEFT2)
flag = -1;
if(!RIGHT2 && LEFT3)
flag = -1;
if(!RIGHT3)
flag = -1;
k = 7;
if(reflect > 50){
k = 10;
}
if(reflect > 85){
if(flag > 0)
if(++left > 32766)
{
left = left - right;
right = 0;
}
if(flag < 0)
if(++right > 32766)
{
right = right - left;
left = 0;
}
old_fix = fix;
fix = left - right;
if(fix > 20)
fix = 20;
if(fix < -20)
fix = -20;
fix = ((fix + RUDDER_FIX) + old_fix)/2;
}
//UPDATE_VALUE(5,fix);
if(MAX_REFLECTION > reflect)
angel = MAX_REFLECTION - reflect;
else
angel = 0;
angel = flag * k * angel / 10 + fix;
//UPDATE_VALUE(4,angel);
set_angel(angel);
// debug info
omron_count = Get_OMRON_Count();
reflection = get_LD_reflection();
LDs_count = TL1 + LD2_count;
UPDATE_VALUE(0,omron_count);
UPDATE_VALUE(1,reflection);
UPDATE_VALUE(2,LDs_count/3);
tmp = tunnel_length[1] * 464 / 1683;
UPDATE_VALUE(3,tmp);
tmp = tunnel_length[2] * 464 / 1683;
UPDATE_VALUE(4,tmp);
tmp = tunnel_length[3] * 464 / 1683;
UPDATE_VALUE(5,tmp);
}
reflect = get_LD_reflection();
if(reflect < 10){ // 60 degree
PCA0CPH1 = 0xB8;
set_angel(flag * 127);
Clear_OMRON_Count();
while(omron_count <= 300);
while(1){
if(flag > 0 && !LEFT1) {
while(!RIGHT1);
break;
}
if(flag < 0 && !RIGHT1) {
while(!LEFT1);
break;
}
}
}else{
// 90 degree
DIRECTION = 1;
PCA0CPH1 = 0x38;
Clear_OMRON_Count();
set_angel(-flag * 127);
while(1){
omron_count = Get_OMRON_Count();
UPDATE_VALUE(0,omron_count);
if(omron_count >= 410) //1062
break;
}
DIRECTION = 0;
PCA0CPH1 = 0xB8;
//Clear_OMRON_Count();
set_angel(flag * 127);
// escape from conner
while(1){
if(flag > 0 && !LEFT1){
while(!RIGHT1);
break;
}
if(flag < 0 && !RIGHT1){
while(!LEFT1);
break;
} // if
} // while
} // if else: 60 or 90 degree
} // main loop
} // main
