code
Code: Select all
sub wander()
{
   int WANDER = (Random(2000)-500);
    int WANDERR= (Random(2000)- 500);
    int count =(Random(500));
    while(count >0){
    OnFwdReg(OUT_C, WANDER/10, OUT_REGMODE_SPEED);
    OnFwdReg(OUT_A, WANDERR/10, OUT_REGMODE_SPEED);
    count--;}
}
void travel();
void updateX(int x, int y, int h);
void updateY();
task main()
{
    while(1)
    {
        wander();
        travel();
    }
}
void travel()
{
    float heading[3] = {0,0,0};
    float position[3] = {0,0,0};
    float buffer[3] = {0,0,0};
   
   float left,right, dr, dl, dt,h,i,j,x,y;
   
   
   left += MotorBlockTachoCount(OUT_C);
   right += GetOutput(OUT_A, BlockTachoCount);
   
   i = right * 0.01745;
   j = left * 0.01745;
   
   dr = i * 18;
   dl = j * 18;
   
   h = position[2] + ((dr - dl)/107);
   position[2] = h;
   dt = (dr + dl)/2;
   
   x = cos(60);
   
   
   NumOut(10,LCD_LINE1, i);
   NumOut(10,LCD_LINE2, j);
   NumOut(10,LCD_LINE3, left);
   NumOut(10,LCD_LINE4, right);
   NumOut(10,LCD_LINE5, h);
   NumOut(10,LCD_LINE6,position[2]);
   
}