---
header-includes:
- \usepackage{natbib}
- \usepackage{longtable,booktabs}
- \usepackage{amsmath,amssymb}
- \usepackage{lmodern}
- \usepackage{minted}
- \usepackage{parskip}
- \usepackage{url}
- \title{Projecte robot autònom}
- \assignatura{Sistemes automàtics i robotitzats}
- \numpract{Projecte robot autònom}
- \autor{Artur Blaya \& Andreu villaró}
- \usepackage{fvextra}
- \DefineVerbatimEnvironment{Highlighting}{Verbatim}{breaklines,commandchars=\\\{\}}
---

###### tags: `AR`

# Els robots mòbils
Els robots mòbils són màquines programables que es desplacen per l'espai de treball i realitzen tasques automatitzades. Poden ser autònoms o controlats per un operador humà i poden estar equipats amb una varietat d'eines i sensors per fer una rgan varietat de tasques.

Els robots mòbils s'utilitzen comunament en aplicacions industrials, com el transport de materials, el manteniment i la reparació, així com en aplicacions de serveis públics, com ara el control de trànsit i la neteja de carrers. També s'utilitzen en aplicacions militars i d'exploració, com ara la cerca i el rescat i l'exploració d'entorns perillosos.

## Aplicacions industrials
Una aplicació industrial dels robots móviles autónoms, seria el **transport de materials**: Aquests robots s'utilitzen en fàbriques i magatzems per transportar materials, components i productes acabats d'un lloc a l'altre. Poden seguir línies de guia o utilitzar sistemes de navegació per satèl·lit per moure's per l'espai de treball sense la intervenció humana.

{width=50%}

Una altra aplicació industrial dels robots mòbils, és el **manteniment i la reparació**: Aquests robots es poden utilitzar per realitzar tasques de manteniment i reparació en entorns industrials, com ara la inspecció d'equips i la neteja de maquinària. Poden estar equipats amb eines i sensors per fer tasques específiques i poden ser controlats de forma remota per operadors humans.

{width=50%}

## Variables claus per classificar els robots mòbils
Hi ha molts factors que es poden utilitzar per classificar els robots autònoms, però dues variables clau són el grau d'autonomia i el tipus de tasca per a la qual està dissenyat el robot.

**Grau d'autonomia**: els robots autònoms es poden classificar en funció del grau en què són capaços de funcionar de manera independent. En un extrem, hi ha robots totalment autònoms que poden funcionar completament sols, amb poca o cap intervenció humana. Aquests robots solen estar equipats amb sensors avançats, intel·ligència artificial i altres tecnologies que els permeten navegar i realitzar tasques sense intervenció humana. A l'altre extrem, hi ha robots semiautònoms que depenen de l'aportació humana fins a cert punt. Aquests robots poden estar programats per realitzar tasques específiques, però requereixen supervisió humana i poden requerir una entrada manual per funcionar correctament.

**Tipus de tasca**: els robots autònoms també es poden classificar en funció del tipus de tasca que estan dissenyats per realitzar. Alguns tipus de tasques habituals per a les quals s'utilitzen els robots inclouen la fabricació, el transport, l'exploració i la recerca i el rescat. Els robots de fabricació s'utilitzen a les fàbriques per realitzar tasques com ara soldadura, pintura i muntatge, mentre que els robots de transport s'utilitzen per traslladar mercaderies d'un lloc a un altre. Els robots d'exploració s'utilitzen per explorar i mapejar entorns nous o perillosos, com ara la superfície d'altres planetes o el mar profund. Els robots de recerca i rescat s'utilitzen per localitzar i rescatar persones en situacions de desastre, com terratrèmols o tornados.

\clearpage

# Aplicacions realitzades

## Seguiment d'una línia
### Objectiu
Ser capaços de seguir una línia amb el mínim error possible.

### Interacció robot entorn
El robot interactua amb l'entorn mitjançant un sensor de color.

### Funcionament
Per tal de seguir la linea utilitzem un sensor de color, juntament amb un controlador P. En els apartats *Representació del funcionament en màquines d'estats*, i *Descripció del programa, llenguatge utilitzat* s'explica detalladament el funcionament del robot.

