Sei servo con movimento armonico arduino

Il progetto mano biomeccanica procede su due fronti: lato software sto lavorando per ottenere sei servo con movimento armonico, cioè tutti i servo devono muoversi ciascuno indipendentemente dagli altri ma con un movimento armonico proprio.

Lato meccanico Federico si sta occupando del montaggio della struttura e dell’automazione con cavi elastici, i tendini, e le minuterie necessarie al completo funzionamento.

Il collegamento dei sei servo con movimento armonico

la parte elettrica del collegamento dei sei servo con movimento armonico è molto semplice, e vale anche se vuoi collegare solo sei servo da controllare in modo non armonico 🙂

Ricorda solo che un servo può essere controllato facilmente con Arduino se utilizzi i pin PWM, che per Arduino uno sono solo sei:

nella figura hai notato che ci sono solo 7 fili che uniscono Arduino alla breadboard e che ho utilizzato un circuito su basetta millefori da me realizzato per collegare i servo.

Lo schema è il seguente:

il terminale segnal di ogni servo è connesso ad uno dei pin PWM di arduino, mentre il positivo ed il negativo di ciascun servo è collegato direttamente alla batteria, mentre il polo negativo del circuito è comune anche ad Arduino.

La difficoltà dello sketch sei servo con movimento armonico

Se hai provato già a lavorare con i servo ti sarai reso conto che quando scrivi uno sketch per un solo servo tutto funziona alla perfezione ma appena introduci il secondo servo il grado di difficoltà cresce, ci sono alcuni concetti che ignorati portano inevitabilmente a non far funzionare lo sketch.

In questo sketch ti spiego proprio le difficoltà che ho riscontrato e che potresti incontrare aggiungendo sei servo al tuo progetto.

Prima di entrare nello specifico dello sketch è necessario chiarire il concetto di istanza di una classe o libreria arduino:

quando includi una libreria, come ad esempio la Servo.h usata in questo sketch, al suo interno è contenuta una classe servo che dispone di alcuni metodi ciascuno dei quali ha una funzione specifica.

Per utilizzare la classe Servo contenuta nel file Servo.h è necessario che tu la istanzi, ossia che tu dichiari di volerla utilizzare nello sketch e la assegni ad una variabile a tua scelta.

Dalla linea successiva dello sketch quella variabile è la tua istanza della classe Servo, per cui tutte le volte che vorrai utilizzare un metodo della classe, o oggetto, Servo dovrai far riferimento a quest’ultima.

Un esempio chiarisce più di mille parole:

/**********************************************************
 * Servo con movimento armonico e Arduino
 *
 * Data creazione 27 marzo 2013
 * Ultima modifica 07 aprile 2013
 *
 * autore: Mauro Alfieri
 * web:    mauroalfieri.it
 * tw:     @mauroalfieri
 *
/**********************************************************/

#include <Servo.h>

#define pinServoA 3
#define pinServoB 5
#define pinServoC 6
#define pinServoD 9
#define pinServoE 10
#define pinServoF 11

#define rad2grad 57.295779513
#define incremento 8
#define limitAngolo 360
#define differenza 30

float angoloA = 0;
float angoloB = 0;
float angoloC = 0;
float angoloD = 0;
float angoloE = 0;
float angoloF = 0;

float gradoA  = 0;
float gradoB  = 0;
float gradoC  = 0;
float gradoD  = 0;
float gradoE  = 0;
float gradoF  = 0;

boolean firstLoop=true;

Servo servoA;
Servo servoB;
Servo servoC;
Servo servoD;
Servo servoE;
Servo servoF;

void setup() {
  Serial.begin( 9600 );
  Serial.println( " ------------------ Avvio ------------------------" );

  servoA.attach( pinServoA );
  servoA.write( gradoA );

  servoB.attach( pinServoB );
  servoB.write( gradoB );

  servoC.attach( pinServoC );
  servoC.write( gradoC );

  servoD.attach( pinServoD );
  servoD.write( gradoD );

  servoE.attach( pinServoE );
  servoE.write( gradoE );

  servoF.attach( pinServoF );
  servoF.write( gradoF );
}

