//Sensores

int trig1 = 35;

int echo1 = 34; //S1
int SA[2] = {trig1, echo1};

int trig2 = 32;

int echo2 = 33; //S2
int SB[2] = {trig2, echo2};

int trig3 = 31;

int echo3 = 30; //S3
int SC[2] = {trig3, echo3};

float dist1 = 0;
float dist2 = 0;
float dist3 = 0;
float distp = 0;
float dev1 = 0;
float dev2 = 0;
float dev3 = 0;
float acc = 0;
float pdis = 0;
int cont = 0;
float t = 0;
float duracion = 0;
float distancia = 0;

//Bluetooth
String tr;

//Motores
int in1 = 41;
int in2 = 40; //M1
int ena = 2;
int MA[3] = {in1, in2, ena};

int in3 = 38;

int in4 = 39; //M2
int enb = 3;
int MB[3] = {in3, in4, enb};

int in5 = 36;

int in6 = 37; //M3
int enc = 4;

int ele_1=8;
int ele_2=9;
int ele_pwm=10;
int ele_pw=50; //velocidad del motor, minim 0 y maximo 255

int MC[3] = {in5, in6, enc};

int l=0.5;
int l2=0.5;
int p=50;
int w=500;
float maxsen=0;
float maxsen2=0;
float minsen=0;
float minsen2=0;
float q1=0;
float q2=0;
float q3=0;
float q4=0;
float q5=0;
float q6=0;
int power=200;
int power2=200;
int power3=200;

void setup() {
pinMode(ele_1, OUTPUT);
pinMode(ele_2, INPUT);
pinMode(ele_pwm, OUTPUT);

pinMode(trig1, OUTPUT);
pinMode(echo1, INPUT);
pinMode(trig2, OUTPUT);
pinMode(echo2, INPUT);
pinMode(trig3, OUTPUT);
pinMode(echo3, INPUT);
Serial.begin(19200);
Serial2.begin(9600);
Serial.println("Enter AT Comands:");
pinMode(in1,OUTPUT);
pinMode(in2,OUTPUT);
pinMode(ena,OUTPUT);
pinMode(in3,OUTPUT);
pinMode(in4,OUTPUT);
pinMode(enb,OUTPUT);
pinMode(in5,OUTPUT);
pinMode(in6,OUTPUT);
pinMode(enc,OUTPUT);
pinMode(53,OUTPUT);
pinMode(52,OUTPUT);
pinMode(51,OUTPUT);
pinMode(50,OUTPUT);
pinMode(49,OUTPUT);
pinMode(48,OUTPUT);
pinMode(47,OUTPUT);
pinMode(46,OUTPUT);
pinMode(45,OUTPUT);
pinMode(44,OUTPUT);
pinMode(43,OUTPUT);
 pinMode(42,OUTPUT);
}

