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; } } }