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]);
}