void main() {
trisb=255;
portb=0;
trisa=0;
porta=0;
trisc=0;
portc=0;
while(1)
{
switch (portb)
{
case 254: {porta.f0=1; center(); // just forward
break;}
case 253: {porta.f1=1; center(); //just back
break;}
case 250: {porta.f0=1; m_right(); // forward and right
break;}
case 246: {porta.f0=1; m_left(); // forward and left
break;}
case 249: {porta.f1=1; m_right(); // back and right
break;}
case 245:{porta.f1=1; m_left(); // back and left
break;}
case 251:{m_right(); porta.f0=0; porta.f1=0; // just move the wheels right
break;}
case 247:{m_left();porta.f0=0; porta.f1=0;
break;}
default: {center(); porta.f0=0 ; porta.f1=0; // default any other state.
break;}
}
}
}
//Heapify the Min- Heap
void min_heapify(Heap H,int n,int s,int i)
{
int l;
int r;
int min;
while(i>=s)
{
l = m_left(i);
r = m_right(i);
if(l < s)
break;
if( l == s)
min = l;
else if ( H.a[l] < H.a[r] )
min = l;
else
min = r;
if(H.a[min] >= H.a[i])
break;
swap(H.a[min],H.a[i]);
i = min;
}
}
Value operator()( const State &x , const Derivs &derivs , Which )
{
return m_left( x , derivs , Which() ) + m_right( x , derivs , Which() );
}