## Detecció d’obstacles
### Objectiu
Ser capaç de regular la velocitat quan es detecten obstacles i esquivar-los.

### Interacció robot entorn
El robot interactua amb l'entorn mitjançant un sensor d'ultrasons.

### Funcionament
A mesura que s’aproxima a un obstacle va reduint la velocitat progresivament. Quan s’arriba a una distància mínima, el robot gira fins que no detecta l’obstacle , a menys de la distància mínima. En els apartats *Representació del funcionament en màquines d'estats*, i *Descripció del programa, llenguatge utilitzat* s'explica detalladament el funcionament del robot.

\clearpage
# Informació dels sistemes sensorials utilitzats
## Sensor de color
Els sensors que hem incorporat en el nostre robot han estat el sensor de color per tal de seguir la línia negra i el sensor d'ultrasons per tal de frenar-se al detectar obstacles.

Un sensor de color és un dispositiu que s'utilitza per detectar i mesurar el color d'un objecte. Funciona il·luminant l'objecte i mesurant les longituds d'ona de la llum que es reflecteixen. Aleshores, el sensor compara aquestes longituds d'ona amb un conjunt conegut de colors i determina la coincidència més propera. Els sensors de color s'utilitzen sovint en aplicacions de robòtica per permetre al robot reconèixer i interactuar amb objectes de diferents colors.

Aquí hi ha un exemple de com es podria veure l'output d'un sensor de color en un robot mòbil:

- Si el sensor de color apunta a una superfície blanca, l'output podria ser un valor alt (per exemple, 255 en una escala de 0 a 255).
- Si el sensor de color apunta a una superfície negra, l'output podria ser un valor baix (per exemple, 0 en una escala de 0 a 255).
- Si el sensor de color està apuntant a una superfície de color vermell, l'output podria ser un valor intermedi alt al canal vermell i valors baixos als canals verd i blau (per exemple, 255 al canal vermell, 0 als canals verd i blau).

## Sensor d' ultrasons
Un sensor d'ultrasons, també conegut com a sensor de sonar, és un dispositiu que utilitza ones sonores d'alta freqüència per detectar la presència d'objectes. Funciona emetent un pols de so ultrasònic i mesura el temps que triga el so a recuperar-se. Aleshores, el sensor pot utilitzar aquesta informació per determinar la distància a l'objecte. Els sensors d'ultrasons s'utilitzen sovint en aplicacions de robòtica per permetre al robot navegar i evitar obstacles al seu entorn.

Aquí hi ha un exemple de com es podria veure l'output d'un sensor d'ultrasons en un robot mòbil:

- Si el sensor d'ultrasons apunta a un objecte a una distància d'1 metre, l'output podria ser un valor de 1000 (en una escala de 0 a 1023).
- Si el sensor d'ultrasons apunta a un objecte a una distància de 2 metres, l'output podria ser un valor de 500 (en una escala de 0 a 1023).
- Si el sensor d'ultrasons apunta a un objecte a una distància de 3 metres, l'output podria ser un valor de 333 (en una escala de 0 a 1023).

## Altres sistemes sensorials
Aquests sensors ens han sigut molt útils per a assolir els nostres objectius, però hi ha opcions alternatives que poden ser igual d'efectives.

Per exemple hi ha diversos tipus de sensors de llum que es poden utilitzar per construir un robot de seguiment de línia amb Arduino. Algunes podrien ser:

**Fotoresistència:** una fotoresistència és un tipus de resistència que canvia la seva resistència en funció de la intensitat de la llum a la qual està exposada. Quan la fotoresistència s'exposa a més llum, la seva resistència disminueix, i quan s'exposa a menys llum, la seva resistència augmenta. Podeu utilitzar una fotoresistència per construir un robot de seguiment de línia col·locant-lo a la part inferior del robot, mirant cap avall cap a terra. A mesura que el robot es mou sobre una línia negra, la fotoresistència estarà exposada a menys llum, fent que la seva resistència augmenti. Mitjançant la mesura de la resistència de la fotoresistència, podeu determinar quan el robot està sobre una línia negra i utilitzar aquesta informació per controlar el moviment del robot.

