Grupo 18

Integrantes:

• Fiorella Victoria Hernández Mejía
• Frantzes Elzaurdia
• Rubén González de Pablo
• Vicente González Pérez

Introducción

La motivación del proyecto surge debido a que nos dimos cuenta de que conocíamos a alguna persona con una finca como segunda residencia donde pasaba tiempo ocasionalmente. El problema reside en que normalmente estas parcelas tienen zonas donde crece la hierba y hay que cortarla con cierta frecuencia, lo cual es un esfuerzo considerable haciendo uso de un cortacésped normal.

Para solucionar este problema decidimos diseñar un cortacésped radiocontrol que pueda dirigirse con un mando y a su vez que también funcione de forma autónoma.

Inicialmente, consideramos que podría contar con dos modos de operar: un modo teledirigido con un mando de radiocontrol, y modo “Roomba” que recortaría automáticamente el terreno contando con un parachoques para cambiar su rumbo. Sin embargo, no nos convenció este último modo y concluimos que no resultaba práctico en el contexto que queremos desplegar nuestro producto. Es por ello que decidimos descartarlo en favor de un modo que utilice un GPS y 4 coordenadas de vértices para recorrer una extensión de tierra. Mantuvimos el parachoques de la idea anterior para que la desbrozadora al chocar con un obstáculo grande, sea capaz de ir marcha atrás y cambiar su orientación para esquivar el obstáculo y seguir recortando siguiendo un algoritmo para asegurar que no quede ningún metro cuadrado sin recortar; sin embargo, finalmente por numerosos contratiempos, quedó a medias la implementación del parachoques.

Implementación

Reparto de tareas

Aunque intentamos repartir las tareas de manera que trabajáramos todos igual en cada uno de los aspectos del proyecto, finalmente como hemos tenido problemas con el hardware, Rubén y Fiorella se han encargado más de ese aspecto que Vicente y Frantzes, los cuales han incidido más en el desarrollo del software.

Diseño del producto

Para calcular las dimensiones del chasis se decidió que lo mejor era hacer un diseño 3D de la estructura. De esta manera podíamos ver de qué tamaño hay que cortar y soldar la estructura de acero para que cumpla nuestros requisitos.

En tema de hardware el robot tiene varios componentes principales:

• Tenemos los 4 módulos controladores de motores. Cada rueda tiene un motor independiente controlado de manera independiente. Estos módulos están compuestos de dos partes. Un grupo de relés que controla la dirección de giro de los motores invirtiendo las conexiones, y otro módulo que controla la velocidad del motor haciendo uso de dos mosfet. 
• Para la parte de la cuchilla tenemos el motor de 250w y otra controladora más potente para poder regular la velocidad y el arranque del motor. 
• La alimentación está compuesta de dos baterías Lipo de 12v 4Ah que forman en conjunto un sistema de 24v. 
• Todo el sistema está controlado por un Arduino Mega. 
• Para el control y comunicación tenemos dos módulos de radio. Un receptor para un mando de RC y una antena de radio telemetría de 915mhz que nos permite comunicación bidireccional entre el Arduino y el ordenador. 
• Por último para el modo autónomo tenemos un GPS para saber en tiempo real la ubicación del robot.

Proceso de montaje y de desarrollo de software

Una vez que montamos todo, probamos que cortara con éxito, y así fue. Escribimos el código del modo RC y al probarlo dimos con un problema grave: nuestros controladores de motor se quemaron. 

El modo RC permite ir hacia delante, atrás, girar de golpe y girar a la vez que está acelerando hacia delante o hacia atrás. La velocidad de los motores no es constante, se puede controlar usando los joysticks del mando RC. Para recortar se gira de la rueda correspondiente que sirve de toggle.

Implementamos la comunicación bidireccional entre el Arduino y el PC creando nuestro propio protocolo usando caracteres para que el PC fuera capaz de enviarle al Arduino de manera codificada cómo debe moverse para recorrer la superficie del cuadrilátero. A la vez, el Arduino envía al ordenador constantemente su ubicación actual.

El funcionamiento es el siguiente:

