void MyRobot::scaner_turn_around()
{
// Get and enable the camera device
_forward_camera = getCamera("camera_f");
_forward_camera->enable(_time_step);
// Get current image from forward camera
const unsigned char *image = _forward_camera->getImage();
const double *compass_val = _my_compass->getValues();
_compass_angle = convert_bearing_to_degrees(compass_val);
//If the robot detect something green.
if (scaner(image) > 20)
{
/*Finded angle person, if it is the first person we will safe this angle in _compass_angle_green[1]
if its the second it will be saved in _compass_angle_green[0]*/
_compass_angle_green[0] = _compass_angle;
}
else
{
_mode = FAST_TURN_AROUND;
if (_compass_angle_green[0] != 1000.0)
{
if (_compass_angle_green[1] == 1000.0)
{
_compass_angle_green[1] = _compass_angle_green[0]-2;
_compass_angle_green[0] = 1000.0;
}
}
}
if(_compass_angle_green[0]!=1000.0 && _compass_angle_green[1]!=1000.0){
_mode = STOP;
}
}
void MyRobot::run()
{
double compass_angle;
while (step(_time_step) != -1) {
// Read distance sensors and show values
cout << "Sensor 0: " << _dsensors[0]->getValue() << endl;
cout << "Sensor 1: " << _dsensors[1]->getValue() << endl;
cout << "Sensor 2: " << _dsensors[2]->getValue() << endl;
cout << "Sensor 3: " << _dsensors[3]->getValue() << endl;
cout << "Sensor 12: " << _dsensors[4]->getValue() << endl;
cout << "Sensor 13: " << _dsensors[5]->getValue() << endl;
cout << "Sensor 14: " << _dsensors[6]->getValue() << endl;
cout << "Sensor 15: " << _dsensors[7]->getValue() << endl;
// Read the compass
const double *compass_val = _my_compass->getValues();
// Convert compass bearing vector to angle, in degrees
compass_angle = convert_bearing_to_degrees(compass_val);
// Print compass values to console
cout << "Compass angle (degrees): " << compass_angle << endl;
compass_move(compass_angle);
}
}
void MyRobot::run()
{
// Variable where the robot store compass angle
double compass_angle = 0.0;
// Variables to store the distance measured by de distance sensors
double ds_values[16];
while (step(_time_step) != -1)
{
// Read the value of all distance sensors and the compass
read_distance_sensors(ds_values);
const double *compass_val = _objetive_compass->getValues();
// Convert compass bearing vector to angle, in degrees
compass_angle = convert_bearing_to_degrees(compass_val);
// Initial mode, the robot just go forward following campass direction
if (_robot_state == FOLLOWING_OBJETIVE)
following_objetive_state(ds_values, compass_angle);
if (_robot_state == FOLLOWING_RIGHT_WALL)
following_right_wall_state(ds_values);
if (_robot_state == FOLLOWING_LEFT_WALL)
following_left_wall_state(ds_values);
// Call to the function that set robot move
do_move();
}
}
void MyRobot::run()
{
double compass_angle;
double timer=0;
double ir0_val = 0.0, ir1_val = 0.0, ir2_val = 0.0, ir14_val = 0.0,ir15_val = 0.0,ir13_val = 0.0;
while (step(_time_step) != -1) {
// Read the sensors
const double *compass_val = _my_compass->getValues();
timer++;
ir0_val = _distance_sensor[0]->getValue();
ir1_val = _distance_sensor[1]->getValue();
ir2_val = _distance_sensor[2]->getValue();
ir15_val = _distance_sensor[5]->getValue();
ir14_val = _distance_sensor[4]->getValue();
ir13_val = _distance_sensor[3]->getValue();
// Convert compass bearing vector to angle, in degrees
compass_angle = convert_bearing_to_degrees(compass_val);
// Print sensor values to console
cout << "Compass angle (degrees): " << compass_angle << endl;
cout << "Desplazamiento:"<<timer<<endl;
cout<< "Value sensor front (right): Ds0:"<<ir0_val<<"Ds1:"<<ir1_val<< "Ds2:"<<ir2_val<<endl;
cout<< "Value sensor front (left): Ds15:"<<ir15_val<<"Ds14:"<<ir14_val<< "Ds13:"<<ir13_val<<endl;
if (timer>=365){
_left_speed = 0;
_right_speed = 0;
}
else{
// Simple bang-bang control
if (compass_angle < (DESIRED_ANGLE - 2)) {
// Turn right
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED - 15;
}
else {
if (compass_angle > (DESIRED_ANGLE + 2)) {
// Turn left
_left_speed = MAX_SPEED - 15;
_right_speed = MAX_SPEED;
}
else {
// Move straight forward
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED;
}
}
}
// Set the motor speeds
setSpeed(_left_speed, _right_speed);
}
}
void MyRobot::run()
{
double compass_angle;
while (step(_time_step) != -1) {
// Read the sensors
const double *compass_val = _my_compass->getValues();
_encoder_right = getRightEncoder();
_encoder_left = getLeftEncoder();
// Convert compass bearing vector to angle, in degrees
compass_angle = convert_bearing_to_degrees(compass_val);
// Print sensor values to console
cout << "Compass angle (degrees): " << compass_angle << endl;
_posicion_final = _encoder_right/1000 *0.0825;// calculate the next position
cout << "posicion :" << _posicion_final << endl;
// control condition
if(_posicion_final < 0.087){
if (compass_angle < (DESIRED_ANGLE - 2)) {
// Turn right
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED - 15;
}
else {
if (compass_angle > (DESIRED_ANGLE + 2)) {
// Turn left
_left_speed = MAX_SPEED - 15;
_right_speed = MAX_SPEED;
}
else {
// Move straight forward
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED;
}
}
}
else{
_left_speed = 0.0;
_right_speed = 0.0;
}
// Set the motor speeds
setSpeed(_left_speed, _right_speed);
}
}
void MyRobot::run()
{
double compass_angle;
double ir0_val = 0.0;
double ir15_val = 0.0;
while (step(_time_step) != -1){
// Read the sensors
const double *compass_val = _my_compass->getValues();
// Convert compass bearing vector to angle, in degrees
compass_angle = convert_bearing_to_degrees(compass_val);
ir0_val = _distance_sensor[0]->getValue();
ir15_val = _distance_sensor[1]->getValue();
// Print sensor values to console
cout << "Compass angle (degrees): " << compass_angle << endl;
cout << "ir0_val(mm): " << ir0_val << endl;
// Simple bang-bang control
if (ir0_val>0){
_left_speed = 0;
_right_speed = 0;
}
else {
if (compass_angle < (DESIRED_ANGLE - 2)) {
// Turn right
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED - 15;
}
else {
if (compass_angle > (DESIRED_ANGLE + 2)) {
// Turn left
_left_speed = MAX_SPEED - 15;
_right_speed = MAX_SPEED;
}
else {
// Move straight forward
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED;
}
}
}
// Set the motor speeds
setSpeed(_left_speed, _right_speed);
}
}
void MyRobot::run()
{
double compass_angle;
double timer=0;
while (step(_time_step) != -1) {
// Read the sensors
const double *compass_val = _my_compass->getValues();
timer++;
// Convert compass bearing vector to angle, in degrees
compass_angle = convert_bearing_to_degrees(compass_val);
// Print sensor values to console
cout << "Compass angle (degrees): " << compass_angle << endl;
cout << "Desplazamiento:"<<timer<<endl;
if (timer>=365){
_left_speed = 0;
_right_speed = 0;
}
else{
// Simple bang-bang control
if (compass_angle < (DESIRED_ANGLE - 2)) {
// Turn right
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED - 15;
}
else {
if (compass_angle > (DESIRED_ANGLE + 2)) {
// Turn left
_left_speed = MAX_SPEED - 15;
_right_speed = MAX_SPEED;
}
else {
// Move straight forward
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED;
}
}
}
// Set the motor speeds
setSpeed(_left_speed, _right_speed);
}
}
void MyRobot::follow_compass_down(double angle)
{
const double *compass_val = _my_compass->getValues();
_compass_angle = convert_bearing_to_degrees(compass_val);
if (_compass_angle < (angle - 1) || _compass_angle >90)
{
_mode = GO_STRAIGHT_RIGHT;
}
else {
if (_compass_angle > (angle + 1) ) {
_mode = GO_STRAIGHT_LEFT;
}
else {
_mode = GO_STRAIGHT;
}
}
}
void MyRobot::run(){
double compass_angle;
double x = 0, z = 0;
double distance = 0;
while (step(_time_step) != -1) {
// Vector to initial position and actual position
vector <double> position, actual_position;
// Read the compass sensor
const double *compass_val = _my_compass->getValues();
compass_angle = convert_bearing_to_degrees(compass_val);
cout << "Compass angle: " << compass_angle << endl;
// Initialization for initial position
position.push_back(x);
position.push_back(z);
position.push_back(compass_angle*M_PI/180);
// If crossed distance (distance) is smaller than desired distance (18.3)
// robot moves depend of encoders position
// gets actual position
if (distance < 18.3 ){ // Estimate error 1.3 m, yellow line distance is 17 m
mode_selection(move());
actual_position = calc_distance(position);
x = actual_position.at(0);
z = actual_position.at(1);
distance = actual_position.at(2);
cout << "x: " << x << ", z: " << z << ", theta: " << compass_angle*M_PI/180 << endl;
cout << "distance: " << distance << endl;
}
// Else, desired distance achivied and robot stops
else {
mode_selection(3);
}
}
}
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;
}
}
void MyRobot::follow_compass(double angle)
{
const double *compass_val = _my_compass->getValues();
_compass_angle = convert_bearing_to_degrees(compass_val);
// Control the direction to the desired angle
if (_compass_angle < (angle - 1))
{
// Turn right
_mode = GO_STRAIGHT_RIGHT;
}
else {
if (_compass_angle > (angle + 1)) {
// Turn left
_mode = GO_STRAIGHT_LEFT;
}
else {
// Move forward
_mode = GO_STRAIGHT;
}
}
}
void MyRobot::control_down()
{
// Read the compass sensor and convert compass bearing vector to angle, in degrees
const double *compass_val = _my_compass->getValues();
_compass_angle = convert_bearing_to_degrees(compass_val);
//If the robot detect the end of a wall at the right side, and the compass point to rigth position
//the robot start turning rigth
if((((_dist_val[12]>400)||(_dist_val[11]>400)) && (_dist_val[0]==0) && (_dist_val[15]==0) && (_dist_val[13]==0) && (_dist_val[14]==0)&&(_dist_val[5]==0 || _dist_val[6]==0 || _dist_val[9]==0 || _dist_val[10] ==0)) && (_compass_angle>0 || _compass_angle<-90))
{
_mode = TURN_RIGHT_MORE;
}
//If the robot detect the end of a wall at the left side, and the compass point to rigth position
//the robot start turning left
if((((_dist_val[3]>400)||(_dist_val[4]>400))&& (_dist_val[2]==0)&& (_dist_val[0]==0) && (_dist_val[15]==0) && (_dist_val[1]==0) && (_dist_val[14]==0)&&(_dist_val[5]==0 || _dist_val[6]==0 || _dist_val[9]==0 || _dist_val[10] ==0)) && (_compass_angle>-179 && _compass_angle<90))
{
_mode = TURN_LEFT_MORE;
}
//If the robot detect a wall in front
if(((_dist_val[0]>DISTANCE || _dist_val[15]> DISTANCE) || (_dist_val[1]>3*DISTANCE || _dist_val[14]>3*DISTANCE)) && (_dist_val[7]<300 || _dist_val[8]<300))
{
//This if-else choose which side of the robot is near to the wall
//and turn to that side
if(_dist_val[3] == 0 && _dist_val[12] == 0)
{
if((_dist_val[2] > _dist_val[13]) || (_dist_val[1] > _dist_val[14]) || (_dist_val[0] > _dist_val[15]))
{
_mode = TURN_BACK_LEFT;
}
else
{
_mode = TURN_BACK_RIGHT;
}
}
else
{
if(_dist_val[3] > _dist_val[12])
{
_mode = TURN_BACK_LEFT;
}
else
{
_mode = TURN_BACK_RIGHT;
}
}
}
else
{
//Logic to follow a wall depending if it is too close or too far to the wall
if(((_dist_val[2]> 4*DISTANCE) || (_dist_val[13]< 3*DISTANCE && _dist_val[13]!=0)) && (_dist_val[1]==0 || _dist_val[14]==0))
{
_mode = TURN_RIGHT;
}
else
{
if((_dist_val[13]> 4*DISTANCE || (_dist_val[2]< 3*DISTANCE && _dist_val[2]!=0)) && (_dist_val[1]==0 || _dist_val[14]==0))
{
_mode = TURN_LEFT;
}
else
{
//If the robot detects a wall behind it
if (_dist_val[7] > 9*DISTANCE || _dist_val[6] > 7*DISTANCE || _dist_val[9] > 7*DISTANCE || _dist_val[8] > 9*DISTANCE)
{
if(_dist_val[0] > 900 || _dist_val[15] > 900){
if(_dist_val[0] > _dist_val[15]){
_mode = TURN_BACK_LEFT;
}else{
_mode = TURN_BACK_RIGHT;
}
}else{
_mode = FORWARD;
}
}
}
}
}
}
// Main function
int main(int argc, char **argv)
{
// Initialize webots
wb_robot_init();
// GPS tick data
int red_line_tick = 0;
int green_circle_tick = 0;
// Get robot devices
WbDeviceTag left_wheel = wb_robot_get_device("left_wheel");
WbDeviceTag right_wheel = wb_robot_get_device("right_wheel");
// Get robot sensors
WbDeviceTag forward_left_sensor = wb_robot_get_device("so3");
wb_distance_sensor_enable(forward_left_sensor, TIME_STEP);
WbDeviceTag forward_right_sensor = wb_robot_get_device("so4");
wb_distance_sensor_enable(forward_right_sensor, TIME_STEP);
WbDeviceTag left_sensor = wb_robot_get_device("so1");
wb_distance_sensor_enable(left_sensor, TIME_STEP);
// Get the compass
WbDeviceTag compass = wb_robot_get_device("compass");
wb_compass_enable(compass, TIME_STEP);
// Get the GPS data
WbDeviceTag gps = wb_robot_get_device("gps");
wb_gps_enable(gps, TIME_STEP);
// Prepare robot for velocity commands
wb_motor_set_position(left_wheel, INFINITY);
wb_motor_set_position(right_wheel, INFINITY);
wb_motor_set_velocity(left_wheel, 0.0);
wb_motor_set_velocity(right_wheel, 0.0);
// Begin in mode 0, moving forward
int mode = 0;
// Main loop
while (wb_robot_step(TIME_STEP) != -1) {
// Get the sensor data
double forward_left_value = wb_distance_sensor_get_value(forward_left_sensor);
double forward_right_value = wb_distance_sensor_get_value(forward_right_sensor);
double left_value = wb_distance_sensor_get_value(left_sensor);
// Read compass and convert to angle
const double *compass_val = wb_compass_get_values(compass);
double compass_angle = convert_bearing_to_degrees(compass_val);
// Read in the GPS data
const double *gps_val = wb_gps_get_values(gps);
// Debug
printf("Sensor input values:\n");
printf("- Forward left: %.2f.\n",forward_left_value);
printf("- Forward right: %.2f.\n",forward_right_value);
printf("- Right: %.2f.\n",left_value);
printf("- Compass angle (degrees): %.2f.\n", compass_angle);
printf("- GPS values (x,z): %.2f, %.2f.\n", gps_val[0], gps_val[2]);
// Send acuator commands
double left_speed, right_speed;
left_speed = 0;
right_speed = 0;
// List the current modes
printf("Mode: %d.\n", mode);
printf("Action: ");
/*
* There are four modes for this controller.
* They are listed as below:
* 0: Finding initial correct angle
* 1: Moving forward
* 2: Wall following
* 3: Rotating after correct point
* 4: Finding green circle + moving on
*/
// If it reaches GPS coords past the red line
if (gps_val[2] > 8.0) {
// Up the GPS tick
red_line_tick = red_line_tick + 1;
}
// If the red line tick tolerance reaches
// more than 10 per cycle, begin mode 3
if (red_line_tick > 10) {
mode = 3;
}
if (mode == 0) { // Mode 0: Find correct angle
printf("Finding correct angle\n");
if (compass_angle < (DESIRED_ANGLE - 1.0)) {
// Turn right
left_speed = MAX_SPEED;
right_speed = 0;
} else if (compass_angle > (DESIRED_ANGLE + 1.0)) {
// Turn left
left_speed = 0;
right_speed = MAX_SPEED;
} else {
// Reached the desired angle, move in a straight line
mode = 1;
}
} else if(mode == 1) { // Mode 1: Move forward
printf("Moving forward.\n");
left_speed = MAX_SPEED;
right_speed = MAX_SPEED;
// When sufficiently close to a wall in front of robot, switch mode to wall following
if ((forward_right_value > 500) || (forward_left_value > 500)) {
mode = 2;
}
}
else if (mode == 2) { // Mode 2: Wall following
if ((forward_right_value > 200) || (forward_left_value > 200)) {
printf("Backing up and turning right.\n");
left_speed = MAX_SPEED / 4.0;
right_speed = - MAX_SPEED / 2.0;
}
else {
if (left_value > 300) {
printf("Turning right away from wall.\n");
left_speed = MAX_SPEED;
right_speed = MAX_SPEED / 1.75;
}
else {
if (left_value < 200) {
printf("Turning left towards wall.\n");
left_speed = MAX_SPEED / 1.75;
right_speed = MAX_SPEED;
}
else {
printf("Moving forward along wall.\n");
left_speed = MAX_SPEED;
right_speed = MAX_SPEED;
}
}
}
} else if (mode == 3) {
// Once arrived, turn to the right
printf("Finding correct angle (again)\n");
if (compass_angle < (90 - 1.0)) {
// Turn right
left_speed = MAX_SPEED;
right_speed = MAX_SPEED / 1.75;
} else if (compass_angle > (90 + 1.0)) {
// Turn left
left_speed = MAX_SPEED / 1.75;
right_speed = MAX_SPEED;
} else {
// Reached the desired angle, move in a straight line
if (green_circle_tick > 10) {
left_speed = 0;
right_speed = 0;
} else {
left_speed = MAX_SPEED;
right_speed = MAX_SPEED;
}
if (gps_val[0] < -3.2) {
green_circle_tick = green_circle_tick + 1;
}
}
}
// Set the motor speeds.
wb_motor_set_velocity(left_wheel, left_speed);
wb_motor_set_velocity(right_wheel, right_speed);
// Perform simple simulation step
} while (wb_robot_step(TIME_STEP) != -1);
// Clean up webots
wb_robot_cleanup();
return 0;
}
void MyRobot::run()
{
double ir0_val = 0.0, ir2_val = 0.0, ir14_val = 0.0,ir1_val = 0.0, compass_angle;
while (step(_time_step) != -1) {
// Read sensors and compass
ir0_val = _distance_sensor[0]->getValue();
ir2_val = _distance_sensor[1]->getValue();
ir14_val = _distance_sensor[2]->getValue();
ir1_val = _distance_sensor[3]->getValue();
const double *compass_val = _my_compass->getValues();
// Convert compass bearing vector to angle, in degrees
compass_angle = convert_bearing_to_degrees(compass_val);
//Show distance sensor values
cout << "ds14: " << ir14_val << "ds1: " << ir1_val << " ds0: " << ir0_val << " ds2: " << ir2_val << endl;
// Robot control logic
if (_mode == GOING_TO_INITIAL_POINT) {
// Shearch for the desired angle
// When sufficiently close to a wall in front of robot,
// switch mode to wall following
if ((ir0_val > DISTANCE_LIMIT -50) || (ir14_val > DISTANCE_LIMIT -20) || (ir1_val > DISTANCE_LIMIT -50)) {
_mode = WALL_FOLLOWER;
cout << "The desired initial position has been reached." << endl;
cout << "Mode " << WALL_FOLLOWER << ": Wall following mode activated" << endl;
}
}
else {
// Wall following
if ((ir0_val > DISTANCE_LIMIT - 50) || (ir14_val > DISTANCE_LIMIT -50) || (ir1_val > DISTANCE_LIMIT -50)) {
_mode = WALL_FOLLOWER;
cout << "Avoiding collision with a wall" << endl;
}
else {
if (ir2_val > DISTANCE_LIMIT + 50) {
_mode = TURN_RIGHT;
cout << "Turning left." << endl;
}
else {
if (ir2_val < DISTANCE_LIMIT - 50) {
_mode = TURN_LEFT;
cout << "Turning right." << endl;
}
else {
_mode = FORWARD;
cout << "Moving forward." << endl;
}
}
}
}
// Send actuators commands according to the mode
switch (_mode){
case FORWARD:
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED;
break;
case GOING_TO_INITIAL_POINT:
if (compass_angle < (DESIRED_ANGLE - 2)) {
// Turn right
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED - 15;
}
else {
if (compass_angle > (DESIRED_ANGLE + 2)) {
// Turn left
_left_speed = MAX_SPEED - 15;
_right_speed = MAX_SPEED;
}
else {
// Move straight forward
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED;
}
}
break;
case TURN_LEFT:
_left_speed = MAX_SPEED / 1.25;
_right_speed = MAX_SPEED;
break;
case TURN_RIGHT:
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED / 1.25;
break;
case WALL_FOLLOWER:
_left_speed = -MAX_SPEED / 20.0;
_right_speed = -MAX_SPEED / 4.0;
break;
default:
break;
}
// Set the motor speeds
setSpeed(_left_speed, _right_speed);
}
}
//////////////////////////////////////////////
//function who start the move of the robot taking a straight way,
//and helped with the convert bearing to degrees function and sensor distance
void MyRobot::run()
{
double compass_angle;
double ir0_val=0.0,ir1_val=0.0,ir2_val=0.0,ir3_val=0.0,ir4_val=0.0,ir5_val=0.0,ir6_val=0.0,ir7_val=0.0;
while (step(_time_step) != -1) {
// Read the compass
const double *compass_val = _my_compass->getValues();
//Read the _distancesensor
ir1_val=_distancesensor[1]->getValue();
ir2_val=_distancesensor[2]->getValue();
ir3_val=_distancesensor[3]->getValue();
ir4_val=_distancesensor[4]->getValue();
ir5_val=_distancesensor[5]->getValue();
ir6_val=_distancesensor[6]->getValue();
ir7_val=_distancesensor[7]->getValue();
ir0_val=_distancesensor[0]->getValue();
// Convert compass bearing vector to angle, in degrees
compass_angle = convert_bearing_to_degrees(compass_val);
// Print sensor values to console
cout << "Compass angle (degrees): " << compass_angle << endl;
cout<<"ds0:"<<ir0_val<<"\tds1:"<<ir1_val<<endl;
cout<<"ds2:"<<ir2_val<<"\tds3:"<<ir3_val<<endl;
cout<<"ds4:"<<ir4_val<<"\tds5:"<<ir5_val<<endl;
cout<<"ds6:"<<ir6_val<<"\tds7:"<<ir7_val<<endl;
// Simple bang-bang control
if (compass_angle < (DESIRED_ANGLE - 2)) {
// Turn right
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED - 15;
}
else{
if (compass_angle > (DESIRED_ANGLE + 2)) {
// Turn left
_left_speed = MAX_SPEED - 15;
_right_speed = MAX_SPEED;
}
else{
// Move straight forward
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED;
}
if((ir0_val>DISTANCEMAX)||(ir1_val>DISTANCEMAX)||(ir2_val>DISTANCEMAX)||(ir3_val>DISTANCEMAX)||(ir4_val>DISTANCEMAX)||(ir5_val>DISTANCEMAX)||(ir6_val>DISTANCEMAX)||(ir7_val>DISTANCEMAX)){
_left_speed=0;
_right_speed=0;
setSpeed(_left_speed,_right_speed);
}
}
// Set the motor speeds
setSpeed(_left_speed, _right_speed);
}
}
void MyRobot::run()
{
//init the variable used by the compass
double compass_angle;
//init a variable sum and put inside the value zero
int sum = 0;
//init all kind of colours who can take the image of the spherical camera
unsigned char green = 0, red = 0, blue = 0;
//init the percentage of yellow
double percentage_yellow = 0.0;
// get size of images for spherical camera
int image_width_s = _spherical_camera->getWidth();
int image_height_s = _spherical_camera->getHeight();
cout << "Size of spherical camera image: " << image_width_s << ", " << image_height_s << endl;
//loop until robot disconect
while (step(_time_step) != -1) {
//init inside the loop the value of sum to zero
sum = 0;
// get current image from forward camera
const unsigned char *image_s= _spherical_camera->getImage();
// count number of pixels that are yellow in the screen
//the pixel values goes until 255, from 245 for all colour components
for (int x = 0; x < image_width_s; x++) {
for (int y = 0; y < image_height_s; y++) {
green = _spherical_camera->imageGetGreen(image_s, image_width_s, x, y);
red = _spherical_camera->imageGetRed(image_s, image_width_s, x, y);
blue = _spherical_camera->imageGetBlue(image_s, image_width_s, x, y);
//depending on the RGB rank the values of each color should change
//in this case a completely yellow screen reach when green is iqual to 255,red to 255 and finally blue zero
//but the spherical camera screen does not caught only yellow,
//for this reason the rank of the values are less restrictive
if ((green > 245) && (red > 245) && (blue <51)) {
sum = sum + 1;
}
}
}
//here percentage yellow takes the width and the height of the image to discover his percentage
percentage_yellow = (sum / (float) (image_width_s * image_height_s)) * 100;
cout << "Percentage of yellow in spherical camera image: " << percentage_yellow << endl;
//if the percentage of yellow is higher than 0.25 the screen put the code: yellow line detected
if(percentage_yellow>0.25){
cout<<"linea amarilla detectada"<<endl;
}
else{
cout<<"linea amarilla no detectada"<<endl;
}
//now the robot is following a straight way with the compass
//Usefull to detect the other yellow line
const double *compass_val = _my_compass->getValues();
// convert compass bearing vector to angle, in degrees
compass_angle = convert_bearing_to_degrees(compass_val);
// print sensor values to console
cout << "Compass angle (degrees): " << compass_angle << endl;
if (compass_angle < (DESIRED_ANGLE - 2)) {
// turn right
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED - 15;
}
else {
if (compass_angle > (DESIRED_ANGLE + 2)) {
// turn left
_left_speed = MAX_SPEED - 15;
_right_speed = MAX_SPEED;
}
else {
// move straight forward
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED;
}
}
// set the motor speeds
setSpeed(_left_speed, _right_speed);
}
}
void MyRobot::run()
{
double sum;
gps_initial[2] = 1000.0;
x = 0;
y = 0;
z = 0;
counter = 0;
_compass_angle_green[0]= 1000.0;
_compass_angle_green[1]= 1000.0;
end = 0;
metres =0;
num_person = 0;
turn = false;
back = false;
inside = false;
person = false;
following = false;
while (step(_time_step) != -1)
{
get_distances();
sum = 0;
for (int i = 0; i < NUM_DISTANCE_SENSOR; i++)
{
sum = sum + _dist_val[i];
}
//First we calculate initial gps position, the robot wont start until we know it
if(gps_initial[2] != 1000.0){
if (((gps[2]- gps_initial[2])>17) && (num_person < 2)){
//Start the identification
if ((_compass_angle_green[0] == 1000.0) || (_compass_angle_green[1] == 1000.0))
{
scaner_turn_around();
}
else
{
gps[2] = gps_initial[2] + 18;
_forward_camera->disable();
if (person == false)
{
rescue_person(_compass_angle_green[1]);
}
else
{
rescue_person(_compass_angle_green[0]);
}
}
}
else
{
//Start the going down
if (num_person == 2){
/*Checks if there is any distance sensor detecting a wall
and if this wall is close enought (200) to change the mode
from follow_compass to control */
if (sum <= 200)
{
//To follow down direction of the map
follow_compass_down(-135);
}
else
{
control_down();
}
}
else
{
//Start the going up
gps_average();
if (sum <= 200)
{
follow_compass(45);
}
else
{
control_up();
}
}
}
}else{
gps_average();
// Read the sensors
const double *compass_val = _my_compass->getValues();
// Convert compass bearing vector to angle, in degrees
_compass_angle = convert_bearing_to_degrees(compass_val);
//We placed the robot in the correct direction.
if(_compass_angle >= 35 && _compass_angle < 55){
_mode = STOP;
}else{
_mode = FAST_TURN_AROUND;
}
}
// Set the mode
mode();
// Set the motor speeds
setSpeed(_left_speed, _right_speed);
}
}
void MyRobot::run()
{
double ir1_val = 0.0, ir14_val = 0.0;
double ir3_val = 0.0, ir12_val = 0.0;
double compass_angle;
while (step(_time_step) != -1) {
// Read the sensors
ir1_val = _distance_sensor[0]->getValue();
ir14_val = _distance_sensor[1]->getValue();
ir3_val = _distance_sensor[2]->getValue();
ir12_val = _distance_sensor[3]->getValue();
const double *compass_val = _my_compass->getValues();
compass_angle = convert_bearing_to_degrees(compass_val);
cout << "ds1: " << ir1_val << " ds14:" << ir14_val << endl;
cout << "ds3: " << ir3_val << " ds12:" << ir12_val << endl;
//Condition to choose between compass and sensor motion mode
if ((ir1_val < DISTANCE_LIMIT) && (ir14_val < DISTANCE_LIMIT)&& (ir12_val < DISTANCE_LIMIT)&& (ir3_val < DISTANCE_LIMIT)){
//Compass mode
if ( compass_angle < (DESIRED_ANGLE - 2)) {
_mode = TURN_RIGHT;
}
else{if(compass_angle > (DESIRED_ANGLE + 2)) {
_mode = TURN_LEFT;
}
else {
_mode = FORWARD;
}
}}else{
//Sensor mode
//Condition to stop and go backwards
if (((ir1_val > 800) || (ir14_val > 800)||(ir3_val > 800) || (ir12_val > 800)) && ((ir1_val != 0) || (ir14_val != 0))){
_mode = BACKWARDS;
cout << "Backwards." << endl;}
else{
//Condition to turn using sensors
if ((ir12_val > DISTANCE_LIMIT-50)|| (ir14_val > DISTANCE_LIMIT)) {
_mode = TURN_LEFT;
cout << "Turning left." << endl;
}
else {
if ((ir3_val > DISTANCE_LIMIT-50)||(ir1_val > DISTANCE_LIMIT)) {
_mode = TURN_RIGHT;
cout << "Turning right." << endl;
}
else {
_mode = FORWARD;
cout << "Moving forward." << endl;
}}}
}
// Send actuators commands according to the mode
switch (_mode){
case STOP:
_left_speed = 0;
_right_speed = 0;
break;
case FORWARD:
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED;
break;
case TURN_LEFT:
_left_speed = MAX_SPEED/3.0;
_right_speed = MAX_SPEED;
break;
case TURN_RIGHT:
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED/3.0;
break;
case BACKWARDS:
_left_speed = -MAX_SPEED;
_right_speed = -MAX_SPEED;
break;
default:
break;
}
// Set the motor speeds
setSpeed(_left_speed, _right_speed);
}
}
