I'm new to using nXc to code my NXT 2.0 robot and I need it to follow other robots and check for obstacles at the same time. However, the two tasks sometimes conflict with each other and when I meet an obstacle it treats it as an object and continues moving forward instead of stopping.
Aim of my project: Follow a leader robot, stop when it meets an obstacle and after the obstacle is cleared, continue to follow the leader robot that is in front of it.
Below is my code:
#define NEAR 50
#define TURNTIME 540
#define MOVETIME 1000
mutex moveMutex;
//check for robot in front of it
task check_robot(void)
{
while(true)
{
if (SensorUS(IN_2)<=NEAR)
{
Acquire(moveMutex);
OnFwd(OUT_BC,50); //if it is within range of 50cm, move
Release(moveMutex);
}
if (SensorUS(IN_2)>NEAR)
{
Acquire(moveMutex); //this section allows the robot to turn
OnFwd(OUT_C,50); //left and right to look for the
Wait(TURNTIME); //leader
OnRev(OUT_B,50);
Wait(TURNTIME);
OnRev(OUT_C,50);
Wait(TURNTIME);
OnFwd(OUT_B,50);
Wait(TURNTIME);
if (SensorUS(IN_2)<=NEAR)
{
Acquire(moveMutex);
OnFwd(OUT_BC,50); //if it finds the leader, it moves
Release(moveMutex);
}
else
{
Acquire(moveMutex);
OnFwd(OUT_C,50); //repeats the search process
Wait(TURNTIME);
OnRev(OUT_B,50);
Wait(TURNTIME);
OnRev(OUT_C,50);
Wait(TURNTIME);
OnFwd(OUT_B,50);
Wait(TURNTIME);
Release(moveMutex);
}
}
Release(moveMutex);
}
}
task check_obstacles(void)
{
Acquire(moveMutex);
until(((SENSOR_1)==1)||((SENSOR_3)==1)); //when any one of the
OnRev(OUT_BC,50); //touch sensors are
Wait(1000); //pressed, it is supposed
Off(OUT_BC); //to stop until both
until(((SENSOR_1)==0)&&((SENSOR_3)==0)); //are released, then
OnFwd(OUT_BC,50); //continue moving
Release(moveMutex);
}
task main(void)
{
SetSensorTouch(IN_1);
SetSensorUltrasonic(IN_2);
SetSensorTouch(IN_3);
SetSensorColorFull(IN_4);
while(true)
{
Precedes(check_robot,check_obstacles);
}
}
Any thoughts on how I can improve this? Would it be better to use subroutines instead of tasks and what is the benefit of using subroutines or tasks?