static void _mrc_unused
mhd_put_line_state_fcons(struct mrc_fld *flux, fld1d_state_t F,
int j, int k, int dir, int p, int ib, int ie)
{
#define PUT_LINE(X, Y, Z, I, J, K) do { \
for (int i = ib; i < ie; i++) { \
M3(flux, RR , I,J,K, p) = F1S(F, RR , i); \
M3(flux, RVX + X, I,J,K, p) = F1S(F, RVX, i); \
M3(flux, RVX + Y, I,J,K, p) = F1S(F, RVY, i); \
M3(flux, RVX + Z, I,J,K, p) = F1S(F, RVZ, i); \
M3(flux, EE , I,J,K, p) = F1S(F, EE , i); \
M3(flux, BX + X , I,J,K, p) = F1S(F, BX , i); \
M3(flux, BX + Y , I,J,K, p) = F1S(F, BY , i); \
M3(flux, BX + Z , I,J,K, p) = F1S(F, BZ , i); \
if (s_opt_divb == OPT_DIVB_GLM) { \
M3(flux, PSI, I,J,K, p) = F1S(F, PSI, i); \
} \
} \
} while (0)
if (dir == 0) {
PUT_LINE(0, 1, 2, i, j, k);
} else if (dir == 1) {
PUT_LINE(1, 2, 0, k, i, j);
} else if (dir == 2) {
PUT_LINE(2, 0, 1, j, k, i);
} else {
assert(0);
}
#undef PUT_LINE
}
static void _mrc_unused
mhd_line_put_state_fcons(fld1d_state_t l_U, fld3d_t p_U,
int j, int k, int dir, int ib, int ie)
{
#define PUT_LINE(X, Y, Z, I, J, K) do { \
for (int i = ib; i < ie; i++) { \
F3S(p_U, RR , I,J,K) = F1S(l_U, RR , i); \
F3S(p_U, RVX + X, I,J,K) = F1S(l_U, RVX, i); \
F3S(p_U, RVX + Y, I,J,K) = F1S(l_U, RVY, i); \
F3S(p_U, RVX + Z, I,J,K) = F1S(l_U, RVZ, i); \
F3S(p_U, EE , I,J,K) = F1S(l_U, EE , i); \
F3S(p_U, BX + X , I,J,K) = F1S(l_U, BX , i); \
F3S(p_U, BX + Y , I,J,K) = F1S(l_U, BY , i); \
F3S(p_U, BX + Z , I,J,K) = F1S(l_U, BZ , i); \
if (s_opt_divb == OPT_DIVB_GLM) { \
F3S(p_U, PSI, I,J,K) = F1S(l_U, PSI, i); \
} \
} \
} while (0)
if (dir == 0) {
PUT_LINE(0, 1, 2, i, j, k);
} else if (dir == 1) {
PUT_LINE(1, 2, 0, k, i, j);
} else if (dir == 2) {
PUT_LINE(2, 0, 1, j, k, i);
} else {
assert(0);
}
#undef PUT_LINE
}
void put_disp(void)
{
$DESCRIPTOR(to_disp, outbuf);
if (smg$enable)
{
to_disp.dsc$w_length = strlen(outbuf);
SMG$PUT_LINE(&disp1_id, &to_disp,0,0,0,&SMG$M_WRAP_CHAR,0);
}
else printf("%s\n",outbuf);
}
static void _mrc_unused
mhd_line_put_state_scons(fld1d_state_t l_U, fld3d_t p_U,
int j, int k, int dir, int ib, int ie)
{
#define PUT_LINE(X,Y,Z, I,J,K) do { \
for (int i = ib; i < ie; i++) { \
F3S(p_U, RR , I,J,K) = F1S(l_U, RR , i); \
F3S(p_U, RVX+X, I,J,K) = F1S(l_U, RVX, i); \
F3S(p_U, RVX+Y, I,J,K) = F1S(l_U, RVY, i); \
F3S(p_U, RVX+Z, I,J,K) = F1S(l_U, RVZ, i); \
F3S(p_U, UU , I,J,K) = F1S(l_U, UU , i); \
} \
} while (0)
if (dir == 0) {
PUT_LINE(0,1,2, i,j,k);
} else if (dir == 1) {
PUT_LINE(1,2,0, k,i,j);
} else if (dir == 2) {
PUT_LINE(2,0,1, j,k,i);
}
#undef PUT_LINE
}
static void _mrc_unused
mhd_put_line_state_scons(struct mrc_fld *flux, fld1d_state_t F,
int j, int k, int dir, int p, int ib, int ie)
{
#define PUT_LINE(X,Y,Z, I,J,K) do { \
for (int i = ib; i < ie; i++) { \
M3(flux, RR , I,J,K, p) = F1S(F, RR , i); \
M3(flux, RVX+X, I,J,K, p) = F1S(F, RVX, i); \
M3(flux, RVX+Y, I,J,K, p) = F1S(F, RVY, i); \
M3(flux, RVX+Z, I,J,K, p) = F1S(F, RVZ, i); \
M3(flux, UU , I,J,K, p) = F1S(F, UU , i); \
} \
} while (0)
if (dir == 0) {
PUT_LINE(0,1,2, i,j,k);
} else if (dir == 1) {
PUT_LINE(1,2,0, k,i,j);
} else if (dir == 2) {
PUT_LINE(2,0,1, j,k,i);
}
#undef PUT_LINE
}
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
