Robot CUSTOS
Di Gioele Carboni
ultimo aggiornamento 7 giugno 2015

/*
 (CUSTOS) - Robot expolrer - guardiano autonomo con possibilità 
 di comando da pagina Web attraverso la connessione WiFi di Arduino Yun
 Nella pagina web (cartella www in YUN) vi è una copia,
 versione "minima" di jQuery. La pagina Web deve essere caricata
 nella scheda SD di Yun - Gioele Carboni 2014/15

 La pagina iniziale si trova in http://192.168.240.1/sd/robot/Home.html //se collegato senza access point ip di default
                      oppure in http://arduino.local/sd/robot/Home.html //se collegato con access point
             il video stream in http://arduino.local:8080/stream.html // come sopra per l'indirizzamento

 */
// Librerie

#include <DistanceSRF04.h> //per il sensore centrale mobile
#include "Ultrasonic.h" //Per i sensori Right e Left
#include <Servo.h>
#include <Bridge.h>
#include <YunServer.h>
#include <YunClient.h>

#define rad2grad 57.295779513 //fattore di spostamento in gradi
#define limitAngolo 360      //limite finecorsa servo
#define incremento 1        //variabile di incremento movimento servo (1 = min.)
#define accedecelera 10    //variabile di incremento - decremento velocità motori dx - sx

DistanceSRF04 dist;         // Definisce la visione centrale
Servo neckServo;            // Definisce l'oggetto "Servo" sonar
const int stopDist = 17;    // distanza minima dall'ostacolo sensore centrale (fino a 18 - variare il valore nel caso serva)
int distance;               // variabile distanza corrente dall'ostacolo sensore centrale
int distArray[2];           // array della distanza misurata dal sonar (su servocomando)

float angolo = 0;
float grado  = 0;
int angEnd = 65;    //angolo massimo rotazione servo
int stato = 0;      // variabile di controllo servo
int conta = 0;      // variabile di controllo servo
long NumeroCasuale = 0;  //numero per funzione random

Servo servoA;    //definisce oggetto servo per Webcam
Servo servoB;    // definisce oggetto servo per luci

//Variabili di controllo
//int RobotMode = 0;

//const int corr = 119;       // velocità iniziale motore Right
//const int corl = 103;      // velocità iniziale motore Left
float corr = 121;
float corl = 101.20;

int IRpin = A0;              // pin analogico per sensore sharp (Ir)
int enableRight = 3;        // Pin abilita/disabilita (PWM) motore Right
int enableLeft = 6;         // Pin abilita/disabilita (PWM) motore Left

int directionRight = A2;     // Pin rotazione motore RIGHT - pin analogico usato in modalità OUTPUT
int directionLeft = A1;      // Pin rotazione motore LEFT - pin analogico usato in modalità OUTPUT

Ultrasonic ultrasonicRight(12,13); //Sensore laterale destro Trigger and Echo (in ordine)
Ultrasonic ultrasonicLeft(8,11);  //Sensore laterale sinistro Trigger and Echo (in ordine)

int dR; //Variabile distanza laterale destra
int dL; //Variabile distanza laterale sinistra

// Listen sulla porta di default 5555, il webserver su Yun
// forward di tutte le pagine memorizzate

YunServer server;
String readString; 

void setup() {
  
  Serial.begin(9600); //velocità di comunicazione Bridge
 
  dist.begin(2,5);            // pins per sonar Echo AND Trigger (rispettivamente)
  neckServo.attach(10);       // settaggio servo al pin 10
  neckServo.write(90);      // centraggio servo a 90°
  
  servoA.attach(7);  // settaggio servo al pin 7
  servoA.write(90);  // centraggio servo a 90°
  servoB.attach(9); // settaggio servo al pin 9
  servoB.write(90); // settaggio servo a 90°

  //settaggio pin controllo motori come out
  pinMode(enableRight, OUTPUT);
  digitalWrite(enableRight, LOW); //tiene basso l'uscita - motore off
  
  pinMode(enableLeft, OUTPUT);
  digitalWrite(enableLeft, LOW); //tiene basso l'uscita - motore off
  
  pinMode(directionRight, OUTPUT);
  pinMode(directionLeft, OUTPUT);
  
  // settaggio pin 4 per illuminazione campo visivo webcam
  pinMode(4, OUTPUT);
  digitalWrite(4, LOW);
  
  Bridge.begin(); // Bridge startup
  
  // In attesa di un evento dalla pagina Web
  server.listenOnLocalhost();
  server.begin();

}

