void red2()
{
while(true)
{
cleargyro();
if(SensorValue(sonar) <50) // Loop while robot's Ultrasonic sensor is further than 20 inches away from an object
{
while(SensorValue[gyro] < 500){
turn_left();
}
go_straight();
}
else
{
motor[right] = 127;
motor[left] = 127;
}
}
}
int main()
{
go_straight();
turn_right();
go_upstair();
turn_left();
enter_restroom();
return 0;
}
void move_to(double curr_coord[2], double x, double y){
double dx = x - curr_coord[0];
double dy = y - curr_coord[1];
double targetA = atan2(dx, dy);
double dangle = targetA - face_angle;
dangle += (dangle > to_rad(180)) ? -to_rad(360) : (dangle < -to_rad(180)) ? to_rad(360) : 0;
spin(curr_coord, dangle);
go_straight(curr_coord, fabs(sqrt(dx*dx + dy*dy)));
printf("Moving Done : X = %f, Y = %f, face_angle = %f \n", curr_coord[0], curr_coord[1], to_degree(face_angle));
}
void MyRobot::rescue_person(double angle)
{
const double *compass_val = _my_compass->getValues();
_compass_angle = convert_bearing_to_degrees(compass_val);
if((_compass_angle >= angle-1.25 && _compass_angle < angle + 1.25)|| metres >0 ){
if ((inside ==false)&&(turn == true ||_dist_val[0] > DISTANCE/2 || _dist_val[1] > DISTANCE/2 || _dist_val[14] > DISTANCE/2 || _dist_val[15] > DISTANCE/2)){
turn = true;
turn_around_complete();
if ((_compass_angle >= angle-5.25 && _compass_angle < angle)&&(end > 20))
{
num_person = num_person + 1;
turn = false;
back =true;
//To exit of this if.
inside = true;
//Until end is 0 the robot is turned around itself.
end=0;
}
}
else
{
if(back== false){
go_straight(angle);
}else{
/*if angle is negative and you try to subtract 180 the result of this is an angle that doesnt exist.*/
if(angle >=0.0){
go_back_straight(angle-180);
}else{
go_back_straight(angle+180);
}
}
}
}
else if(_compass_angle< angle-20 || _compass_angle> angle +10){
_mode = FAST_TURN_AROUND;
} else{
_mode = TURN_AROUND;
}
}
task main()
{
pid_controller_t left_pid_controller;
init_drive_pid_controller(&left_pid_controller);
pid_controller_t right_pid_controller;
init_drive_pid_controller(&right_pid_controller);
reset_readings();
while (1)
{
check_and_switch_mode();
switch (robot_mode)
{
case Do_Nothing:
stop_moving();
motor[sonar_rotate] = 0;
StopTask(sonar_scanner);
break;
case Turn:
turn_left();
break;
case Follow_Closest_Object:
// keep moving the sonar
StartTask(sonar_scanner);
// turn the robot towards the closest object
int closest_bin = find_closest_sonar_bin();
writeDebugStreamLine("closest_bin: %d", closest_bin);
int degrees_to_turn = closest_bin * (360/SONAR_BINS);
//writeDebugStreamLine("degrees_to_turn: %d", degrees_to_turn);
while (degrees_to_turn > 180)
{
degrees_to_turn -= 360;
}
while (degrees_to_turn < -180)
{
degrees_to_turn += 360;
}
//writeDebugStreamLine("degrees_to_turn: %d", degrees_to_turn);
if (degrees_to_turn < 360/SONAR_BINS)
{
move_motor_to_position(right,
nMotorEncoder[right] + RIGHT_MOTOR_ENCODING_TICKS_PER_DEGREE * degrees_to_turn,
&right_pid_controller);
//turn_left();
}
else if (degrees_to_turn > 360/SONAR_BINS)
{
move_motor_to_position(left,
nMotorEncoder[left] + LEFT_MOTOR_ENCODING_TICKS_PER_DEGREE * degrees_to_turn,
&left_pid_controller);
//turn_right();
}
else
{
go_straight();
}
wait1Msec(1);
break;
default:
return;
}
}
}
