Controller for receiver robotic car

/*
 * File:          purple_2_0.c
 * Date:          08/07/2020
 * Description:  custom made for purple bot v2.
 * Author:       Azeem.
 * Modifications:
 */

/*
 * You may need to add include files like <webots/distance_sensor.h> or
 * <webots/motor.h>, etc.
 */
#include <webots/robot.h>
#include <webots/distance_sensor.h>
#include <webots/motor.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h> 
#include <string.h>
#include <webots/receiver.h>
#include <webots/supervisor.h>

// black line = 490+
// white surface = 105-

/*
 * You may want to add macros here.
 */
#define TIME_STEP 64
#define COMMUNICATION_CHANNEL 1
#define Communication_channel_b 2

/*
 * This is the main program.
 * The arguments of the main function can be specified by the
 * "controllerArgs" field of the Robot node
 */
int main(int argc, char **argv) {
  /* necessary to initialize webots stuff */
  wb_robot_init();
 // do this once only
  WbNodeRef robot_node = wb_supervisor_node_get_from_def("Purple");
  WbFieldRef trans_field = wb_supervisor_node_get_field(robot_node, "translation");

 //internal Variables 
 int i,j=0,k=0,A=0;
 int message_printed = 0,message_printed2 = 0;
 
 // setup receiver
  WbDeviceTag communication = wb_robot_get_device("receiver");
  wb_receiver_enable(communication, TIME_STEP);
  
  //setup Emitter
 WbDeviceTag communicationb = wb_robot_get_device("Purple Receiver");
 wb_receiver_enable(communicationb ,TIME_STEP);
  
      // initialise distance sensors
  
  WbDeviceTag DSR = wb_robot_get_device("ds_right");
  wb_distance_sensor_enable(DSR, TIME_STEP);
  WbDeviceTag DSL = wb_robot_get_device("ds_left");
  wb_distance_sensor_enable(DSL, TIME_STEP);
  
    // initialise motors
  WbDeviceTag wheels[4];
  char wheels_names[4][8] = {"wheel1", "wheel2", "wheel3", "wheel4"};
  for (i = 0; i < 4; i++) {
    wheels[i] = wb_robot_get_device(wheels_names[i]);
    wb_motor_set_position(wheels[i], INFINITY);
    wb_motor_set_velocity(wheels[i], 0.0);
    }
  //  wb_motor_set_velocity(wheels[0], 0.0);
  //  wb_motor_set_velocity(wheels[1], 0.0);
  //  wb_motor_set_velocity(wheels[2], 0.0);
  //  wb_motor_set_velocity(wheels[3], 0.0);
  
  // Initialize other sensors
  
  WbDeviceTag lsensor = wb_robot_get_device("S2");
  wb_distance_sensor_enable(lsensor, TIME_STEP);
  WbDeviceTag Rsensor = wb_robot_get_device("S3");
  wb_distance_sensor_enable(Rsensor, TIME_STEP);
  WbDeviceTag lstopsensor = wb_robot_get_device("SS1");
  wb_distance_sensor_enable(lstopsensor, TIME_STEP);
  WbDeviceTag Rstopsensor = wb_robot_get_device("SS2");
  wb_distance_sensor_enable(Rstopsensor, TIME_STEP);
  
// Define various  functions

void Forward()
{ 
    double lspeed = 4.0;
    double rspeed = 4.0;
                 // write actuators inputs
    wb_motor_set_velocity(wheels[0], lspeed);
    wb_motor_set_velocity(wheels[1], rspeed);
    wb_motor_set_velocity(wheels[2], lspeed);
    wb_motor_set_velocity(wheels[3], rspeed);
    
}

void turnleft()
{ 
    double lspeed = -2.0;
    double rspeed = 4.0;
                 // write actuators inputs
    wb_motor_set_velocity(wheels[0], lspeed);
    wb_motor_set_velocity(wheels[1], rspeed);
    wb_motor_set_velocity(wheels[2], lspeed);
    wb_motor_set_velocity(wheels[3], rspeed);
    
}

void turnright()
{ 
    double lspeed = 4.0;
    double rspeed = -2.0;
                 // write actuators inputs
    wb_motor_set_velocity(wheels[0], lspeed);
    wb_motor_set_velocity(wheels[1], rspeed);
    wb_motor_set_velocity(wheels[2], lspeed);
    wb_motor_set_velocity(wheels[3], rspeed);
    
}

void Stop()
{ 
    double lspeed = 0.0;
    double rspeed = 0.0;
                 // write actuators inputs
    wb_motor_set_velocity(wheels[0], lspeed);
    wb_motor_set_velocity(wheels[1], rspeed);
    wb_motor_set_velocity(wheels[2], lspeed);
    wb_motor_set_velocity(wheels[3], rspeed);
    
}
  

  /*
   * You should declare here WbDeviceTag variables for storing
   * robot devices like this:
   *  WbDeviceTag my_sensor = wb_robot_get_device("my_sensor");
   *  WbDeviceTag my_actuator = wb_robot_get_device("my_actuator");
   */

  /* main loop
   * Perform simulation steps of TIME_STEP milliseconds
   * and leave the loop when the simulation is over
   */
  while (wb_robot_step(TIME_STEP) != -1) {

  const double *Svalues = wb_supervisor_field_get_sf_vec3f(trans_field);
 // printf("My_Purple_ROBOT is at position: %g %g %g\n", Svalues[0], Svalues[1], Svalues[2]);
 //  fprintf(fptr,"%g, %g, %g\n", Svalues[0], Svalues[1], Svalues[2]);

         // this is done repeatedly
   
    const double lvalue = wb_distance_sensor_get_value(lsensor);
    const double Rvalue = wb_distance_sensor_get_value(Rsensor);
    const double lstopvalue = wb_distance_sensor_get_value(lstopsensor);
    const double Rstopvalue = wb_distance_sensor_get_value(Rstopsensor);
    const double DSRV = wb_distance_sensor_get_value(DSR);
    const double DSLV = wb_distance_sensor_get_value(DSL);
  //  printf("Left Sensor value is %f\n , Right Sensor value is %f\n", lvalue , Rvalue);
  //  printf("DSLvalue is %f\n , DSRvalue is %f\n", DSLV , DSRV );
 
 /// For Receiving Red bot signal
  
  if (wb_receiver_get_queue_length(communication) > 0) {
        /* read current packet's data */
    
        const char *buffer = wb_receiver_get_data(communication);

        if (message_printed != 1) {
          /* print null-terminated message */
          printf("Communication Inbound: \"%s\"\n", buffer);
          printf("Purple Robot En-Route to Starting Point\n");
          message_printed = 1;
          A = 1;
          j = 1;   
            const double *Svalues = wb_supervisor_field_get_sf_vec3f(trans_field);
 //  printf("My_Purple_ROBOT is at position: %g %g %g\n", Svalues[0], Svalues[1], Svalues[2]);
 //  fprintf(fptr,"%g, %g, %g\n", Svalues[0], Svalues[1], Svalues[2]);
       
        }
        /* fetch next packet */
        wb_receiver_next_packet(communication);
      } else {
        if (message_printed != 2) {
          printf("No Inbound Communication! \n");
          message_printed = 2;
         // j = 0;
        }
      }

 /// For Receiving Blue bot signal
   
   if (wb_receiver_get_queue_length(communicationb) > 0) {
        /* read current packet's data */
    
        const char *buffer2 = wb_receiver_get_data(communicationb);

        if (message_printed2 != 1) {
          /* print null-terminated message */
          printf("Communication Inbound: \"%s\"\n", buffer2);
          printf("Purple Robot En-Route to Starting Point\n");
          message_printed2 = 1;
          k = 1;   
          A = 1;
            const double *Svalues = wb_supervisor_field_get_sf_vec3f(trans_field);
 //  printf("My_Purple_ROBOT is at position: %g %g %g\n", Svalues[0], Svalues[1], Svalues[2]);
 //  fprintf(fptr,"%g, %g, %g\n", Svalues[0], Svalues[1], Svalues[2]);
       
        }
        /* fetch next packet */
        wb_receiver_next_packet(communicationb);
      } else {
        if (message_printed2 != 2) {
          printf("No Inbound Communication! \n");
          message_printed2 = 2;
          j = 0;
        }
      }
   
  
  
     if(DSLV <= 850 || DSRV <= 850)
   { turnright();}   
   
else{   
    if(lvalue <= 105 && Rvalue <= 105 && A == 1) //Go forward
    { Forward();}
    
    if(lvalue >= 110 && Rvalue <= 105 )    // turn left
    { turnleft();}
    
       if(lvalue >= 110 && Rvalue >= 110 && j == 1) //Left turn cond 2
   { turnleft();}
    
       if(lvalue >= 110 && Rvalue >= 110) //Right turn cond 2
   { turnright();}
    
    if(lvalue <= 105 && Rvalue >= 110 ) //Right turn
    { turnright();}
    



  

    
   if (lvalue >= 110 && Rvalue >=110 && DSLV <= 999 && DSRV <= 999) //Stop
    { Stop();
   //  fclose(fptr);
            if (message_printed == 0) {
          /* print null-terminated message */
         printf("Purple bot has reached the initial position of Red Bot \n");
          message_printed = 1;
        }
       else {
        if (message_printed == 1) {
          printf("Blue bot is En-Route to Common collection Point \n");
          message_printed = 2;
        }
      }
     const char *message = "Move To Point A";
     wb_emitter_send(communicationb, message, strlen(message) + 1);
     }     
}
  
    /*
     * Read the sensors :
     * Enter here functions to read sensor data, like:
     *  double val = wb_distance_sensor_get_value(my_sensor);
     */

    /* Process sensor data here */

    /*
     * Enter here functions to send actuator commands, like:
     * wb_motor_set_position(my_actuator, 10.0);
     */
  };

  /* Enter your cleanup code here */

  /* This is necessary to cleanup webots resources */
  wb_robot_cleanup();
 return 0;
}