void loop() {
  webpagecommand(); //E' stato premuto un pulsante sulla pagina Web?
   distanza(); // legge distanza da eventuali ostacoli - Sensore centrale e laterali
   lowostacle(); // legge distanza da ostacoli a livello superficie - Sensore Sharp
   
  if(distance < stopDist){
    stopBot();
//  suona(); //era previsto un Beep ma la libreria Tone va in conflitto con la servo library
    lookAround(); //il sensore centrale su servo compie una panoramica e misura il campo visivo
                  // cercando una via di fuga favorevole (la distanza misurata più lunga)
                  
if(distArray[0] <= stopDist && distArray[1] <= stopDist){
      reverse();
      delay(750); // in origine il valore è 500
      spinLeft();
      delay(750);
      forward();
    }
    
    else if(distArray[0] <= distArray[1] && distArray[1] > stopDist){ 
      reverse();
      delay(270); // in origine 260
      forwardRight();
      delay(500);
      forward();
            
    }
    else if(distArray[0] > distArray[1] && distArray[0] > stopDist){  
      reverse ();
      delay(270);  // in origine 260
      forwardLeft();
      delay(500);
      forward();
    }
    else if(distArray[0] <= distArray[1] && distArray[1] <= stopDist){   
      reverseLeft();
      delay(500);
      forward();
    }
    else if(distArray[0] > distArray[1] && distArray[1] <= stopDist){ 
      reverseRight();
      delay(500);
      forward();
    }

  }


}  // chiude la funzione loop()

void distanza()
{
  delay(25);
  distance = dist.getDistanceCentimeter();
  //delay(25);
   if (distance < 0){   // correzione in caso di distanza negativa 
    distance = 666;
  }
  delay(80);
  dR = ultrasonicRight.Ranging(CM);
  if (dR < 14.5) {
  sensorLeft();
  dR = 100;
}
  delay(80);
  dL = ultrasonicLeft.Ranging(CM);
 if (dL < 15) {
  sensorRight();
  dL = 100;
}
}

void lowostacle()
{
  float volts = analogRead(IRpin)*0.00322265625;   // valore dal sensore * (5/1024 - ovvero 10 bit) - se alimentato a 3.3.volts cambiare da 5 a 3.3 
  float IRdistance = 65*pow(volts, -1.10);         // (0.0048828125)inserire il valore tra parentesi al posto dell'esistente
  NumeroCasuale = random(1,3);                     //Estrae un numero a caso
  //Serial.println(IRdistance);                    // stampa la distanza rilevata - un valore di 50 equivale a 5 cm (4 cm distanza minima)
  if (IRdistance < 50 && NumeroCasuale == 1) {    //la funzione random rende casuale il comportamento
    IRdistance = 200;   
    reverse();
    delay (200);
    spinLeft();                // in modo da evitare loop infiniti
    delay (250);          //500 valore init      // in quanto vi è un solo sensore Ir centrale (e non sui lati destro - sinistro)
    forward();
  }
  else if (IRdistance < 50 && NumeroCasuale > 1){
  IRdistance = 200;
  reverse();
  delay (200);
  spinRight();
  delay (250);
  forward();
  }
}