void loop() {
// dev1=dist1-distp;
// dev2=dist2-distp;
// dev3=dist3-distp ;
// Serial.print(dev1,DEC);
// Serial.print(" dev ");
// Serial.print(dev2,DEC);
// Serial.print(" dev ");
// Serial.println(dev3,DEC);
// //delay(5000);}

if (Serial2.available()) {
tr = GetLine();
Serial.println(tr);

if (tr == "Up") {
Serial.println("Power");
Serial2.println("Hay un problema");
}
else if (tr == "UMA"){
mup(MA,power);
delay(w);
moff(MA);
}
else if (tr == "UMB"){
mup(MB,power);
delay(w);
moff(MB);
}
else if (tr == "UMC"){
mup(MC,power);
delay(w);
moff(MC);
}
else if (tr == "DMA"){
mdw(MA,power);
delay(w);
moff(MA);
}
else if (tr == "DMB"){
mdw(MB,power);
delay(w);
moff(MB);
}
else if (tr == "DMC"){
mdw(MC,power);
delay(w);
moff(MC);
}
 else if (tr == "AMU"){
mup(MA,power2);
mup(MB,power2);
mup(MC,power2);
delay(w);
moff(MA);
moff(MB);
moff(MC);
}
else if (tr == "AMD"){
mdw(MA,power2);
mdw(MB,power2);
mdw(MC,power2);
delay(w);
moff(MA);
moff(MB);
moff(MC);
}
else if (tr == "OFF"){
moff(MA);
moff(MB);
moff(MC);
}
else if (tr == "UU"){
maxsen = senup();
if(maxsen>25){
Serial2.println("No puedes subir más, riesgo
estructural");
}
else if(maxsen<=25 && maxsen>12){
do{
mup(MA,power3);
mup(MB,power3);
mup(MC,power3);
delay(1000);
moff(MA);
moff(MB);
moff(MC);
equup();
maxsen2 = senup();
}while(maxsen2<25);}
}
else if (tr == "DD"){
minsen = sendw();
if(minsen<12){
Serial2.println("No puedes bajar más, riesgo
estructural");
}
else if(minsen<25 && minsen>=12){
do{
mdw(MA,150);
mdw(MB,150);
 mdw(MC,150);
delay(2000);
moff(MA);
moff(MB);
moff(MC);
equdw();
minsen2 = sendw();
}while(minsen2>12);}
}
else if (tr == "SEN"){
sen();
}
else if (tr == "LED"){
Serial.println("hola mundo");
digitalWrite(53,HIGH);
digitalWrite(52,HIGH);
digitalWrite(51,HIGH);
digitalWrite(50,HIGH);
digitalWrite(49,HIGH);
digitalWrite(48,HIGH);
digitalWrite(47,HIGH);
digitalWrite(46,HIGH);
digitalWrite(45,HIGH);
digitalWrite(44,HIGH);
digitalWrite(43,HIGH);
digitalWrite(42,HIGH);
}
else if (tr == "LOFF"){
digitalWrite(53,HIGH);
digitalWrite(52,LOW);
digitalWrite(51,LOW);
digitalWrite(50,LOW);
digitalWrite(49,LOW);
digitalWrite(48,LOW);
digitalWrite(47,LOW);
digitalWrite(46,LOW);
digitalWrite(45,LOW);
digitalWrite(44,LOW);
digitalWrite(43,LOW);
digitalWrite(42,LOW);
}

else if (tr=="EON"){
digitalWrite(ele_1, HIGH);
digitalWrite(ele_2, LOW);
analogWrite(ele_pwm, ele_pw);
delay(100);
digitalWrite(ele_1, LOW);
digitalWrite(ele_2, LOW);
analogWrite(ele_pwm, 0);
}
else if (tr=="EOFF"){
digitalWrite(ele_1, LOW);
digitalWrite(ele_2, HIGH);
analogWrite(ele_pwm, ele_pw);
delay(100);
digitalWrite(ele_1, LOW);
digitalWrite(ele_2, LOW);
analogWrite(ele_pwm, 0);
}

else if (tr == "DANCE"){

for(int i=0; i<=10; i++){
digitalWrite(53,HIGH);
delay(p);
digitalWrite(53,LOW);
digitalWrite(52,HIGH);
delay(p);
digitalWrite(52,LOW);
digitalWrite(51,HIGH);
delay(p);
digitalWrite(51,LOW);
digitalWrite(50,HIGH);
delay(p);
digitalWrite(50,LOW);
digitalWrite(49,HIGH);
delay(p);
digitalWrite(49,LOW);
digitalWrite(48,HIGH);
delay(p);
digitalWrite(48,LOW);
digitalWrite(47,HIGH);
delay(p);
digitalWrite(47,LOW);
digitalWrite(46,HIGH);
delay(p);
digitalWrite(46,LOW);
digitalWrite(45,HIGH);
delay(p);
digitalWrite(45,LOW);
digitalWrite(44,HIGH);
delay(p);
digitalWrite(44,LOW);
digitalWrite(43,HIGH);
delay(p);
digitalWrite(43,LOW);
digitalWrite(42,HIGH);
delay(p);
digitalWrite(42,LOW);
 }
}
}
}
String GetLine()
{ String S = "" ;
if (Serial2.available())
{ char c = Serial2.read(); ;
while ( c != '\n') //Hasta que el caracter sea
intro
{ S = S + c ;
delay(25) ;
c = Serial2.read();
}
return (S) ;
}
}
float dist(int trig, int echo) {
digitalWrite(trig, LOW);
delayMicroseconds(4);
digitalWrite(trig, HIGH);
delayMicroseconds(10);
digitalWrite(trig, LOW);
duracion = pulseIn(echo, HIGH);
distancia = duracion / 58.4;
return distancia;
}
float pdist(int trig, int echo) {
do {
t = dist(trig, echo);
if (t < 150 && t > 2) {
acc += t;
cont++;
//Serial.print(t,DEC);
//Serial.print(" ");
//Serial.println(cont,DEC);
delayMicroseconds(10000);
}
} while (cont < 50);
cont = 0;
pdis = acc / 50;
acc = 0;
return pdis;
}
void mup(int M[3], int pw) {
digitalWrite(M[0], HIGH);
digitalWrite(M[1], LOW);
analogWrite(M[2], pw);
}
void mdw(int M[3], int pw) {
digitalWrite(M[0], LOW);
digitalWrite(M[1], HIGH);
 analogWrite(M[2], pw);
}
void moff(int M[3]) {
digitalWrite(M[2], LOW);
}
void sen(){
dist1= pdist(trig1,echo1);
delay(10);
dist2= pdist(trig2,echo2);
delay(10);
dist3= pdist(trig3,echo3);
delay(10);
Serial2.print(dist1,DEC);
Serial2.print(" - ");
Serial2.print(dist2,DEC);
Serial2.print(" - ");
Serial2.println(dist3,DEC);
distp=(dist1+dist2+dist3)/3;
Serial2.println(distp,DEC);}
float sendw(){
dist1= pdist(trig1,echo1);
delay(10);
dist2= pdist(trig2,echo2);
delay(10);
dist3= pdist(trig3,echo3);
delay(10);
Serial.print(dist1,DEC);
Serial.print(" - ");
Serial.print(dist2,DEC);
Serial.print(" - ");
Serial.println(dist3,DEC);
float r = min(dist1,dist2);
Serial.println(min(r,dist3),DEC);
return min(r,dist3);
}
float senup(){
dist1= pdist(trig1,echo1);
delay(10);
dist2= pdist(trig2,echo2);
delay(10);
dist3= pdist(trig3,echo3);
delay(10);
Serial.print(dist1,DEC);
Serial.print(" - ");
Serial.print(dist2,DEC);
Serial.print(" - ");
Serial.println(dist3,DEC);
float r = max(dist1,dist2);
Serial.println(max(r,dist3),DEC);
return max(r,dist3);
}
void equup(){
dist1= pdist(trig1,echo1);
dist2= pdist(trig2,echo2);
dist3= pdist(trig3,echo3);
Serial.print(dist1,DEC);
Serial.print(" - ");
Serial.print(dist2,DEC);
Serial.print(" - ");
Serial.println(dist3,DEC);
distp=(dist1+dist2+dist3)/3;
Serial.println(distp,DEC);
if(dist1>distp+l){
do{
mdw(MA,power3);
delay(500);
moff(MA);
q1=pdist(trig1,echo1);
Serial.print(q1,DEC);
}
while(q1>distp+l);}
else if (dist1<distp-l){
do{
mup(MA,power3);
delay(500);
moff(MA);
q2=pdist(trig1,echo1);
Serial.print(q2,DEC);
}
while(q2<distp-l);}
else if(dist2>distp+l){
do{
mdw(MB,power3);
delay(500);
moff(MB);
q3=pdist(trig2,echo2);
Serial.print(q3,DEC);
}
while(q3>distp+l);}
else if (dist2<distp-l){
do{
mup(MB,power3);
delay(500);
moff(MB);
q4=pdist(trig2,echo2);
Serial.print(q4,DEC);
}
while(q4<distp-l);}
else if(dist3>distp+l){
do{
mdw(MC,power3);
delay(500);
moff(MC);
q5=pdist(trig3,echo3);
 Serial.print(q5,DEC);
}
while(q5>distp+l);}
else if (dist3<distp-l){
do{
mup(MC,power3);
delay(500);
moff(MC);
q6=pdist(trig3,echo3);
Serial.print(q6,DEC);
}
while(q6<distp-l);}
}
void equdw(){
dist1= pdist(trig1,echo1);
dist2= pdist(trig2,echo2);
dist3= pdist(trig3,echo3);
Serial.print(dist1,DEC);
Serial.print(" - ");
Serial.print(dist2,DEC);
Serial.print(" - ");
Serial.println(dist3,DEC);
distp=(dist1+dist2+dist3)/3;
Serial.println(distp,DEC);
if(dist1>distp+l2){
do{
mdw(MA,power3);
delay(500);
moff(MA);
q1=pdist(trig1,echo1);
Serial.print(q1,DEC);
}
while(q1>distp+l2);}
else if (dist1<distp-l2){
do{
mup(MA,power3);
delay(500);
moff(MA);
q2=pdist(trig1,echo1);
Serial.print(q2,DEC);
}
while(q2<distp-l2);}
else if(dist2>distp+l2){
do{
mdw(MB,power3);
delay(500);
moff(MB);
q3=pdist(trig2,echo2);
Serial.print(q3,DEC);
}
while(q3>distp+l2);}
else if (dist2<distp-l2){
 do{
mup(MB,power3);
delay(500);
moff(MB);
q4=pdist(trig2,echo2);
Serial.print(q4,DEC);
}
while(q4<distp-l2);}
else if(dist3>distp+l2){
do{
mdw(MC,power3);
delay(500);
moff(MC);
q5=pdist(trig3,echo3);
Serial.print(q5,DEC);
}
while(q5>distp+l2);}
else if (dist3<distp-l2){
do{
mup(MC,power3);
delay(500);
moff(MC);
q6=pdist(trig3,echo3);
Serial.print(q6,DEC);
}
while(q6<distp-l2);}
}