**Sensor òptic reflectant:** un sensor òptic reflectant és un tipus de sensor que utilitza un díode emissor de llum (LED) per fer brillar un feix de llum sobre una superfície i un detector per mesurar la quantitat de llum que es reflecteix. Podeu utilitzar un sensor òptic reflectant per construir un robot de seguiment de línia col·locant-lo a la part inferior del robot, mirant cap avall cap a terra. A mesura que el robot es mou per sobre d'una línia negra, el sensor detectarà menys llum, cosa que indica que el robot està per sobre de la línia. A continuació, podeu utilitzar aquesta informació per controlar el moviment del robot.

També hi ha altres tipus de sensors, per aquestes aplicacions, explicats a l'apartat *Aplicacions reals*.

\clearpage
# Descripció del programa, llenguatge utilitzat
Per a programar el nostre robot hem utilitzat el programa d'arduino IDE escrit amb el llenguatge C++. Hem decidit utilitzar aquest entorn perquè hi ha bastants llibreries per a programar els diferents components utilitzant la placa EVShield, com és la propia llibreria de *EVShield*.

{width=30%}

L'EVShield és una placa de desenvolupament per a la plataforma Arduino que proporciona una manera senzilla d'incorporar sensors i altres components electrònics en un projecte de robòtica. Inclou connectors per a una varietat de sensors, inclosos sensors de color i sensors d'ultrasons, així com connectors per a motors i altres actuadors. Mitjançant l'EVShield, podem construir de manera ràpida i senzilla un robot que pugui detectar i interactuar amb el seu entorn.

## Menu
La funció del menu permet seleccionar la funció que l'usuari vol usar.

char menu() {
  Serial.println("Escull Opció:"); 
  Serial.println("1 - Seguir Línia"); 
  Serial.println("2 - Calibrar"); 
  Serial.println("3 - Trajectòria");
  String option = "";
  while ((option != "1\n") && (option != "2\n") && (option != "3\n")) {
    option = Serial.readString();
  }
  return option[0];
}

## Calibrador
Aquesta funcionalitat serveix per a modificar els valors de les variables blanc i negre utilitzades per al seguidor de línia, ja que la llum en la sala o altres factors podrien alterar les lectures dels sensors i d'aquesta manera actualitzem els valors de lectura tenint en compte les condicions actuals.

Per a realitzar aquesta calibració primer es col·loca el cotxe en el fons blanc i es prem enter. Al fer-ho el sensor fa una lectura i guarda el valor en la variable blanc. Després es repeteix el mateix procés, però posant el cotxe en la línia negra i se sobreescriu la variable negra. I així tindríem ja el robot ben calibrat.

void calibrar() {
  Serial.println("Blanc"); 
  while (Serial.readString() != "\n") {}
  blanc = light1.readRaw();
  Serial.println("Negre"); 
  while (Serial.readString() != "\n") {}
  negre = light1.readRaw() * errorNegre;

  sp = (blanc + negre) / 2;
  offset = negre - sp;  // or sp - blanc
  k = vMax / offset;

  Serial.println("Blanc: " + String(blanc)); 
  Serial.println("Negre: " + String(negre)); 
}

## Seguidor de línia
Per entendre bé el codi, primerament explicarem a que fan referècia les constants definides.

Per entendre bé el codi, primerament explicarem a què fan referència les constants definides.

- vMax: valor màxim de velocitat que rebran els motors.
- llindar: valor de y pel qual es considerarà que estem en una corba.
- vReverse: valor de velocitat que es donarà a la roda interior en una corba. Aquest valor és negatiu, ja que fem girar la roda interior a la corba enrere per donar la corba de forma més precisa.
- factorCurva: valor que es divideix a la velocitat de les rodes en corbes.
- errorNegre: valor que multiplica al valor del sensor a l'estar sobre la línia negra. Serveix perquè el cotxe tendeixi a estar més cap a la zona blanca, ja que d'aquesta manera té més marge d'error.
- blanc: Valor de lectura del sensor de color en posicionar-lo sobre el fons blanc.
- negre: Valor de lectura del sensor de color en posicionar-lo sobre la línia negra.
- Sp: Valor que llegirà el sensor de color quan el cotxe estigui a la seva posició ideal. En desviar-se el cotxe tendirà a tornar-se a col·locar en aquesta posició.
- Offset: Distancia entre les lectures del blanc i del negre (ja multiplicat per errorNegre) amb el valor del Sp.

