me and a friend programmed this nxt 2.0 robot (with nxc) but found some problems.
The idea is to make a robot (from the standard pack) that will search for objects and once if finds one, go there, pick it up, and bring it back to its original spot, then drop it there.
I will attach the program below but first I''ll explain some things.
Motor B and C are the wheels (B is left, C is right).
Sensor 1 is the ultrasonic sensor
Sensor 2 is a touch sensor installed in the grabber to detect if it has already picked up the object or not.
The robot has 3 wheels, 2 front wheels (B and C) and one rearwheel which can turn 360 degrees losely.
This is to make it able to turn on the spot.
The problem is that it does not pick up the objects on the moment it is supposed to, but it starts grabbing later on.
Please have a look at our program and try to find anything that's wrong with it.
Code: Select all
#define one_degree_turn 50
#define distance_step 100
#define grabber_step 100
#define speed 20
int turn_way;
// turn_way == 1 = right && turn_way == 0 = left
int dist_start;
int turn_start;
int temp_turn;
int pre_dist_start;
int grabber;
int dist;
int turn;
int a;
int b;
int c;
task calc_dist_start()
{
a = dist_start;
b = dist;
pre_dist_start=dist_start;
dist_start = floor(sqrt(pow(a,2.0)+pow(b,2.0)-2*a*b*cosd(180-turn-turn_start)));
}
task calc_turn_start()
{
a = dist_start;
b = dist;
c = pre_dist_start;
temp_turn = acosd((pow(a,2.0)+pow(b,2.0)-pow(c,2.0))/(2*a*b));
if (turn>180)
{
turn_start = turn_start-temp_turn;
}
else
{
turn_start = turn_start+temp_turn;
}
turn = 0;
dist = 0;
}
task drop_load()
{
while (grabber != 0)
{
OnRev(OUT_A,25);
Wait(grabber_step);
grabber--;
}
}
task main()
{
dist_start = 2;
SetSensorLowspeed(IN_1);
SetSensor(IN_2,SENSOR_TOUCH);
SetSensorMode(IN_2,SENSOR_MODE_RAW);
SetSensor(IN_3,SENSOR_TOUCH);
SetSensorMode(IN_3,SENSOR_MODE_RAW);
while (true)
{
if (SensorUS(IN_1)<20) // found!
{
while (SensorUS(IN_1)>6) // go there
{
dist++;
OnFwd(OUT_BC,speed);
Wait(distance_step);
}
OnFwd(OUT_BC,0);
while (grabber != 6 && SENSOR_2 <10) // here it is supposed to start picking up
{
OnFwd(OUT_A,25);
Wait(grabber_step);
grabber++;
}
start calc_dist_start;
Wait(100);
start calc_turn_start;
Wait(100);
while (turn_start>360)
{
turn_start = turn_start-360;
}
if (turn_start>180) // right turn
{
turn_start = turn_start-180;
while (turn_start != 0)
{
OnFwd(OUT_B,speed);
OnRev(OUT_C,speed);
Wait(one_degree_turn);
turn_start--;
}
}
else if (turn_start<0) // right turn
{
turn_start = turn_start*-1;
while (turn_start != 0)
{
OnFwd(OUT_B,speed);
OnRev(OUT_C,speed);
Wait(one_degree_turn);
turn_start--;
}
}
else // left turn
{
while (turn_start != 0)
{
OnFwd(OUT_C,speed);
OnRev(OUT_B,speed);
Wait(one_degree_turn);
turn_start--;
}
}
while (dist_start != 2)
{
OnFwd(OUT_BC,speed);
dist_start--;
Wait(distance_step);
}
start drop_load;
Wait(1000);
}
else
{
while (SensorUS(IN_1)>100 || SensorUS(IN_1)<6)
{
OnFwd(OUT_C,speed);
OnRev(OUT_B,speed);
Wait(one_degree_turn);
turn++;
if(turn = 360)
{
turn = 0;
Wait(90*one_degree_turn);
turn = turn + 90;
OnFwd(OUT_BC,speed);
Wait(50*distance_step);
start calc_dist_start;
start calc_turn_start;
}
}
Off(OUT_BC);
}
}
// end main
}
. I'm 'the friend' btw. 