void webpagecommand()
{
  // Get dalla pagina web del client - server in attesa
  YunClient client = server.accept();

  // Questa è una richiesta dal computer?
  if (client) {
    // legge il comando
    String command = client.readString();
    command.trim();        //kill spaziovuoto
    Serial.println(command);

    if (command == "accendi") {
       forward();
    }
    else if (command == "spegni") {
       stopBot();
    }
     if (command == "sinistra") {
       forwardLeft2();
    }
    if (command == "destra") { 
      forwardRight2();
    }
     if (command == "indietro") {
       secondreverse();
    }
    if (command == "camerapan") {
       stato = 1 - stato;
       conta = 1;
       stopBot(); // ferma il robot e compie una panoramica 75° a sinistra e destra
       movimentocam();
     //  stato = 0;
    }
     if (command == "accelera") {
       
       corr = (corr + accedecelera);
       corl = (corl + accedecelera);
       forward();
    }
     if (command == "decelera") {
       
       corr = (corr - accedecelera);
       corl = (corl - accedecelera);
       forward();
    }
    
     if (command == "ruotaleft") {
       
      spinRight();
      delay(325); //batteria Low 650
      forward();
    }
    
     if (command == "ruotaright") {
       
      spinLeft();
      delay(355); //batteria Low 710
      forward();
    }
    
     if (command == "lucion") {
       
      digitalWrite(4, HIGH);
    }
    
     if (command == "lucioff") {
       
      digitalWrite(4, LOW);
    }
    
    
    // Chiude le connessioni e tutte le risorse libere.
    client.stop();
   
  
  }
  
 

  delay(50); // Poll ogni 50ms e ripete il Loop
}  

void lookAround() //ci siamo infilati in un angolo? Il sonar misura la distanza e trova la via libera
{
  int pos;     
  
  for(pos = 30; pos <= 150; pos += 1)    // va da 30 gradi a 150 gradi un grado per volta
  {                                      
    neckServo.write(pos);               // muove il servo nella posizione contenuta in 'pos' 
    delay(15);
    
    if(pos == 45){
      delay(500); // in origine 1000
      distArray[0] = dist.getDistanceCentimeter();
      // correzione per distanze negative o inesatte
      if(distArray[0] < 0){distArray[0] = 444;} // in origine 500 (anche sotto)
    }
    if(pos == 135){
      delay(500);
      distArray[1] = dist.getDistanceCentimeter();
      if(distArray[1] < 0){distArray[1] = 444;}
    }
  }
  neckServo.write(90);    // servo posizione centrale
  // testing -- legge distanza accuratamente su monitor seriale (se occorre in fase di progettazione)
  //delay(100);
  //distanza();
  //delay(50);
 // Serial.print("\nLeft: ");  
 // Serial.print(distArray[0]);
 // Serial.print("\nRight: ");  
 // Serial.print(distArray[1]);  
 delay(100); // 200 in origine
}


// Robot - Controlli direzionali
void forward()
{
  analogWrite(enableRight, corr);
  analogWrite(enableLeft, corl);
    
  digitalWrite(directionRight, HIGH);
  digitalWrite(directionLeft, HIGH);
}
void forwardRight()
{
  digitalWrite(enableRight, LOW); //motore Right fermo
  
  analogWrite(enableLeft, 225); //settaggio alla massima velocità
  digitalWrite(directionLeft, HIGH); //motore Left avanti
  
}
void forwardLeft()
{
  analogWrite(enableRight, 225); //settaggio alla massima velocità
  digitalWrite(directionRight, HIGH); //motore Right avanti
  
  digitalWrite(enableLeft, LOW); //motore Left fermo

}
void reverse()
{
  //Motori indietro tutta
  digitalWrite(directionRight, LOW);
  digitalWrite(directionLeft, LOW); 
  analogWrite(enableRight, 255);
  analogWrite(enableLeft, 248);
}
void secondreverse()
{
  digitalWrite(directionRight, LOW);
  digitalWrite(directionLeft, LOW);
  analogWrite(enableRight, 200);
  analogWrite(enableLeft, 194);
  
  delay (200);
  
  digitalWrite(directionRight, HIGH);
  digitalWrite(directionLeft, LOW);
  analogWrite(enableRight, 255);
  analogWrite(enableLeft, 255);
  
  delay (562); //low battery 1125
  
  analogWrite(enableRight, LOW);
  analogWrite(enableLeft, LOW);
  //int RobotMode = 0;
}