L'algoritme funciona de la següent forma: Primer es calcula la diferència entre el valor de Sp (valor desitjat) i s (valor llegit pel sensor). El resultat es multiplica per la k de forma que si estem totalment en el blanc y=Vmax i si estem totalment en el negre y=-Vmax. Després actuem en funció d'aquest valor. Si aquest supera el llindar actuarem en el mode de curba, s'adaptarà la velocitat de la roda exterior a la curba, i es dona la velocitat de vReverse a la roda interior.
Si la y no supera el llindar es dona el valor de Vmax a la roda exterior i Vmax-y a la roda interior de forma que com més gran sigui l'error més es reduirà la velocitat de la roda interior i el cotxe tendirà a tornar al la posició de Sp.

// Constants Seguidor Línia
const int vMax = 30;
const int llindar = vMax - vMax/4;
const int vReverse = -vMax/3;
const int factorCurva = 1.2;
const float errorNegre = 0.8;
float blanc = 461;
float negre = 588;
float sp = (blanc + negre) / 2;
float offset = negre - sp;  // or sp - blanc

float k = vMax / offset;

void motors() {
  while (true) {
    int s = light1.readRaw();
    float y = (s - sp) * k;
    if (y > vMax) {
      y = vMax;
    } else if (y < -vMax) {
      y = -vMax;
    }   
    if (y > llindar) {
      evshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, (vReverse/factorCurva));
      evshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, (vMax/factorCurva));
        }
    else if (y > 0) {
      evshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, (vMax - y));
      evshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, (vMax));
    } 
    else if (y < -llindar) {
      Serial.println("full blanc"); 
      evshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, (vMax/factorCurva));
      evshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, (vReverse/factorCurva));
    }
    else if ( y < 0){
      evshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, (vMax));
      evshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, (vMax + y));
    }
  }
}

## Detector d'obstacles
Primerament, definim la distància mínima a la qual el vehicle pot estar d'un objecte, i la distància mínima a la qual el vehicle pot continuar a velocitat normal.

Seguidament, iniciem el sensor infraroig, i calculem constantment la distancia a la que es troben els objectes més propers. Cada vegada que es calcula la distància, també es calcula una variable que serveix per adaptar la velocitat a la distància a la qual es troba l'objecte més proper. Fent que a la distància mínima permesa, la velocitat sigui 0. Un cop calculada la variable ( li hem dit *k* ), multipliquem la velocitat màxima ( velocitat regular ) per aquesta variable, per tal d'adaptar la velocitat a la distància.

A continuació, s'avalua si la distància entre el robot i l'objecte més proper, és:

* Més petita que la distància mínima ( més una tolerància )
* girar per evitar l'obstacle
* Més gran que la distància que fa reduir la velocitat
* continua a màxima velocitat
* Més gran que la distància que fa parar el robot
* adapta la velocitat

// Constants Infraroig
const int minD = 150;
const int maxD = 500;

EVs_EV3Infrared infra01;

void obstacles() {
   while (true) {
    int s = infra01.readProximity();
    float k = (s - minD ) / (float)(maxD - minD);
    int v = k * vMax;

    if ( s < (minD + 50)) {
      evshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, (vReverse));
      evshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, (-vReverse));
    }
    else if (s > maxD) {
      evshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, (vMax));
      evshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, (vMax));
    }
    else if (s > minD) {
      evshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, (v));
      evshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, (v));
    } 
  }
}

void setup() {
  infra01.init( &evshield, SH_BBS2 );
  infra01.setMode(MODE_Infrared_Proximity);
}

\clearpage
# Representació del funcionament en màquines d'estats
Les dues màquines d'estats són d'un sol estat, ja que el valor de la sortida no depèn de l'estat anterior, només del valor de les entrades. És a dir, que el robot actua només en funció de la situació en la qual es troba en aquell moment (no té memòria).