1. Con el mando se cambia el modo de operaciones a “modo PC”. 
2. Una vez establecida la comunicación entre ambos dispositivos, el PC comprueba la orientación y la ubicación actual del Arduino y le manda una orden de moverse siguiendo el algoritmo para recorrer la superficie entera.
3. El Arduino recibe la orden y sigue moviéndose en dicha dirección hasta recibir una nueva orden que puede ser cambiar la dirección, girando en forma de “u” o detenerse porque ya ha terminado de recortar.

Por otro lado hemos añadido funcionalidades para evitar accidentes. 

• Si se desconecta el mando la máquina se detiene.
• Si queda poca batería, contamos con un zumbador que emite pitidos para notificarlo.
• En cualquier momento durante la ejecución del modo de recorte automático se puede cambiar al modo manual usando el mando gracias a las interrupciones.

Poniendo el enfoque en la parte de software del ordenador, desarrollamos una app de escritorio simple utilizando Unity para poder utilizar la API de Google Maps para pinchar sobre el mapa y obtener de una manera intuitiva las coordenadas de los vértices de nuestra superficie a cortar. Utilizar un ordenador nos proporciona la gran ventaja de no estar limitado por el procesador del Arduino, pudiendo ejecutar algoritmos más complejos y manteniendo un canal bidireccional de comunicación en tiempo real. Aprovechamos también para utilizar una antena que se enchufa al puerto USB con un alcance de hasta 2 km, posibilitando el recorte de terrenos más grandes.

Habíamos considerado desarrollar una aplicación móvil que utilizara bluetooth para enviar inicialmente las coordenadas de los vértices de un rectángulo y que ya se encargara el Arduino de recorrerlo por sí solo; no obstante, esto limitaría mucho las posibilidades del software, ya que además de depender de las limitaciones del hardware, una vez que arranca y se desconecta  el bluetooth por su rango muy limitado, no tendríamos manera de conocer qué está sucediendo a partir de ese instante. En cualquier caso, no descartamos la posibilidad de incorporar esta funcionalidad para una versión del producto orientada a recortar patios y parcelas pequeñas.

Costes materiales

Finalmente, el coste total de los materiales ha sido de 517,74 euros aproximadamente, contando el precio de los componentes que poseíamos de antemano, y que no tuvimos que comprar. En la Figura 5 pueden encontrar cada uno de los materiales utilizados con su respectivo precio.

Dificultades encontradas

Hardware

El principal problema que encontramos fue en el sistema de propulsión. Los motores elegidos originalmente no tenían suficiente fuerza para mover el robot a través de la hierba alta. Por eso compramos unos motores con 6 veces más potencia, pero eso acarreó otro problema. Estos nuevos motores tenían un consumo demasiado elevado para los controladores originales, los L298N, lo que hizo explotar varios de ellos. La solución la encontramos en otros reguladores de velocidad más potentes, de hasta 80w de consumo y uno grupos de relés para poder invertir la dirección de giro. Para llegar hasta esta solución hubo que hacer ingeniería inversa.

Por otro lado, las ruedas no tenían suficiente agarre, por lo que en ocasiones se quedaba el robot atascado en terreno complicado o no giraba correctamente. Hemos reemplazado dos de ellas por unas creadas con impresora 3D, pero aunque hemos notado una pequeña mejora, no es muy significativa.
Compramos un módulo GPS que pensábamos que contaba con una brújula para conocer la orientación del cortacésped, sin embargo al no tenerla, hemos tenido que tirar de software, aproximando la orientación utilizando las coordenadas que obtenemos mientras se mueve.

Software

Tuvimos dificultades con la documentación de las funciones exclusivas del Arduino y el proceso de establecer la comunicación entre este y el PC. También con el funcionamiento de los nuevos controladores de los motores que no están diseñados para Arduino, así como el módulo de GPS. 

En cuanto a la comunicación, llevó varias horas lograr establecerla entre el Arduino y el ordenador usando la antena, principalmente porque no nos dimos cuenta de aumentar los baudios.

En general el desarrollo del software fue lento debido a que en varias ocasiones teníamos que esperar a que el que tuviera el hardware lo probara, y como hemos tenido numerosos problemas con el hardware, sumando el tiempo de los envíos de los componentes sustituidos, etc., había periodos de días en los que no podíamos avanzar. En este aspecto podríamos habernos organizado mejor para evitar estos cuellos de botella.

Posibles mejoras

