robot_Green-Blue.ino

#include <esp_now.h>
#include <WiFi.h>

//pins to control the robot
#define IN1 18
#define IN2 19
#define IN3 21
#define IN4 22
#define ENA 25 //enable M1
#define ENB 26 //enable M2

//define robot and ball
#define team "Green"
#define pos "Blue"
#define adteam "Pink"
#define ball "Yellow"
#define quant_robots 2
#define quant_team 2

//define limits
#define angl 20
#define distl 50
#define time 8

//define goal coords
#define xgoal 10
#define ygoal 225

String coords_str; //variable that receives the info
int goal=0;

void setup() {
  Serial.begin(115200);
  WiFi.mode(WIFI_STA);
  Serial.print("Mac Address in Station: "); 
  Serial.println(WiFi.macAddress());
  InitESPNow(); //starts ESPNow
  esp_now_register_recv_cb(OnDataRecv); //define function to receive data via ESPNow
  //define pins as output
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  pinMode(IN3,OUTPUT);
  pinMode(IN4,OUTPUT);
  pinMode(ENA,OUTPUT);
  pinMode(ENB,OUTPUT);
  //control speed: speed(speed_m1, speed_m2)
  speed(1024,1024); //use values between 600 and 1024
  stop(); //stops the robot
}

void loop(){
  func();
}

//***main function***//
void func(){
  //variables for positions
  int xrobot=-1,yrobot=-1,arobot=400;
  int xball=-1,yball=-1;
  int xad[3]={-1,-1,-1},yad[3]={-1,-1,-1};
  String robot = team; robot += "-"; robot += pos;
  
  Serial.println();
  Serial.println(coords_str);
  //finds robot
  if(coords_str.indexOf(robot)!=-1){ //gets x, y and angle of the robot
    String robot_temp = coords_str.substring(coords_str.indexOf(robot),coords_str.indexOf(";",coords_str.indexOf(robot)));
    xrobot = robot_temp.substring(robot_temp.indexOf('(')+1,robot_temp.indexOf(',')).toInt();
    yrobot = robot_temp.substring(robot_temp.indexOf(',')+1,robot_temp.indexOf(')')).toInt();
    arobot = robot_temp.substring(robot_temp.indexOf(')')+1,robot_temp.indexOf(';')).toInt();
  }else{
    stop();
  }
  
  //finds ball
  if(coords_str.indexOf(ball)!=-1){ //x e y da bola
    String ball_temp = coords_str.substring(coords_str.indexOf(ball),coords_str.indexOf(";",coords_str.indexOf(ball)));
    xball = ball_temp.substring(ball_temp.indexOf('(')+1,ball_temp.indexOf(',')).toInt();
    yball = ball_temp.substring(ball_temp.indexOf(',')+1,ball_temp.indexOf(')')).toInt();
  }else{
    stop();
  }
  
  //finds adversaries
  int i,j=-1;
  for(i=0;i<(quant_robots/quant_team);i++){
    j=coords_str.indexOf(adteam,j+1);
    if(j!=-1){
      String ad = coords_str.substring(j,coords_str.indexOf(";",j));
      xad[i] = ad.substring(ad.indexOf('(')+1,ad.indexOf(',')).toInt();
      yad[i] = ad.substring(ad.indexOf(',')+1,ad.indexOf(')')).toInt();
      printc("Adversary",xad[i],yad[i]);
    }
  }
  
  //starts strategy
  if(xrobot!=-1 && yrobot!=-1 && arobot!=400 && xball!=-1 && yball!=-1){ 
    printc("Robot",xrobot,yrobot);
    printc("Ball",xball,yball);
    strategy(xrobot,yrobot,arobot,xball,yball,xad,yad);
  }else{
    parar();
  }
}

//***functions for the strategy***//