## Seguidor de línia
{width=60%}

## Detector d'obstacles
{width=60%}

\clearpage
# Aplicacions reals
## Seguiment d'una línia
Avuí dia, els robots seguidors de línia s'utilitzen en una varietat d'aplicacions industrials i comercials, com ara:

1. Sistemes de transport automatitzats: Els robots seguidors de línia es poden utilitzar per transportar productes i materials d'una ubicació a una altra de manera automatitzada i sense la intervenció humana.
2. Processament d'aliments i begudes: Els robots seguidors de línia es poden utilitzar per manipular i envasar aliments i begudes de manera ràpida i precisa.
3. Fabricació: Els robots seguidors de línia es poden utilitzar en la producció en massa per realitzar tasques repetitives de manera ràpida i precisa.
4. Sistemes de seguretat: Els robots seguidors de línia es poden utilitzar per vigilar àrees de seguretat i per detectar intrusions no autoritzades.
5. Serveis de neteja: Els robots seguidors de línia es poden utilitzar per a la neteja i manteniment d'edificis i espais públics.
6. Entreteniment: Els robots seguidors de línia també es poden utilitzar en aplicacions d'entreteniment, com en parcs temàtics o en espectacles públics.

### Sensors
Per a les aplicacions esmentades anteriorment, és possible que es necessitin una varietat de sensors, incloent:

1. Sensors de llum: Els robots seguidors de línia sovint utilitzen sensors de llum, com a fotodiodes o cèl·lules fotoelèctriques, per seguir una línia o un camí marcat en el sostre.
2. Sensors de distància: Els robots seguidors de línia sovint utilitzen sensors de distància, com a sensors d'ultrasons o làser, per detectar obstacles i evitar col·lisions.
3. Sensors de moviment: Els robots seguidors de línia sovint utilitzen sensors de moviment, com acceleròmetres o giroscopis, per determinar la seva posició i orientació.
4. Sensors de temperatura: Els robots seguidors de línia poden utilitzar sensors de temperatura per mesurar la temperatura dels productes alimentaris o per protegir-se a si mateixos de temperatures extremes.
5. Sensors d'humitat: Els robots seguidors de línia poden utilitzar sensors d'humitat per mesurar l'humitat dels productes

### Mesures de seguretat
Per assegurar la seguretat en les aplicacions esmentades anteriorment, és important tenir en compte les següents mesures de seguretat:

1. Disseny segur del robot: És important assegurar-se que el robot estigui dissenyat de manera segura i tingui proteccions adequades per evitar col·lisions i lesions a les persones i a altres objectes.
2. Seguretat de l'àrea de treball: És important assegurar-se que l'àrea de treball estigui dissenyada de manera segura i tingui senyalització adequada per indicar la presència de robots i per evitar l'entrada no autoritzada.
3. Protecció dels sensors: És important protegir els sensors del robot per evitar danys i garantir un funcionament precís.
4. Manteniment regular: És important realitzar un manteniment regular del robot per assegurar-se que estigui en bon estat i per detectar i corregir qualsevol problema.
5. Formació del personal: És important proporcionar formació adequada al personal que treballi amb els robots seguidors de línia per garantir la seva seguretat i la dels altres.
6. Sistemes d'emergència: És important tenir sistemes d'emergència en lloc per aturar el robot de manera segura en cas d'emergència.

## Detecció d’obstacles
Avuí dia, la detecció d’obstacles s'utilitza en una varietat d'aplicacions, com ara:

1. Navegació autònoma de vehicles: Els robots de detecció d'obstacles es poden utilitzar per navegar de manera autònoma en vehicles, com a cotxes o drones, per evitar col·lisions amb obstacles.
2. Seguretat industrial: Els robots de detecció d'obstacles es poden utilitzar en indústries per detectar obstacles i evitar col·lisions en màquines o equipament.
3. Neteja automatitzada: Els robots de detecció d'obstacles es poden utilitzar per a la neteja automatitzada d'edificis i espais públics, evitant col·lisions amb objectes o persones.
4. Aplicacions de jardineria: Els robots de detecció d'obstacles es poden utilitzar en aplicacions de jardineria, com ara tallar l'herba o poda d'arbres, evitant col·lisions amb obstacles.
5. Assistència a les persones amb discapacitat: Els robots de detecció d'obstacles es poden utilitzar per ajudar a les persones amb discapacitat a moure's de manera més segura, evitant col·lisions amb obstacles.

