Poblema codici con microservo

kingterminetor · May 29, 2019, 9:52am

Ciao a tutti, sto realizzando un piccolo progetto che prevede l'utilizzo di 5 microservo e un sensore a ultrasuoni, ma sto riscontrando un problema nel codice, metto la prima condizione if (se la distanza è tra 40cm e 100cm) e fin qui tutto ok, alla seconda condizione if o if else, ho provato entrambe, i sevo non funzionano ma se provo un print è tutto ok. cosa sto sbagliando?

    #include <Servo.h>
    #define trigpin 5 //set trigpin
    #define echopin 6 //set echopin
    
    const int servo1 = 3; 
    const int servo2 = 8; 
    const int servo3 = 4; 
    const int servo4 = 9;
    const int servo5 = 10;  
    const int trigPin = 5;
    const int echoPin = 6;
    int servoVal; 
    int duration, distance;
    int pos = 0;
    int val = 0;                    // variable for reading the pin status
    
    Servo myservo1; //tira su occhio destro
    Servo myservo2; //tira su il sinistro
    Servo myservo3; //tira su le palpebre
    Servo myservo4; // rotazione destra
    Servo myservo5; // rotazione sinistra
   
    
////     
    void setup() {
      
      myservo1.attach(servo1); 
      myservo2.attach(servo2);  // attaches the servo
      myservo3.attach(servo3);  // attaches the servo
      myservo4.attach(servo4);  // attaches the servo
      myservo5.attach(servo5);  // attaches the servo
      
      pinMode(trigPin, OUTPUT);

      pinMode(echoPin, INPUT);

     
      Serial.begin(9600);
    }
     
    void loop(){

/////// Allineamento ///////

         myservo1.write(90);
         myservo2.write(90);
         myservo3.write(90);
         myservo4.write(90);
         myservo5.write(90);
         

      delay(50);
////
      
      //long duration, distance;
    digitalWrite(trigpin,HIGH);
     _delay_ms(500);
    digitalWrite(trigpin, LOW);
  
  duration=pulseIn(echopin,HIGH); 
  distance=(duration/2)/29.1; 
 
 ////////////////////////////////////////////
 if(distance >40 && distance <80 )
 {
        
         
/////////////// FIRST MOVEMENT /////////
               
         myservo3.write(200);  // OPEN EYES
         delay(1000);          
         myservo4.write(60);  
         myservo5.write(60);  

         delay(2000); 

////////////// SECOND MOVEMENT //////////

         myservo4.write(100);  
         myservo5.write(100);  
         myservo1.write(100);
         myservo2.write(75); // RICORDA CHE è INVERSO
         delay(1000); 

/////////////// THIRD MOVEMENT /////////
          
         
         myservo3.write(90);
         delay(500);
         myservo3.write(200);  
         myservo1.write(80);
         myservo2.write(100);
         delay(2000); 

////////////// FOURTH MOVEMENT //////////

           
         myservo4.write(70);
         myservo5.write(70);  

         delay(1000); 
         
////////////// FIFTH MOVEMENT //////////
         myservo3.write(90);
         delay(500);
         myservo3.write(200);
         delay(500);
         myservo2.write(115);
         myservo1.write(60); 
         myservo3.write(200);  

         delay(1000); 

////////////// SIXTH MOVEMENT //////////  
        
         myservo1.write(90);
         myservo2.write(70);  
         myservo3.write(200);  

         delay(1000); 
       
        }

        if (distance <40)
        {

         delay(500);
         myservo1.write(105);
         myservo2.write(75);  
         myservo3.write(200); 
         
          
          }
          Serial.print("cm"); 
          Serial.println(distance);
          }
         
        
          
       
//        else {
         // Serial.print("cm"); 
          //Serial.println(distance);
        //  }
    
     // }

docdoc · May 29, 2019, 11:12am