void loop() {

  if ( angoloA <= differenza &&  firstLoop == true ) { angoloB = angoloC = angoloD = angoloE = angoloF = 0; }
  if ( angoloB <= differenza &&  firstLoop == true ) { angoloC = angoloD = angoloE = angoloF = 0; }
  if ( angoloC <= differenza &&  firstLoop == true ) { angoloD = angoloE = angoloF = 0; }
  if ( angoloD <= differenza &&  firstLoop == true ) { angoloE = angoloF = 0; }
  if ( angoloE <= differenza &&  firstLoop == true ) { angoloF = 0; }

  if ( angoloA > limitAngolo ) { angoloA = 0; firstLoop = false; }
  if ( angoloB > limitAngolo ) { angoloB = 0; firstLoop = false; }
  if ( angoloC > limitAngolo ) { angoloC = 0; firstLoop = false; }
  if ( angoloD > limitAngolo ) { angoloD = 0; firstLoop = false; }
  if ( angoloE > limitAngolo ) { angoloE = 0; firstLoop = false; }
  if ( angoloF > limitAngolo ) { angoloF = 0; firstLoop = false; }

  gradoA = (( sin( (angoloA/rad2grad) ) * 80 ) + 90 );
  gradoB = (( sin( (angoloB/rad2grad) ) * 80 ) + 90 );
  gradoC = (( sin( (angoloC/rad2grad) ) * 80 ) + 90 );
  gradoD = (( sin( (angoloD/rad2grad) ) * 80 ) + 90 );
  gradoE = (( sin( (angoloE/rad2grad) ) * 80 ) + 90 );
  gradoF = (( sin( (angoloF/rad2grad) ) * 80 ) + 90 );

  Serial.print( "gradi: " );
  Serial.print( gradoA );
  Serial.print( "\t" );
  Serial.print( gradoB );
  Serial.print( "\t" );
  Serial.print( gradoC );
  Serial.print( "\t" );
  Serial.print( gradoD );
  Serial.print( "\t" );
  Serial.print( gradoE );
  Serial.print( "\t" );
  Serial.print( gradoF );
  Serial.print( "\n" );

  servoA.write( gradoA );
  servoB.write( gradoB );
  servoC.write( gradoC );
  servoD.write( gradoD );
  servoE.write( gradoE );
  servoF.write( gradoF );

  angoloA = (angoloA + incremento );
  angoloB = (angoloB + incremento );
  angoloC = (angoloC + incremento );
  angoloD = (angoloD + incremento );
  angoloE = (angoloE + incremento );
  angoloF = (angoloF + incremento );
}

soffermati subito sulle linee 15-20: per ogni servo definisci un pin a cui è collegato, chiama le definizioni dei pin da A a F;

linee 27-39: definisci due gruppi di sei variabili in cui le prime sei memorizzano l’angolo o numero progressivo su cui calcoli il sen() da passare al servo. Il secondo gruppo ti serve per memorizzare il valore di ciascun grado da passare al corrispondente servo;

linea 41: definisci una variabile di tipo booleano ( true / fasle ) denominata firstLoop il cui compito è permetterti di capire se sei al primo giro della funzione loop od ad un loop successivo;

linee 43-48: sono proprio le linee in cui definisci le sei istanze relative ai sei servo collegati. NOTA: ogni istanza ha un nome differente, questo perchè non puoi dichiarare due istanze con il medesimo nome. Da questo momento in poi nello sketch quando parlerai al servo A lo farai sempre e solo attraverso l’istanza servoA;

linee 54-55: usa subito due metodi della classe Servo applicandoli all’istanza servoA, il primo metodo è attach() e serve a definire a quale pin hai collegato il servo che chiamo A. Il secondo metodo è write() e lo troverai anche in seguito, il suo compito è permetterti di posizionare il servo al grado che desideri;

linee 57-70: esegui le stesse operazioni delle linee 54-55 per ognuno dei sei servo con movimento armonico collegati;

linee 75-79: mediante una serie di condizioni farai in modo che i servo partano in modo sfalsato l’uno rispetto all’altro, concentrati sulla prima linea la 75, questa linea dice alla sketch: fino a quando il valore dell’angoloA è minore del valore della variabile differenza in cui avrai memorizzato di quanto vuoi sfalsare gli angoli, e solo se sei al primo loop() allora imposta i valori degli ancgoli da B ad F a 0. Ecco come ottieni che i servo da B ad F partano sfalsati del valore di differenza impostato rispetto al servoA.

linee 81-86: anche queste linee sono identiche tra loro a meno delle variabili, istanze e valori definiti. In pratica sulla linea 81 dici allo sketch che se il valore dell’angoloA è maggiore del valore della definizione di limitAngolo, impostata a 360°, deve reimpostare l’angolo a 0 e la variabile boolena firstLoop a false, ossia il primo giro è compiuto. Le righe seguenti fanno la medesima cosa per tutti gli angoli successivi.

Questa sequenza di linee ti permette di controllare l’angolo di cui calcoli il sin( x ) nelle righe successive dello sketch.

Dopo il primo giro ( loop ) quando il valore dell’angoloX arriva a 360 viene azzerato e imposti la variabile firstLoop a false per dire che da questo momento i loop() sono dal secondo in poi, ecco un’immagine di quello che vedrai sul monitor seriale:

avrai notato che all’inizio i si servo con movimento armonico partono tutti da 90° ma a valori sfalsati tra loro, quando il servoA raggiunge il valore 101° circa il servoB inizia il suo flusso, al raggiungimento del medesimo valore servoB parte il servoC e così via.

Avrai anche notato che in giallo ti ho evidenziato il passaggio del primo loop() ossia quando il valore dell’angolo viene riportato a 0, vedrai che ciclicamente tutti i servo raggiungono gli stessi valori in tempi sfalsati tra loro.

Linee 88-93: sono le linee in cui l’angoloA,B,C,D,E ed F si trasforma in un grado da applicare al servo corrispondente. Se hai letto gli articoli precedenti sul movimento armonico dei servo sai già come viene calcolato questo valore e perchè, se non lo hai fatto ti consiglio di leggerli se vuoi comprendere meglio il meccanismo che usano i sei servo con movimento armonico;

linee 109-114: applica a ciascun servo il valore in gradi calcolato alle linee 88-93;

linee 116-121: calcola per ogni angolo l’incremento del rispettivo valore.

Il video di sei servo con movimento armonico

Ecco il video che ho realizzato con 6 servo motori di marche differenti, non possiedo 6 servo tutti identici tra loro 🙂

Buon divertimento !!!