task main()
{
int i;
servo[leftEye] = LSERVO_CENTER;
servo[rightEye] = RSERVO_CENTER;
init_path();
add_segment(24, 0, 50); // Move to beacon sensing position.
stop_path();
dead_reckon();
if (SensorValue[carrot] < 60) {
position = 3;
} else if (SensorValue[carrot] < 80) { // If ultrasonic sensor sees a position,
position = 1; // set variable "position" to position number.
} else {
position = 2;
}
move_to_pole(position); // Move to pole using position number.
for (i = 0; i < position; i++) { // Beep for the position number that the
playImmediateTone(251, 50); // ultrasonic sensor sees.
wait1Msec(1000);
}
}
void move_to_pole(int count)
{
init_path();
switch (count) {
case 1:
add_segment(22, 180, 30);
add_segment(0, 90, 30);
add_segment(20, 0, 100);
break;
case 2:
add_segment(-7, 0, 50);
add_segment(0, -160, 30);
add_segment(50, 0, 100);
break;
case 3:
add_segment(0, -90, 30);
add_segment(15, 0, 60);
add_segment(0, -90, 30);
add_segment(45, 0, 80);
break;
}
stop_path();
dead_reckon();
}
int ultrasound(tSensors us_sensor, int distance, int ultrasound_dist1, int ultrasound_dist3)
#endif
{
int s_val;
init_path();
add_segment(distance, 0, 45);
stop_path();
dead_reckon();
wait1Msec(1000);
#ifdef __HTSMUX_SUPPORT__
s_val = USreadDist(us_sensor);
#else
s_val = SensorValue[us_sensor];
#endif
if (s_val < ultrasound_dist3) {
return 3;
} else if (s_val < ultrasound_dist1) {
return 1;
} else {
return 2;
}
}
task main()
{
initializeRobot();
RNRR_waitForStart();
init_path();
add_segment(28, 0, 100);
add_segment(27, 45, 100);
stop_path();
dead_reckon();
dumpBlock();
while (true) { }
}
void move_to_pole(int count)
{
init_path();
switch (count) {
case 1:
add_segment(26, 15, 45);
add_segment(-7.5, 105, 80);
break;
case 2:
add_segment(22, 0, 45);
add_segment(-7.5, -135, 50);
break;
case 3:
add_segment(27, 27.5, 45);
add_segment(-13.5, -202, 50);
break;
}
stop_path();
dead_reckon();
}
void move_to_position(int position)
{
init_path();
switch (position) { // add_segment(distance, angle, power);
case 1:
add_segment(18, 0, 50); // Move robot to position one.
break;
case 2:
add_segment(30, -45, 50); // Move robot to position two.
add_segment(0, 90, 25);
break;
case 3:
add_segment(15, -90, 50); // Move robot to position three.
add_segment(0, 135, 25);
break;
stop_path();
dead_reckon();
}
}
