Scount 2.0 collision avoidance controller for Scout 2.0 supplied with four distance sensors

ScoutCollisionAvoidance.cpp

#include <webots/Robot.hpp>
#include <webots/Motor.hpp>
#include <webots/DistanceSensor.hpp>
#include <webots/Lidar.hpp>
#include <algorithm>
#include <iostream>

#define TIME_STEP 64
#define MAX_SPEED 5.0
#define ALPHA 2.0
#define DISTANCE_THRESHOLD 980.0
#define DISTANCE_MAX 1000.0

using namespace webots;

int main(int argc, char **argv) {
  Robot *robot = new Robot();

  DistanceSensor *ds[4];
  
  char dsNames[4][12] = {"ds_left_0", "ds_left_1", "ds_right_0", "ds_right_1"};
  
  for (int i = 0; i < 4; i++) {
    ds[i] = robot->getDistanceSensor(dsNames[i]);
    ds[i]->enable(TIME_STEP);
  }
  
  Motor *wheels[4];
  
  char wheelsNames[4][10] = {"motor_fl", "motor_fr", "motor_rl", "motor_rr"};
    
  for (int i = 0; i < 4 ; i++) {
    wheels[i] = robot->getMotor(wheelsNames[i]);
    wheels[i]->setPosition(INFINITY);
    wheels[i]->setVelocity(0.0);
  }
  
  // Lidar *lid;
  // lid = robot->getLidar("Velodyne VLP-16");
  // lid->enable(TIME_STEP);
  // lid->enablePointCloud();
  
  double dv[4];
  
  int stuck_count = 0;
  
  while (robot->step(TIME_STEP) != -1) {
  
    // read sensors
    dv[0] = (ds[0]->getValue() < 980.0)? (1000.0 - ds[0]->getValue()) / 1000.0 : 0;
    dv[1] = (ds[1]->getValue() < 980.0)? (1000.0 - ds[1]->getValue()) / 1000.0 : 0;
    dv[2] = (ds[2]->getValue() < 980.0)? (1000.0 - ds[2]->getValue()) / 1000.0 : 0;
    dv[3] = (ds[3]->getValue() < 980.0)? (1000.0 - ds[3]->getValue()) / 1000.0 : 0;
  
    // process sensor data
    bool left_obstacle = dv[0] + dv[1] > 0.0001; 
    bool right_obstacle = dv[2] + dv[3] > 0.0001;
    
    double v0 = 0.2 * MAX_SPEED;
    double v_left  = v0;
    double v_right = v0;
    
    if (stuck_count > 0) {
      stuck_count++;
      v_left  = -v0;
      v_right = v0;
      
      if (stuck_count >= 10) {
        if (!left_obstacle || !right_obstacle) {
          stuck_count = 0;
        }
      }
    }
    else {
    
      if (left_obstacle && right_obstacle && abs((dv[0] + dv[1])-(dv[2] + dv[3])) <= 0.1 ) {
        // stuck
        stuck_count = 1;
        v_left  = 0;
        v_right = 0;
      }
      else if (left_obstacle && dv[0] + dv[1] > dv[2] + dv[3]) {
        // turn right
        v_left  = v0 + ALPHA * dv[0] * v0 + ALPHA * dv[1] * v0;
        v_right = -v0 - ALPHA * dv[0] * v0 - ALPHA * dv[1] * v0;
      }
      else if (right_obstacle && dv[2] + dv[3] > dv[0] + dv[1]) {
        // turn left
        v_left  = -v0 - ALPHA * dv[2] * v0 - ALPHA * dv[3] * v0;
        v_right = v0 + ALPHA * dv[2] * v0 + ALPHA * dv[3] * v0;
      }
    
    }
    
    // progress printouts
    printf("\n%g | %g ^ %g | %g \n", ds[1]->getValue(), ds[0]->getValue(), ds[2]->getValue(), ds[3]->getValue());
    printf("%d (%g) <obstacle> %d (%g) diff=%g\n", left_obstacle, dv[0] + dv[1], right_obstacle, dv[2] + dv[3], abs((dv[0] + dv[1])-(dv[2] + dv[3])));
    printf("%g < v > %g\n", v_left, v_right);
    
    // printf("lidar has %d points\n", lid->getNumberOfPoints());
    
    // send actuator commands
    wheels[0]->setVelocity(v_left);
    wheels[1]->setVelocity(v_right);
    wheels[2]->setVelocity(v_left);
    wheels[3]->setVelocity(v_right);
    
  };

  delete robot;
  return 0;
}