### Sensors
Per a les aplicacions esmentades anteriorment, és possible que es necessitin una varietat de sensors, incloent:

1. Sensors de distància: Els robots de detecció d'obstacles sovint utilitzen sensors de distància, com a sensors d'ultrasons o làser, per detectar obstacles i evitar col·lisions.
2. Sensors de moviment: Els robots de detecció d'obstacles sovint utilitzen sensors de moviment, com acceleròmetres o giroscopis, per determinar la seva posició i orientació.
3. Càmeres: Els robots de detecció d'obstacles sovint utilitzen càmeres per detectar obstacles i per realitzar tasques d'inspecció i verificació.
4. Sensors de pressió: Els robots de detecció d'obstacles poden utilitzar sensors de pressió per detectar la pressió exercida per un objecte o persona.
5. Sensors de proximitat: Els robots de detecció d'obstacles poden utilitzar sensors de proximitat per detectar la presència d'objectes o persones a prop.
6. Sensors de temperatura: Els robots de detecció d'obstacles poden utilitzar sensors de temperatura per mesurar la temperatura dels objectes o per protegir-se a si mateixos de temperatures extremes.

### Mesures de seguretat
Per assegurar la seguretat en les aplicacions esmentades anteriorment, és important tenir en compte les següents mesures de seguretat:

1. Disseny segur del robot: És important assegurar-se que el robot estigui dissenyat de manera segura i tingui proteccions adequades per evitar col·lisions i lesions a les persones i a altres objectes.
2. Seguretat de l'àrea de treball: És important assegurar-se que l'àrea de treball estigui dissenyada de manera segura i tingui senyalització adequada per indicar la presència de robots i per evitar l'entrada no autoritzada.
3. Protecció dels sensors: És important protegir els sensors del robot per evitar danys i garantir un funcionament precís.
4. Manteniment regular: És important realitzar un manteniment regular del robot per assegurar-se que estigui en bon estat i per detectar i corregir qualsevol problema.
5. Formació del personal: És important proporcionar formació adequada al personal que treballi amb els robots de detecció d'obstacles per garantir la seva seguretat i la dels altres.
6. Sistemes d'emergència: És important tenir sistemes d'emergència en lloc per aturar el robot de manera segura en cas d'emergència.

\clearpage
# Conclusions
En conclusió, el projecte sobre els robots autònoms ha demostrat ser un èxit en termes de desenvolupament i posada en marxa. Els robots han estat capaços de seguir una línia i detectar obstacles amb una alta precisió, el que demostra l'efectivitat dels sensors utilitzats i el bon disseny del sistema de control. A més, s'ha demostrat que els robots són capaços de detectar diferents tipus d'objectes i reaccionar de manera adequada, el que indica la versatilitat del sistema.

En general, aquest projecte té un gran potencial per a la seva aplicació en diverses indústries i entorns. En el futur es podria seguir realitzant millores i avanços en el disseny i la tecnologia utilitzada per millorar encara més l'efectivitat dels robots.

\clearpage
## Codi
A continuació s'adjunta el codi:

