Robots suiveurs 3 - Code Communication Initial (C)

/*
 * Copyright 1996-2020 Cyberbotics Ltd.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/*
 * Description:   A controller which sends datas from the emitter to the
 *                receiver while the robots avoid the obstacles.
 */

#include <stdio.h>
#include <string.h>
#include <webots/distance_sensor.h>
#include <webots/emitter.h>
#include <webots/motor.h>
#include <webots/receiver.h>
#include <webots/robot.h>

#define MAX_SPEED 6.28
#define TIME_STEP 64
#define COMMUNICATION_CHANNEL 1

typedef enum { EMITTER, RECEIVER } robot_types;

int main() {
  int i;
  WbDeviceTag ps[8], communication, left_motor, right_motor;
  char ps_names[8][4] = {
    "ps0", "ps1", "ps2", "ps3",
    "ps4", "ps5", "ps6", "ps7"
  };
  robot_types robot_type;
  int message_printed = 0; /* used to avoid printing continuously the communication state */

  wb_robot_init();
  
  /* get a handler to the motors and set target position to infinity (speed control). */
  left_motor = wb_robot_get_device("left wheel motor");
  right_motor = wb_robot_get_device("right wheel motor");
  wb_motor_set_position(left_motor, INFINITY);
  wb_motor_set_position(right_motor, INFINITY);
  wb_motor_set_velocity(left_motor, 0.0);
  wb_motor_set_velocity(right_motor, 0.0);

  /*
   * as we are using the same controller for the emitter and the reciever,
   * we need to differentiate them
   */
  if (strncmp(wb_robot_get_name(), "MyBot emitter", 13) == 0) {
    robot_type = EMITTER;
    communication = wb_robot_get_device("emitter");

    /* if the cannel is not the right one, change it */
    const int channel = wb_emitter_get_channel(communication);
    if (channel != COMMUNICATION_CHANNEL) {
      wb_emitter_set_channel(communication, COMMUNICATION_CHANNEL);
    }
  } else if (strncmp(wb_robot_get_name(), "MyBot receiver", 14) == 0) {
    robot_type = RECEIVER;
    communication = wb_robot_get_device("receiver");
    wb_receiver_enable(communication, TIME_STEP);
  } else {
    printf("Unrecognized robot name '%s'. Exiting...\n", wb_robot_get_name());
    wb_robot_cleanup();
    return 0;
  }

  /* get a handler to the distance sensors and enable them */
  for (i = 0; i < 8 ; i++) {
    ps[i] = wb_robot_get_device(ps_names[i]);
    wb_distance_sensor_enable(ps[i], TIME_STEP);
  }

  while (wb_robot_step(TIME_STEP) != -1) {
    /*
     * the emitter simply sends the message but the receiver
     * has to check if there is something before it can reads the buffer.
     */
    if (robot_type == EMITTER) {
      /* send null-terminated message */
      const char *message = "Hello!";
      wb_emitter_send(communication, message, strlen(message) + 1);
    } else {
      /* is there at least one packet in the receiver's queue ? */
      if (wb_receiver_get_queue_length(communication) > 0) {
        /* read current packet's data */
        const char *buffer = wb_receiver_get_data(communication);
        const double *dir = wb_receiver_get_emitter_direction(communication);
        double signal = wb_receiver_get_signal_strength(communication);


        if (message_printed != 1) {
          /* print null-terminated message */
          printf("received: %s (signal=%g, dir=[%g %g %g])\n",
          buffer, signal, dir[0], dir[1], dir[2]);
          message_printed = 1;
        }
        /* fetch next packet */
        wb_receiver_next_packet(communication);
      } else {
        if (message_printed != 2) {
          printf("Communication broken!\n");
          message_printed = 2;
        }
      }
    }

    // read sensors outputs
    double ps_values[8];
    for (i = 0; i < 8 ; i++)
      ps_values[i] = wb_distance_sensor_get_value(ps[i]);

    // detect obstacles
    bool right_obstacle =
      ps_values[0] > 80.0 ||
      ps_values[1] > 80.0 ||
      ps_values[2] > 80.0;
    bool left_obstacle =
      ps_values[5] > 80.0 ||
      ps_values[6] > 80.0 ||
      ps_values[7] > 80.0;

    // initialize motor speeds at 50% of MAX_SPEED.
    double left_speed  = 0.5 * MAX_SPEED;
    double right_speed = 0.5 * MAX_SPEED;

    // modify speeds according to obstacles
    if (left_obstacle) {
      // turn right
      left_speed  += 0.5 * MAX_SPEED;
      right_speed -= 0.5 * MAX_SPEED;
    }
    else if (right_obstacle) {
      // turn left
      left_speed  -= 0.5 * MAX_SPEED;
      right_speed += 0.5 * MAX_SPEED;
    }

    // write actuators inputs
    wb_motor_set_velocity(left_motor, left_speed);
    wb_motor_set_velocity(right_motor, right_speed);
  }

  wb_robot_cleanup();

  return 0;
}