void reverseRight()
{
  digitalWrite(enableRight, LOW); //motore Right fermo
  
  analogWrite(enableLeft, 225);
  digitalWrite(directionLeft, LOW); //motore Left indietro massima velocità
  
}
void reverseLeft()
{
  analogWrite(enableRight, 225);
  digitalWrite(directionRight, LOW); //motore Right indietro massima velocità
  
  digitalWrite(enableLeft, LOW); // motore Left fermo
}
void spinLeft()
{
  //entrambi i motori a velocità sostenuta
  analogWrite(enableRight, 225);
  digitalWrite(directionRight, HIGH); //avanti Right

  analogWrite(enableLeft, 225);
  digitalWrite(directionLeft, LOW); //indietro Left
}

void spinRight()
{
  //entrambi i motori a velocità sostenuta
  analogWrite(enableRight, 225);
  digitalWrite(directionRight, LOW); //indietro Right

  analogWrite(enableLeft, 225);
  digitalWrite(directionLeft, HIGH); //avanti Left
}  

void stopBot()  // Spegne entrambi i motori
{
  digitalWrite(enableRight, LOW);
  digitalWrite(enableLeft, LOW);
//  int RobotMode = 0;
}

void PlayBot()
{  
  analogWrite(enableRight, corr);
  analogWrite(enableLeft, corl);
    
  digitalWrite(directionRight, HIGH);
  digitalWrite(directionLeft, HIGH);
//  int RobotMode = 1;
}

void forwardRight2()
{
  digitalWrite(enableRight, LOW); //motore Right fermo
  
  analogWrite(enableLeft, 180);
  digitalWrite(directionLeft, HIGH); //motore Left avanti
  delay(350);
 forward();
}
void forwardLeft2()
{
  digitalWrite(enableLeft, LOW); //motore Left fermo
  analogWrite(enableRight, 197); //settaggio alla massima velocità
  digitalWrite(directionRight, HIGH); //motore Right avanti
  
  delay(350);
 forward();
}

void sensorLeft()
{
  stopBot();
  delay(150);
  
  digitalWrite(directionLeft, LOW); //effettua breve retromarcia poi evita l'ostacolo
  analogWrite(enableLeft, 101.10);
  
  digitalWrite(directionRight, LOW);
  analogWrite(enableRight, 121);
  delay(375); //750
  
  stopBot();
  delay(150);
  
  digitalWrite(directionRight, LOW); //motore Right indietro
  analogWrite(enableRight, 187);
  
  analogWrite(enableLeft, 170);
  digitalWrite(directionLeft, HIGH); //motore Left avanti
  delay(250); // 500
 forward();
}

void sensorRight()
{
  stopBot();
  delay(150);
  
  digitalWrite(directionLeft, LOW); //effettua breve retromarcia poi evita l'ostacolo
  analogWrite(enableLeft, 101.10);
  
  digitalWrite(directionRight, LOW);
  analogWrite(enableRight, 121);
  
  delay(375);
  
  stopBot();
  delay(150);
  
  digitalWrite(directionLeft, LOW); //motore Left indietro
  analogWrite(enableLeft, 170);
  
  analogWrite(enableRight, 187);
  digitalWrite(directionRight, HIGH); //motore Right avanti
  delay(250);
 forward();
}

void slowforward() //se serve un retromarcia più dolce....
{
  
}

void movimentocam() {
    if (conta > 200 && grado == 90){
   conta = 0 - conta;
   servoA.write(90);
   servoB.write(90);
   stato = 0;
//   forward();
    
}
  if ( angolo > limitAngolo ) { angolo = 0; }
 
  grado = (( sin( (angolo/rad2grad) ) * angEnd ) + 90 );
 
  servoA.write( grado );
  servoB.write( grado );
  delay(40);
  angolo = (angolo + incremento );

  if (conta > 0 && grado > 10) {
   conta ++;
   //slowforward();
  movimentocam();
 }      
} 

7/6/2015