Beh intanto, il primo consiglio è sempre: indenta bene. Premi Ctrl-T nell'IDE e fallo fare a lui, poi mantieni l'indentazione. Serve a te per capire meglio le istruzioni che hai inserito (ad esempio se ti manca qualche graffa o l'hai posizionata male...) ed a noi per leggere più facilmente il tuo codice. ti consiglio anche di mettere su una riga separata anche le graffe aperte perché così ti diventa più evidente dove inizia un blocco di codice.

Secondo, non ho capito perché metti nel loop() il riposizionamento al centro di tutti i servo, generalmente questa cosa la si fa all'inizio. Quindi per ora, dopo che avrai indentato il codice, fai una funzione (ad esempio "Allineamento()") mettici dentro quelle istruzioni e richiamala nel setup(). Poi se alla fine di ogni movimento devi riallineare, la richiamerai nuovamente ma solo alla fine del movimento.

Vedi sotto il codice come te l'ho un pochino rivisto.

Infine però spiega meglio cosa intendi. Cos'è "alla seconda condizione if"?
E "provato entrambe", entrambe cosa?
E "i servo non funzionano" in che senso?
E "se provo un print è tutto ok", ma "ok" cosa?

Ecco il codice, partiamo da qui:

#include <Servo.h>

const int trigpin = 5;
const int echopin = 6;
const int servo1 = 3; 
const int servo2 = 8; 
const int servo3 = 4; 
const int servo4 = 9;
const int servo5 = 10;  

int servoVal; 
int duration, distance;
int pos = 0;
int val = 0;                    // variable for reading the pin status

Servo myservo1; //tira su occhio destro
Servo myservo2; //tira su il sinistro
Servo myservo3; //tira su le palpebre
Servo myservo4; // rotazione destra
Servo myservo5; // rotazione sinistra


void setup()
{
  myservo1.attach(servo1); 
  myservo2.attach(servo2);  // attaches the servo
  myservo3.attach(servo3);  // attaches the servo
  myservo4.attach(servo4);  // attaches the servo
  myservo5.attach(servo5);  // attaches the servo
  
  pinMode(trigpin, OUTPUT);
  pinMode(echopin, INPUT);
 
  Serial.begin(9600);

  Allineamento();
}
   
void loop()
{
  delay(50);
  digitalWrite(trigpin,HIGH);
  _delay_ms(500);
  digitalWrite(trigpin, LOW);

  duration=pulseIn(echopin,HIGH); 
  distance=(duration/2)/29.1; 

  if(distance >40 && distance <80 )
  {
    /////////////// FIRST MOVEMENT /////////
    myservo3.write(200);  // OPEN EYES
    delay(1000);          
    myservo4.write(60);  
    myservo5.write(60);  
    delay(2000); 
    ////////////// SECOND MOVEMENT //////////
    myservo4.write(100);  
    myservo5.write(100);  
    myservo1.write(100);
    myservo2.write(75); // RICORDA CHE è INVERSO
    delay(1000); 
    /////////////// THIRD MOVEMENT /////////
    myservo3.write(90);
    delay(500);
    myservo3.write(200);  
    myservo1.write(80);
    myservo2.write(100);
    delay(2000); 
    ////////////// FOURTH MOVEMENT //////////
    myservo4.write(70);
    myservo5.write(70);  
    delay(1000); 
    ////////////// FIFTH MOVEMENT //////////
    myservo3.write(90);
    delay(500);
    myservo3.write(200);
    delay(500);
    myservo2.write(115);
    myservo1.write(60); 
    myservo3.write(200);  
    delay(1000); 
    ////////////// SIXTH MOVEMENT //////////  
    myservo1.write(90);
    myservo2.write(70);  
    myservo3.write(200);  
    delay(1000); 
  }
  if (distance <40)
  {
    delay(500);
    myservo1.write(105);
    myservo2.write(75);  
    myservo3.write(200); 
  }
  Serial.print("cm"); 
  Serial.println(distance);
}

void Allineamento()
{
  /////// Allineamento ///////
  myservo1.write(90);
  myservo2.write(90);
  myservo3.write(90);
  myservo4.write(90);
  myservo5.write(90);
}

EDIT: perché definivi i pin per il sensore sia come #define sia come "const int"? O uno o l'altro!

kingterminetor · May 29, 2019, 3:24pm

docdoc:
Beh intanto, il primo consiglio è sempre: indenta bene. Premi Ctrl-T nell'IDE e fallo fare a lui, poi mantieni l'indentazione. Serve a te per capire meglio le istruzioni che hai inserito (ad esempio se ti manca qualche graffa o l'hai posizionata male...) ed a noi per leggere più facilmente il tuo codice. ti consiglio anche di mettere su una riga separata anche le graffe aperte perché così ti diventa più evidente dove inizia un blocco di codice.

Secondo, non ho capito perché metti nel loop() il riposizionamento al centro di tutti i servo, generalmente questa cosa la si fa all'inizio. Quindi per ora, dopo che avrai indentato il codice, fai una funzione (ad esempio "Allineamento()") mettici dentro quelle istruzioni e richiamala nel setup(). Poi se alla fine di ogni movimento devi riallineare, la richiamerai nuovamente ma solo alla fine del movimento.

Vedi sotto il codice come te l'ho un pochino rivisto.

Infine però spiega meglio cosa intendi. Cos'è "alla seconda condizione if"?
E "provato entrambe", entrambe cosa?
E "i servo non funzionano" in che senso?
E "se provo un print è tutto ok", ma "ok" cosa?

Ecco il codice, partiamo da qui:
#include <Servo.h>

const int trigpin = 5;
const int echopin = 6;
const int servo1 = 3;
const int servo2 = 8;
const int servo3 = 4;
const int servo4 = 9;
const int servo5 = 10;

int servoVal;
int duration, distance;
int pos = 0;
int val = 0; // variable for reading the pin status

Servo myservo1; //tira su occhio destro
Servo myservo2; //tira su il sinistro
Servo myservo3; //tira su le palpebre
Servo myservo4; // rotazione destra
Servo myservo5; // rotazione sinistra

void setup()
{
myservo1.attach(servo1);
myservo2.attach(servo2); // attaches the servo
myservo3.attach(servo3); // attaches the servo
myservo4.attach(servo4); // attaches the servo
myservo5.attach(servo5); // attaches the servo

pinMode(trigpin, OUTPUT);
pinMode(echopin, INPUT);

Serial.begin(9600);

Allineamento();
}

void loop()
{
delay(50);
digitalWrite(trigpin,HIGH);
_delay_ms(500);
digitalWrite(trigpin, LOW);

duration=pulseIn(echopin,HIGH);
distance=(duration/2)/29.1;

if(distance >40 && distance <80 )
{
/////////////// FIRST MOVEMENT /////////
myservo3.write(200); // OPEN EYES
delay(1000);
myservo4.write(60);
myservo5.write(60);
delay(2000);
////////////// SECOND MOVEMENT //////////
myservo4.write(100);
myservo5.write(100);
myservo1.write(100);
myservo2.write(75); // RICORDA CHE è INVERSO
delay(1000);
/////////////// THIRD MOVEMENT /////////
myservo3.write(90);
delay(500);
myservo3.write(200);
myservo1.write(80);
myservo2.write(100);
delay(2000);
////////////// FOURTH MOVEMENT //////////
myservo4.write(70);
myservo5.write(70);
delay(1000);
////////////// FIFTH MOVEMENT //////////
myservo3.write(90);
delay(500);
myservo3.write(200);
delay(500);
myservo2.write(115);
myservo1.write(60);
myservo3.write(200);
delay(1000);
////////////// SIXTH MOVEMENT //////////
myservo1.write(90);
myservo2.write(70);
myservo3.write(200);
delay(1000);
}
if (distance <40)
{
delay(500);
myservo1.write(105);
myservo2.write(75);
myservo3.write(200);
}
Serial.print("cm");
Serial.println(distance);
}

void Allineamento()
{
/////// Allineamento ///////
myservo1.write(90);
myservo2.write(90);
myservo3.write(90);
myservo4.write(90);
myservo5.write(90);
}



EDIT: perché definivi i pin per il sensore sia come #define sia come "const int"? O uno o l'altro!

Ciao Grazie mille per l'aiuto e i consigli!

L'allineamento l'ho dovuto tenere nel void perché ho bisogno che i motori si allineano ogni volta che termini il movimento.

Mi spiego meglio, sto realizzato come Tesi una testa "animatronica" se così possiamo definirla, Stampata in 3d.

I 5 servo muovono gli occhi e le palpebre, adesso quello che vorrei è avere 2 tipi di movimenti in risposta alla distanza, ovvero se la distanza è maggiore di 40cm e minore di 80cm, entra il primo movimento, mentre se la distanza è minore di 40cm entra il secondo movimento.
Però pur mettendo i due if con le varie condizioni, il secondo if quello dove è specificata la condizione <40 non fa muovere i servo e non riesco a capire il perché.

PS. nel codice come tu stesso mi hai fatto notare ci sono alcune imperfezioni, non sono molto esperto in programmazione

docdoc · May 29, 2019, 3:43pm

kingterminetor:
L'allineamento l'ho dovuto tenere nel void perché ho bisogno che i motori si allineano ogni volta che termini il movimento.

Ok, in tal caso usa il mio codice (ma lo hai provato?) e a parte metterlo nel setup(), come detto, metti il richiamo alla Allineamento() anche dopo l'animazione, e anche nel caso in cui la distanza sia superiore al valore che hai stabilito. Il fatto è che nel codice come lo hai impostato (e che io ho mantenuto) se hai una distanza minore di 40 eseguirà continuamente l'animazione minima finché l'ostacolo è a meno di 40 cm...
Per migliorare il controllo, metti la print della distanza non appena l'hai misurata.

Prova quindi QUESTO codice, e fammi sapere come si comporta.

#include <Servo.h>

const int trigpin = 5;
const int echopin = 6;
const int servo1 = 3; 
const int servo2 = 8; 
const int servo3 = 4; 
const int servo4 = 9;
const int servo5 = 10;  

int servoVal; 
int duration, distance;
int pos = 0;
int val = 0;                    // variable for reading the pin status

Servo myservo1; //tira su occhio destro
Servo myservo2; //tira su il sinistro
Servo myservo3; //tira su le palpebre
Servo myservo4; // rotazione destra
Servo myservo5; // rotazione sinistra


void setup()
{
  myservo1.attach(servo1); 
  myservo2.attach(servo2);  // attaches the servo
  myservo3.attach(servo3);  // attaches the servo
  myservo4.attach(servo4);  // attaches the servo
  myservo5.attach(servo5);  // attaches the servo
  
  pinMode(trigpin, OUTPUT);
  pinMode(echopin, INPUT);
 
  Serial.begin(9600);

  Allineamento();
}
   
void loop()
{
  delay(50);
  digitalWrite(trigpin,HIGH);
  _delay_ms(500);
  digitalWrite(trigpin, LOW);

  duration=pulseIn(echopin,HIGH); 
  distance=(duration/2)/29.1; 
  Serial.print(distance);
  Serial.println(" cm");

  if(distance >80 )
    Allineamento();

  if(distance >40 && distance <80 )
  {
    /////////////// FIRST MOVEMENT /////////
    myservo3.write(200);  // OPEN EYES
    delay(1000);          
    myservo4.write(60);  
    myservo5.write(60);  
    delay(2000); 
    ////////////// SECOND MOVEMENT //////////
    myservo4.write(100);  
    myservo5.write(100);  
    myservo1.write(100);
    myservo2.write(75); // RICORDA CHE è INVERSO
    delay(1000); 
    /////////////// THIRD MOVEMENT /////////
    myservo3.write(90);
    delay(500);
    myservo3.write(200);  
    myservo1.write(80);
    myservo2.write(100);
    delay(2000); 
    ////////////// FOURTH MOVEMENT //////////
    myservo4.write(70);
    myservo5.write(70);  
    delay(1000); 
    ////////////// FIFTH MOVEMENT //////////
    myservo3.write(90);
    delay(500);
    myservo3.write(200);
    delay(500);
    myservo2.write(115);
    myservo1.write(60); 
    myservo3.write(200);  
    delay(1000); 
    ////////////// SIXTH MOVEMENT //////////  
    myservo1.write(90);
    myservo2.write(70);  
    myservo3.write(200);  
    delay(1000);
    
    Allineamento();
    
  }
  if (distance <40)
  {
    delay(500);
    myservo1.write(105);
    myservo2.write(75);  
    myservo3.write(200); 
  }
}

void Allineamento()
{
  /////// Allineamento ///////
  myservo1.write(90);
  myservo2.write(90);
  myservo3.write(90);
  myservo4.write(90);
  myservo5.write(90);
}

Però pur mettendo i due if con le varie condizioni, il secondo if quello dove è specificata la condizione <40 non fa muovere i servo e non riesco a capire il perché.

Ripeto, ma tu hai usato il MIO codice? Perché nel tuo, con le indentazioni "strane", ho l'impressione che ci fosse qualche problemino di graffe chiuse...
Prova il mio e fammi sapere.

PS: non quotare MAI tutto il testo al quale stai rispondendo, ma seleziona solo le parti rilevanti cper capire a cosa stai rispondendo (o anche nulla, se non ci sono cose particolari).
Quindi per favore modifica in tal senso il tuo ultimo post (puoi anche rimuovere tutto il queote, si capisce lo stesso).

kingterminetor · May 29, 2019, 4:12pm

Si, avevo usato il tuo codice prima, con quest'ultimo che hai postato funziona, i motori si muovono ma succede una cosa molto strana, completato il movimento vanno in loop e il terminale restituisce una letteratura di 25/26cm costante pur non essendo quella la reale distanza. immagino si questo che li mandi in loop.

docdoc · May 30, 2019, 9:32am

kingterminetor:
completato il movimento vanno in loop

Certo, è quello che ti avevo detto. Dato che il movimento si trova nel loop ed è condizionato dalla lettura della distanza, se l'ostacolo si trova alla distanza prevista (40-80 cm) e ci resta, alla fine del movimento cicla il loop e torna nuovamente a fare il movimento.
Quindi dovresti definire e descriverci meglio il tipo di reazione che vorresti avere. Per dire, un oggetto entra nel range 40-80, il robot fa UNA VOLTA il movimento, poi resta fermo finché l'oggetto non esce da quel range.
Stessa cosa per il range inferiore <40, cosa deve fare esattamente? Come è ora ripete all'infinito l'ultimo movimento (quello da 3 stepper per intenderci).

Descrivi meglio tutto il comportamento previsto, ed in base a questo si può modificare il programma, altrimenti stiamo qui all'infinito pure noi a parlare.

e il terminale restituisce una letteratura di 25/26cm costante pur non essendo quella la reale distanza. immagino si questo che li mandi in loop.

Non è questo, ma comunque un sensore funzionante non può darti una distanza non reale.

Se per caso stai usando un sensore HC-SR04 ti avviso che io ho sempre avuto GROSSISSIMI problemi con quelli, ne ho provati svariati e praticamente TUTTI si "bloccano" fino a che non li resetti togliendo alimentazione (ne ho parlato varie volte anche qui nel forum, se cerchi SR04).
Per questo motivo avevo anche creato una piccola libreria (la trovi QUI) che se impostata per gestire un SR04, riconoscendo quel tipo di situazione provava a recuperarla, e ci riusciva quasi sempre.

Ma resta il fatto che SR04 non sono affidabili, se è questa anche la tua situazione, fai prima a prendere gli SRF05 che costano qualche centesimo in più ma non mi hanno MAI dato quei problemi!

A parte ciò, non mi convince neanche l'uso che fai del sensore ossia come rilevi la distanza. Tu fai:

  delay(50);
  digitalWrite(trigpin,HIGH);
  _delay_ms(500);
  digitalWrite(trigpin, LOW);

Primo, perché usi la "_delay_ms()"?
Secondo, tu così mandi un impulso da 500 ms (mezzo secondo)?? :o
Qui si parla di microsecondi, non di millisecondi... Il "normale" uso del sensore è questo:

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

Ossia emetti un impulso da (circa) 10 microsecondi, e poi leggi in quanto tempo il trigger echo va alto.

In alternativa, prova ad usare la mia libreria e risolvi questi dubbi..

Topic		Replies	Views
Sensore ultrasuoni su pin 14-15 Software	7	417	June 5, 2022
problema codice controllo servo Generale	5	1274	May 7, 2021
Problema codice servo Software	5	1390	May 7, 2021
Controllo 3 servomotori Software	17	632	October 22, 2022
Servo su Arduino Mega micro movimenti continui [RISOLTO] Software	9	988	May 7, 2021

Poblema codici con microservo

Related Topics