#include <BaseI2CDevice.h>
#include <EVShield.h>
#include <EVShieldAGS.h>
#include <EVShieldI2C.h>
#include <EVShieldUART.h>
#include <EVs_AbsoluteIMU.h>
#include <EVs_AngleSensor.h>
#include <EVs_DISTNx.h>
#include <EVs_EV3Color.h>
#include <EVs_EV3Gyro.h>
#include <EVs_EV3Infrared.h>
#include <EVs_EV3SensorMux.h>
#include <EVs_EV3Touch.h>
#include <EVs_EV3Ultrasonic.h>
#include <EVs_IRThermometer.h>
#include <EVs_LightSensorArray.h>
#include <EVs_LineLeader.h>
#include <EVs_MagicWand.h>
#include <EVs_NumericPad.h>
#include <EVs_NXTCam.h>
#include <EVs_NXTColor.h>
#include <EVs_NXTCurrentMeter.h>
#include <EVs_NXTLight.h>
#include <EVs_NXTMMX.h>
#include <EVs_NXTServo.h>
#include <EVs_NXTThermometer.h>
#include <EVs_NXTTouch.h>
#include <EVs_NXTVoltMeter.h>
#include <EVs_PFMate.h>
#include <EVs_PiLight.h>
#include <EVs_PSPNx.h>
#include <EVs_RTC.h>
#include <EVs_SumoEyes.h>
#include <MsTimer2.h>
#include <SHDefines.h>
#include <SoftI2cMaster.h>
#include <Wire.h>

EVShield evshield(0x34, 0x36);
EVs_NXTLight light1;
EVs_EV3Infrared infra01;

// Constants Seguidor Línia
const int vMax = 30;
const int llindar = vMax - vMax/4;
const int vReverse = -vMax/3;
const int factorCurva = 1.2;
const float errorNegre = 0.8;
float blanc = 461;
float negre = 588;
float sp = (blanc + negre) / 2;
float offset = negre - sp;  // or sp - blanc
float k = vMax / offset;

// Constants Infraroig
const int minD = 150;
const int maxD = 500;

char menu() {
  Serial.println("Escull Opció:"); 
  Serial.println("1 - Seguir Línia"); 
  Serial.println("2 - Calibrar"); 
  Serial.println("3 - Trajectòria");
  String option = "";
  while ((option != "1\n") && (option != "2\n") && (option != "3\n")) {
    option = Serial.readString();
  }
  return option[0];
}

void calibrar() {
  Serial.println("Blanc"); 
  while (Serial.readString() != "\n") {}
  blanc = light1.readRaw();
  Serial.println("Negre"); 
  while (Serial.readString() != "\n") {}
  negre = light1.readRaw() * errorNegre;

  sp = (blanc + negre) / 2;
  offset = negre - sp;  // or sp - blanc
  k = vMax / offset;

  Serial.println("Blanc: " + String(blanc)); 
  Serial.println("Negre: " + String(negre)); 
}

void motors() {
  while (true) {
    int s = light1.readRaw();
    float y = (s - sp) * k;
    if (y > vMax) {
      y = vMax;
    } else if (y < -vMax) {
      y = -vMax;
    }   
    if (y > llindar) {
      evshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, (vReverse/factorCurva));
      evshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, (vMax/factorCurva));
    }
    else if (y > 0) {
      evshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, (vMax - y));
      evshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, (vMax));
    } 
    else if (y < -llindar) {
      Serial.println("full blanc"); 
      evshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, (vMax/factorCurva));
      evshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, (vReverse/factorCurva));
    }
    else if ( y < 0){
      evshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, (vMax));
      evshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, (vMax + y));
    }
  }
}

void obstacles() {
   while (true) {
    int s = infra01.readProximity();
    float k = (s - minD ) / (float)(maxD - minD);
    int v = k * vMax;

    if ( s < (minD + 50)) {
      evshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, (vReverse));
      evshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, (-vReverse));
    }
    else if (s > maxD) {
      evshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, (vMax));
      evshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, (vMax));
    }
    else if (s > minD) {
      evshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, (v));
      evshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, (v));
    } 
  }
}

void setup() {
  evshield.init(SH_HardwareI2C);
  evshield.bank_a.motorReset();
  Serial.begin(115200);  // start serial for output
  light1.init(&evshield, SH_BAS2); 
  light1.setReflected();
  infra01.init( &evshield, SH_BBS2 );
  infra01.setMode(MODE_Infrared_Proximity);
}

void loop() {
  char o = menu();
  if (o == '1') {
    Serial.println("Seguint Línia");
    motors();
  } 
  else if (o == '2') {
    calibrar();
  }
  else if (o == '3') {
    obstacles();
  }
}