//main
void strategy(int xrobot,int yrobot,int arobot,int xball,int yball,int xad[],int yad[]){
  int aball = angle(xrobot,yrobot,xball,yball); //calculates the angle bteween the robot and the ball
  int distball = distance(xrobot,yrobot,xball,yball); //calculates the distance between the robot and the ball
  int with_ball = approach(arobot,aball,distball); //approaches the robot to the ball
  if(with_ball){
    int agoal = angle(xrobot,yrobot,xgoal,ygoal); //calculates the angle bteween the robot and the goal
    int distgoal = distancia(xrobot,yrobot,xgoal,ygoal); //calculates the distance between the robot and the goal
    int goal = approach(arobot,agoal,distgoal); //approaches the robot to the goal
  }
}

//checks for aligned objects
int aligned(int arobot,int a){
  //if there's an object forward
  if((arobot > (a-angl) && arobot < (a+angl)) || (arobot > (a-angl+360) && arobot < (a+angl+360))){
    return 1;
  }else{
    //if there's an object backward
    if((arobot > (a-angl+180) && arobot < (a+angl+180)) || (arobot > (a-angl+540) && arobot < (a+angl+540))){
      return -1;
    }else{
      return 0;
    }
  }
}

//aligns the robot to an object
int search(int arobot,int a){
  int object = aligned(arobot,a); //checks for aligned objects
  if(!object){ //if the object isn't aligned
    //turns the robot
    int dif = a - arobot;
    if(dif < 0)
      dif+=360;
    if(dif < 180){
      right();
    }else{
      left();
    }
  }
  return object; //returns the alignment
}

//approaches the robot to an object
int approach(int arobot,int a,int dist){
  int dir = search(arobot,a); //aligns the robot to an object
  if(dir){ //if they're aligned
    if(dist > distl){ //if they're distant
      if(dir==1){ //if the object is forward
        forward();
      }else{
        if(dir==-1){ //if the object is backward
          backward();
        }
      }
    }else{
      return 1; //they're close
    }
  }
  return 0; //they're distant
}

//calculates distance
int distance(int x1,int y1,int x2,int y2){
  return sqrt(pow((y2-y1),2)+pow((x2-x1),2)); //calculates and returns the distance between two objects
}

//calculates angle
int angle(int x1,int y1,int x2,int y2){
  int a = atan2((y2-y1),(x2-x1))*180/M_PI; //calculates the angle between two objects
  //fixes negative angles
  if(a < 0){
    a+=360;
  }
  return a; //returns the angle
}

//***moving functions***//

//function for the speed
void speed(int speed1, int speed2){
  ledcAttachPin(ENA, 0);//pin ENA to channel 0. (Engine 1)
  ledcSetup(0, 15000, 10);//frequency of 1000Hz and resolution of 10bits for channel 0.
  ledcAttachPin(ENB, 1);//pin ENB to channel 1. (Engine 2)
  ledcSetup(1, 15000, 10);//frequency of 1000Hz and resolution of 10bits for channel 1. 

  //control speed
  ledcWrite(0, speed1);
  ledcWrite(1, speed2);
}

//function to stop the robot
void stop(){
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,HIGH);
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,HIGH);
  digitalWrite(ENA,LOW);
  digitalWrite(ENB,LOW);
}

//function to go backward
void backward() {
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH);
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  delay(time);
  stop();
}

//function to go forward
void forward(){
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW);
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  delay(time);
  stop();
}

//function to turns to the right
void rigth(){
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW);
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  delay(time);
  stop();
}

//function to turns to the left
void left(){
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH);
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  delay(time);
  stop();
}

//***communication functions***//

//print coords
void printc(String type,int x, int y){
  Serial.print(type);
  Serial.print(": (");
  Serial.print(x);
  Serial.print(",");
  Serial.print(y);
  Serial.println(")");
}

//ESPNow inicialization
void InitESPNow() {
  //If it was succeed
  if (esp_now_init() == ESP_OK) {
    Serial.println("ESPNow Init Success");
  }
  //If there was an error
  else {
    Serial.println("ESPNow Init Failed");
    ESP.restart();
  }
}

//recieved data
void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) {
  char macStr[18];
  snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  //Serial.print("Received from: "); 
  //Serial.println(macStr);
  //Serial.println(data_len);
  //Serial.println((char*)data);
  //Serial.println("");
  coords_str = "";
  coords_str = (String)((char*)data);
}