Hi,
I am new here so I apologize in advance if I am posting in the wrong section.
I am working on a code for a tankbot with a light sensor and a touch sensor. It has to follow a black line, and make turns at t intersections. It will bump into a small object, back up and turn. It has to stop at a green pad at the end of a track. This is my code so far:
#define THRESHOLD 39
int x=0;
task main()
{
SetUserDisplay(x,0);
SetSensor(SENSOR_1,SENSOR_TOUCH);
SetSensor(SENSOR_2,SENSOR_LIGHT);
start turn;
OnFwd(OUT_A+OUT_C);
while(true)
{
if(SENSOR_2 > THRESHOLD)
{OnRev(OUT_C);
OnFwd(OUT_A);
until(SENSOR_2 <=THRESHOLD);}
}
}
task turn()
{
if(x == 0 && SENSOR_1 == 1) //first turn
{
until(SENSOR_1 == 0);
x=x+1;
OnRev(OUT_A+OUT_C);
Wait(20);
OnFwd(OUT_A);
OnRev(OUT_C);
until(SENSOR_2 <= THRESHOLD);}
if(x == 1 && SENSOR_1 == 1) //second turn
{
until(SENSOR_1 == 0);
x=x+1;
OnRev(OUT_A+OUT_C);
Wait(20);
OnFwd(OUT_C);
OnRev(OUT_A);
until(SENSOR_2 <= THRESHOLD);
}
if(x == 2 && SENSOR_1 == 1) //third turn
{until(SENSOR_1 == 0);
x=x+1;
OnRev(OUT_A+OUT_C);
Wait(20);
OnFwd(OUT_A);
OnRev(OUT_C);
until(SENSOR_2 <= THRESHOLD);
}
}
-------------------
Can somebody help me? It is not working correctly and I have no idea how to improve it.
Here is the link to the track
http://i.imgur.com/XPIRBD3.jpg