• Como habíamos mencionado previamente, se podría añadir un módulo bluetooth u otro tipo de conectividad, ya que se podría reutilizar casi todo el software incluyendo la del Unity generando un APK y quedaría implementar el proceso de pairing y de recorrido simplificado en el Arduino, sin necesidad de utilizar una antena con ese rango. Este sería un caso de uso diferente, ya que no es tan versátil. También se podría considerar crear una aplicación web.

• Ser capaz de indicar de antemano en el mapa la ubicación de obstáculos para evitarlos sin tener el cortacésped que chocarse contra ellos y hacer un recorrido óptimo usando algún algoritmo más complejo como backtracking.

• Acabar la implementación del parachoques.

• Tener una batería recargable de mucha más capacidad para que el robot tenga una autonomía muy supe

Código Arduino

#include <SoftwareSerial.h>
#include <TinyGPS.h>
#include <math.h>

const int CHNUM = 5; // número de canales de la emisora
int chval[CHNUM]; // valores de los canales
const int PWM_ADVANCE_PIN = 5;
const int PWM_ROTATE_PIN = 3;
const int PWM_BLADE_PIN = 4;
const int PWM_MODE_SWITCH_PIN = 6;
const int PWM_BUZZER_ON = 7;

const int VOLTAGE_PIN = A0;

const int BUZZER_PIN = 27;
const int BLADE_ENABLE_PIN = 28;

const int MIN_VOLTAGE_LIMIT = 4.2;

const int CHANGE_DIR_DELAY = 200;

const int THRESHOLD_FORWARD_MIN = 1500;
const int THRESHOLD_FORWARD_MAX = 1900;
const int THRESHOLD_BACKWARD_MIN = 1020;
const int THRESHOLD_BACKWARD_MAX = 1450;
const int THRESHOLD_TURN_LEFT_MIN = 1225;
const int THRESHOLD_TURN_LEFT_MAX = 1425;
const int THRESHOLD_TURN_RIGHT_MIN = 1475; 
const int THRESHOLD_TURN_RIGHT_MAX = 1700; 
const int THRESHOLD_BLADE_MIN = 1040; 
const int THRESHOLD_BLADE_MAX = 1900; 

const int RELE_FL_MOTOR_PIN = 22;
const int RELE_FR_MOTOR_PIN = 23;
const int RELE_BL_MOTOR_PIN = 24;
const int RELE_BR_MOTOR_PIN = 25;

const int PWM_FL_PIN = 13;
const int PWM_FR_PIN = 12;
const int PWM_BL_PIN = 11;
const int PWM_BR_PIN = 10;

const int PWM_BLADE_SPEED_PIN = 9;

bool batteryCharged = true;
bool bladeMotorSpinning = false;
volatile bool rcMode;

bool l_motor_forward = false;
bool r_motor_forward = false;
bool l_motor_backward = false;
bool r_motor_backward = false;  

TinyGPS gps;

void setup() {
  pinMode(VOLTAGE_PIN, INPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  pinMode(RELE_FL_MOTOR_PIN, OUTPUT);
  pinMode(RELE_FR_MOTOR_PIN, OUTPUT);
  pinMode(RELE_BL_MOTOR_PIN, OUTPUT);
  pinMode(RELE_BR_MOTOR_PIN, OUTPUT);
  pinMode(BLADE_ENABLE_PIN, OUTPUT);
  digitalWrite(BLADE_ENABLE_PIN, LOW);
  pinMode(PWM_BLADE_SPEED_PIN, OUTPUT);

  pinMode(PWM_FL_PIN, OUTPUT);
  pinMode(PWM_FR_PIN, OUTPUT);
  pinMode(PWM_BL_PIN, OUTPUT);
  pinMode(PWM_BR_PIN, OUTPUT);

  Serial.begin(9600);
  Serial1.begin(57600); // Antena
  Serial3.begin(9600); // GPS communication initialization
  //PWM INPUTs
  pinMode(PWM_ADVANCE_PIN, INPUT);
  pinMode(PWM_ROTATE_PIN, INPUT);
  pinMode(PWM_BLADE_PIN, INPUT);
  pinMode(PWM_MODE_SWITCH_PIN, INPUT);
  pinMode(PWM_BUZZER_ON, INPUT);

  startTone();

  attachInterrupt(digitalPinToInterrupt(PWM_MODE_SWITCH_PIN), changeMode, CHANGE);
}

void startTone(){
  soundBuzzer(HIGH);
  delay(500);
  soundBuzzer(LOW);
  delay(300);

  soundBuzzer(HIGH);
  delay(300);
  soundBuzzer(LOW);
  delay(300);

  soundBuzzer(HIGH);
  delay(300);
  soundBuzzer(LOW);
}

void lowBatteryTone(){
  soundBuzzer(HIGH);
  delay(800);
  soundBuzzer(LOW);
  delay(300);

  soundBuzzer(HIGH);
  delay(800);
  soundBuzzer(LOW);
  delay(300);
}



void RCmodeTone(){
  soundBuzzer(HIGH);
  delay(200);
  soundBuzzer(LOW);
}

void automaticModeTone(){
  soundBuzzer(HIGH);
  delay(200);
  soundBuzzer(LOW);
  delay(300);
  soundBuzzer(HIGH);
  delay(200);
  soundBuzzer(LOW);
}

void changeMode(){
  rcMode = !rcMode;
}

void soundBuzzer(bool value){
  digitalWrite(BUZZER_PIN, value);
}

void checkVoltage(){
  int readValue = analogRead(VOLTAGE_PIN);
  float voltage = readValue * 5.0/1023;
 
  if (voltage < MIN_VOLTAGE_LIMIT){
	stopWheels();
	lowBatteryTone ();
	batteryCharged = false;
  } else{
	batteryCharged = true;
  }
}

int to255Range(int value){
  return max(0, min(255, value));
}



float mapToRange(float value) {
  float r =  (float)map(value, 0, 255, 0, 1000) / 1000.0;
  return max(0, min(255, r));
}

void stopWheels() {
  analogWrite(PWM_FL_PIN, 0);
  analogWrite(PWM_FR_PIN, 0);
  analogWrite(PWM_BL_PIN, 0);
  analogWrite(PWM_BR_PIN, 0);
  delay(CHANGE_DIR_DELAY);
  digitalWrite(RELE_FR_MOTOR_PIN, HIGH);
  digitalWrite(RELE_FL_MOTOR_PIN, HIGH);
  digitalWrite(RELE_BL_MOTOR_PIN, HIGH);
  digitalWrite(RELE_BR_MOTOR_PIN, HIGH);
  l_motor_forward = true;
  r_motor_forward = true;
  l_motor_backward = false;
  r_motor_backward = false;
}
// Receives as parameters the forward movement speed, and then two values between 1 and 2 used to decrease the speed of the wheels on one side to make it turn
void accelerateWheels(int forwardSpeed, float turningLeftRate, float turningRightRate) {
  int leftWheelsSpeed = round(forwardSpeed - 255 * (1 - turningLeftRate));
  int rightWheelsSpeed = round(forwardSpeed - 255 * (1 - turningRightRate));
    moveMotors(HIGH, HIGH, leftWheelsSpeed, rightWheelsSpeed);
}
// Receives as parameters the backward movement speed, and then two values between 1 and 2 used to decrease the speed of the wheels on one side to make it turn
void reverseWheels(int backwardSpeed, float turningLeftRate, float turningRightRate) {
  int leftWheelsSpeed = round(backwardSpeed - 255 * (1 - turningLeftRate));
  int rightWheelsSpeed = round(backwardSpeed - 255 * (1 - turningRightRate));
    moveMotors(LOW, LOW, leftWheelsSpeed, rightWheelsSpeed);
}

void turnLeft(int leftSpeed) {
  moveMotors(LOW, HIGH, leftSpeed, leftSpeed);
}

void turnRight(int rightSpeed) {
  moveMotors(HIGH, LOW, rightSpeed, rightSpeed);
}

void moveMotors(bool left, bool right, int analogL, int analogR) {
  Serial.println("R: " + String(analogR) + " L: " + String(analogL));
  if (left && l_motor_forward) {
	analogWrite(PWM_FL_PIN, analogL);
	analogWrite(PWM_BL_PIN, analogL);
  } else if (!left && l_motor_backward) {
	analogWrite(PWM_FL_PIN, analogL);
	analogWrite(PWM_BL_PIN, analogL);
  } else {
	analogWrite(PWM_FL_PIN,0);
	analogWrite(PWM_BL_PIN,0);
	delay(CHANGE_DIR_DELAY);
	digitalWrite(RELE_FL_MOTOR_PIN, left);
	digitalWrite(RELE_BL_MOTOR_PIN, left);
	delay(CHANGE_DIR_DELAY);
	analogWrite(PWM_FL_PIN, analogL);
	analogWrite(PWM_BL_PIN, analogL);

	l_motor_forward = left;
	l_motor_backward = !left;
  }
  if (right && r_motor_forward) {
	analogWrite(PWM_FR_PIN, analogR);
	analogWrite(PWM_BR_PIN, analogR);
  } else if (!right && r_motor_backward) {
	analogWrite(PWM_FR_PIN, analogR);
	analogWrite(PWM_BR_PIN, analogR);
  } else {
	analogWrite(PWM_FR_PIN,0);
	analogWrite(PWM_BR_PIN,0);
	delay(CHANGE_DIR_DELAY);
	digitalWrite(RELE_FR_MOTOR_PIN, right);
	digitalWrite(RELE_BR_MOTOR_PIN, right);
	delay(CHANGE_DIR_DELAY);
	analogWrite(PWM_FR_PIN, analogR);
	analogWrite(PWM_BR_PIN, analogR);
    
	r_motor_forward = right;
	r_motor_backward = !right;
  }
}

bool isTurning(int leftSpeed, int rightSpeed) {
  return leftSpeed > 0 || rightSpeed > 0;
}

bool isAccelerating(int forwardSpeed, int backwardSpeed) {
  return forwardSpeed > 0 || backwardSpeed > 0;
}

void toggleBladeMotor(int pwmValue){
  int bladeSpeed = map(pwmValue, THRESHOLD_BLADE_MIN+100, THRESHOLD_BLADE_MAX, 0, 255);
  bladeSpeed = to255Range(bladeSpeed);
  if(pwmValue<THRESHOLD_BLADE_MIN+100){
	analogWrite(PWM_BLADE_SPEED_PIN, 0);
	digitalWrite(BLADE_ENABLE_PIN, LOW);
  }else{
	analogWrite(PWM_BLADE_SPEED_PIN, bladeSpeed);
	digitalWrite(BLADE_ENABLE_PIN, HIGH);
  }
}

String getCoordinates(){
  bool newData = false;
	for (unsigned long start = millis(); millis() - start < 1000;) {
  	while (Serial3.available())
  	{
    	char c = Serial3.read();
    	if (gps.encode(c)) // Did a new valid sentence come in?
      	newData = true;
  	}
	}
 
  if (newData)
  {
	String coordinates = "";
	float flat, flon, course;
	unsigned long age;
	gps.f_get_position(&flat, &flon, &age);
	course = gps.f_course();
	Serial.print(flat == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flat, 6);
	Serial.print(flon == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flon, 6);
	Serial.print(course == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : course, 6);
	// Warning, if the data received is corrupt the value will be reassigned to 0.0
	if ((flat != 0.0) && (flon != 0.0))
  	coordinates = "G:" + String(flat) + " " + String(flon) + " " + String(gps.cardinal(course));
	return coordinates;
  }

  unsigned long chars;
  unsigned short sentences, failed;
  gps.stats(&chars, &sentences, &failed);
  Serial1.print(" CHARS=");
  Serial1.print(chars);
  Serial1.print(" SENTENCES=");
  Serial1.print(sentences);
  Serial1.print(" CSUM ERR=");
  Serial1.println(failed);
  if (chars == 0)
	Serial1.println("** No characters received from GPS: check wiring **");
  return "";
}

void pcMode(){
  Serial.println("PC");
  /*
  Serial recieves messages from PC. Serial1 sends GPS coordinates to PC.

  READING PROTOCOL:
	Movement:
	S => Stop
	F => Forward
	B => Backward
	R => Turn Right
	L => Turn Left
	C => Toggle Cutting Motor

	Completed route:
	Z => Stop, empty Serial buffer and exit pcMode
  */

  char incomingChar = 'X';

  const int PC_MODE_SPEED =  125;
  getCoordinates();
  // Turn on blade
  toggleBladeMotor(1500);

  while (incomingChar != 'Z' && !rcMode){
    
	if(pulseIn(PWM_MODE_SWITCH_PIN, HIGH) > 1500){
  	return;
	}
	incomingChar = Serial1.read();
	// Check if data has been received
    
	// Switch statement to handle different commands
	switch (incomingChar) {
    	case 'S':
        	stopWheels();
        	break;
    	case 'F':
        	accelerateWheels(PC_MODE_SPEED, 1, 1);
        	break;
    	case 'B':
        	reverseWheels(PC_MODE_SPEED, 1, 1);
        	break;
    	case 'R':
        	turnRight(PC_MODE_SPEED);
        	break;
    	case 'L':
        	turnLeft(PC_MODE_SPEED);
        	break;
    	case 'C':
        	toggleBladeMotor(1500);//hacer esto regulable
        	break;
    	default:
        	Serial.println("No movement char detected");
        	break;
	}

	String coordinates = getCoordinates();
	// Checks if buffer isn't full and sends coordinates and if a collision has been detected
	if (Serial1.availableForWrite() && !rcMode){
  	Serial1.println("Sending coordinates: ");
 	 
  	Serial1.println(coordinates);
	}
  }

  stopWheels();

  // Turn off blade
  toggleBladeMotor(1500);

  Serial.flush();
}

void readRCInput() {
  Serial.println("RC");
  // Read channels
  chval[0] = pulseIn(PWM_ADVANCE_PIN, HIGH);
  chval[1] = pulseIn(PWM_ROTATE_PIN, HIGH);
  chval[2] = pulseIn(PWM_BLADE_PIN, HIGH);
  chval[3] = pulseIn(PWM_BUZZER_ON, HIGH);
  while(chval[0]<500){
	delay(200);
	chval[0] = pulseIn(PWM_ADVANCE_PIN, HIGH);
  }
 
  int forwardSpeed = map(chval[0], THRESHOLD_FORWARD_MIN, THRESHOLD_FORWARD_MAX, 0, 255);
  int backwardSpeed = map(chval[0], THRESHOLD_BACKWARD_MAX, THRESHOLD_BACKWARD_MIN, 0, 255);
  int leftSpeed = map(chval[1], THRESHOLD_TURN_LEFT_MAX, THRESHOLD_TURN_LEFT_MIN, 0, 255);
  int rightSpeed = map(chval[1], THRESHOLD_TURN_RIGHT_MIN, THRESHOLD_TURN_RIGHT_MAX, 0, 255);
 
  forwardSpeed = to255Range(forwardSpeed);
  backwardSpeed = to255Range(backwardSpeed);
  leftSpeed = to255Range(leftSpeed);
  rightSpeed = to255Range(rightSpeed);

  toggleBladeMotor(chval[2]);
 
  if(chval[3]>1000){
	Serial.println("buffer");
	soundBuzzer(HIGH);
  }else{
	soundBuzzer(LOW);
  }
  
  if (isAccelerating(forwardSpeed,backwardSpeed) && !isTurning(leftSpeed,rightSpeed)) {
	// Not turning
	// Get if its moving forward or backward
	if (forwardSpeed > 0){
  	accelerateWheels(forwardSpeed, 1, 1);
	} else {
  	reverseWheels(backwardSpeed, 1, 1);
	}
  } else if (!isAccelerating(forwardSpeed,backwardSpeed) && isTurning(leftSpeed,rightSpeed)) {
	// Not turning
	// Get if its moving forward or backward
	if (rightSpeed > 0){
  	turnRight(rightSpeed);
	} else {
  	turnLeft(leftSpeed);
	}
  } else if(rightSpeed > 10 || leftSpeed > 10){
	// Is accelerating and turning at the same time
	// To make it turn and move forward at the same time, we decelerate the wheels on the side where it intends to turn.
	if (forwardSpeed > 0) {
  	Serial.println("rotate");
    	if (rightSpeed > 0){
    	accelerateWheels(forwardSpeed, 1, mapToRange(rightSpeed));
  	} else {
    	accelerateWheels(forwardSpeed, mapToRange(leftSpeed), 1);
  	}
	} else {
    	if (rightSpeed > 0){
    	reverseWheels(backwardSpeed, 1, mapToRange(rightSpeed));
  	} else {
    	reverseWheels(backwardSpeed, mapToRange(leftSpeed), 1);
  	}
	}
  }else {
  stopWheels();
  }
}

void loop() {
  interrupts();
  checkVoltage();
  if (batteryCharged){
	int changeModeValue= pulseIn(PWM_MODE_SWITCH_PIN, HIGH);
	if(changeModeValue > 1500){
  	if(!rcMode){
    	RCmodeTone();
  	}
  	rcMode = true;
  	readRCInput();

	}else if(changeModeValue>200){
  	if(rcMode){
    	automaticModeTone();
  	}
  	rcMode=false;
  	pcMode();
	}
  }
}

Código C# (Aplicación en Unity)

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.IO.Ports;

public class ArduinoPcComunication : MonoBehaviour
{

	SerialPort portToArduino = new SerialPort("COM6", 57600);
	public bool comunication;
	public RobotMovement robot;
	// Start is called before the first frame update
	void Start()
	{
    	Debug.Log("hola");
   	 
    	foreach (string str in SerialPort.GetPortNames())
    	{
        	Debug.Log(string.Format("Port : {0}", str));
    	}
    	portToArduino.Open();
	}
	void OnDisable()
	{
    	Debug.Log("disable");
    	portToArduino.Close();

	}
	// Update is called once per frame
	void Update()
	{
    	if (comunication && portToArduino.IsOpen)
    	{
        	string data = readSerial();
        	if (data.StartsWith("G:"))
        	{
            	data.Replace("G:", "");
            	string[] parts = data.Split(" ");
            	Vector2 point = new Vector2(float.Parse(parts[0]), float.Parse(parts[1]));
        	}
        	else
        	{
            	Debug.Log(data);
        	}
    	}
  	 
	}
	string readSerial()
	{
    	if (portToArduino.IsOpen)
    	{
        	return portToArduino.ReadLine();
    	}
    	return "";
	}
	public void writeSerial(string data)
	{
    	if (portToArduino.IsOpen && comunication)
    	{
        	portToArduino.WriteLine(data);
    	}
	}
}


//MIT License
//Copyright (c) 2023 DA LAB (https://www.youtube.com/@DA-LAB)
//Permission is hereby granted, free of charge, to any person obtaining a copy
//of this software and associated documentation files (the "Software"), to deal
//in the Software without restriction, including without limitation the rights
//to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//copies of the Software, and to permit persons to whom the Software is
//furnished to do so, subject to the following conditions:
//The above copyright notice and this permission notice shall be included in all
//copies or substantial portions of the Software.
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
//SOFTWARE.

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.UI;
using UnityEngine.Networking;
using System;

public class Map : MonoBehaviour
{
	private string apiKey = "tu clave de maps";
	public float lat = 40.336643f;
	public float lon = -3.876557f;
	public int zoom = 20;
	public enum resolution { low = 1, high = 2 };
	public resolution mapResolution = resolution.low;
	public enum type { roadmap, satellite, gybrid, terrain };
	public type mapType = type.roadmap;
	public Material mapMaterial;
	private string url = "";
	public int mapWidth = 640;
	public int mapHeight = 640;
	private bool mapIsLoading = false; //not used. Can be used to know that the map is loading

	private string apiKeyLast;
	private float latLast = 40.336643f;
	private float lonLast = -3.876557f;
	private int zoomLast = 12;
	private resolution mapResolutionLast = resolution.low;
	private type mapTypeLast = type.roadmap;
	private bool updateMap = true;

	public float latCoordsPerUnit = 3.5e-05f;
	public float lonCoordsPerUnit = 5.3e-05f;


	// Start is called before the first frame update
	void Start()
	{
    	StartCoroutine(GetGoogleMap());
  	 
	}

	// Update is called once per frame
	void Update()
	{
    	if (updateMap && (apiKeyLast != apiKey || !Mathf.Approximately(latLast, lat) || !Mathf.Approximately(lonLast, lon) || zoomLast != zoom || mapResolutionLast != mapResolution || mapTypeLast != mapType))
    	{
        	StartCoroutine(GetGoogleMap());
        	updateMap = false;
    	}
	}
	public Vector2 getCoordOfPoint(Vector2 mapPosition)
	{
   	 
    	float precisionLat = latCoordsPerUnit / zoom;
    	float precisionLon = lonCoordsPerUnit / zoom;
    	float pointLon = lonLast - lonCoordsPerUnit * mapPosition.x;
    	float pointLat = latLast - latCoordsPerUnit * mapPosition.y;
    	Debug.Log(pointLat.ToString().Replace(",", ".") + ", " + pointLon.ToString().Replace(",", "."));
    	return new Vector2(pointLat, pointLon);
   	 
	}
	public Vector2 getPointOfCoords(Vector2 coords)
	{
   	 
    	float precisionLat = latCoordsPerUnit / zoom;
    	float precisionLon = lonCoordsPerUnit / zoom;
    	float pointLon = (coords.x - lonLast) / lonCoordsPerUnit;
    	float pointLat =  (coords.y - latLast) / latCoordsPerUnit;
    	Debug.Log(pointLat.ToString().Replace(",", ".") + ", " + pointLon.ToString().Replace(",", "."));
    	return new Vector2(pointLat, pointLon);

	}


	IEnumerator GetGoogleMap()
	{
    	url = "https://maps.googleapis.com/maps/api/staticmap?center=" + lat + "," + lon + "&zoom=" + zoom + "&size=" + mapWidth + "x" + mapHeight + "&scale=" + mapResolution + "&maptype=" + mapType + "&key=" + apiKey;
    	mapIsLoading = true;
    	UnityWebRequest www = UnityWebRequestTexture.GetTexture(url);
    	yield return www.SendWebRequest();
    	if (www.result != UnityWebRequest.Result.Success)
    	{
        	Debug.Log("WWW ERROR: " + www.error);
    	}
    	else
    	{
        	mapIsLoading = false;
        	mapMaterial.mainTexture = ((DownloadHandlerTexture)www.downloadHandler).texture;

        	apiKeyLast = apiKey;
        	latLast = lat;
        	lonLast = lon;
        	zoomLast = zoom;
        	mapResolutionLast = mapResolution;
        	mapTypeLast = mapType;
        	updateMap = true;
    	}
	}

}


using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class SelectPoints : MonoBehaviour
{
	private Camera camera;
	public Map map;
	public Area area;
	public GameObject prefabPoint;
	public RobotMovement robot;
	// Start is called before the first frame update
	void Start()
	{
    	camera = Camera.main;
	}

	// Update is called once per frame
	void Update()
	{
    	if (Input.GetKeyDown(KeyCode.Mouse0))
    	{
        	selectPointInMap();
    	}
	}
	void selectPointInMap()
	{
    	RaycastHit hit;
    	Ray ray = camera.ScreenPointToRay(Input.mousePosition);

    	if (Physics.Raycast(ray, out hit))
    	{
        	Transform objectHit = hit.transform;
        	Vector2 point = new Vector2(hit.point.x, hit.point.z);
        	Vector2 p = map.getCoordOfPoint(point);
        	map.getPointOfCoords(p);
        	GameObject.Instantiate(prefabPoint, hit.point, Quaternion.identity, this.transform);
        	area.points.Add(p);
    	}
    	if(area.points.Count >= 4)
    	{
        	robot.startMovement();
    	}
	}
}


using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Area : MonoBehaviour
{
	public List<Vector2> points;
	public bool isInArea(Vector2 point)
	{
    	PolygonCollider2D collider = gameObject.AddComponent<PolygonCollider2D>();

    	collider.points = points.ToArray();

    	bool pointInside = collider.OverlapPoint(point);
    	Destroy(collider);

    	return pointInside;
	}
    
}




using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class RobotMovement : MonoBehaviour
{
	public Area area;
	public ArduinoPcComunication comunication;
	private bool rotating;
	private bool moveEnabled = false;
	public void startMovement()
	{
    	moveEnabled = true;
	}
	void actualCoords(Vector2 coords)
	{
    	if (!moveEnabled)
    	{
        	return;
    	}
    	Vector3 worldPos = new Vector3(coords.x, 0, coords.y);
    	this.transform.position = worldPos;
    	if (area.isInArea(coords) && !rotating)
    	{
        	comunication.writeSerial("F");
    	}
    	else
    	{
        	StartCoroutine(rotateAndContinue());
    	}
	}
	IEnumerator rotateAndContinue()
	{
    	rotating = true;
    	comunication.writeSerial("R");
    	yield return new WaitForSeconds(2f);
    	rotating = false;
    	comunication.writeSerial("F");
	}
}