Micro Serial Servo Controller Pololu sketch

Hai letto nel precedente articolo cosa fa e come funziona il Micro Serial Servo Controller Pololu, in questo ti mostro lo sketch di controllo con cui ho realizzato il video del movimento congiunto di 8 servo.

Il sito ufficiale pololu nella sezione resources, in fondo, riporta un link al GitHub The Pololu serial servo API for Arduino ossia una libreria scritta per il controllo del Micro Serial Servo Controller da arduino MEGA, si proprio dalla MEGA in quanto fa uso di una seconda porta seriale Hw di cui, come sai, arduino uno non dispone.

Volendo provare il mio Micro Serial Servo Controller con un arduino Uno ho cercato in internet qualcuno che avessa già sviluppato uno sketch con la SoftwareSerial.h ed ho trovato questo link

In cui è presente sia lo sketch per utilizzare il controller con il pololu mode e con il Mini SSC Protocol.

Io ho provato il Pololu Mode.

Micro Serial Servo Controller Pololu Mode Sketch

Chiaramente lo sketch proposto nel link indicato non è stat scritto per controllare 8 servo contemporaneamente per cui ho dovuto apportare qualche modifica.

Non è stata l’unica modifica che ho eseguito, in pratica la costante BYTE come secondo parametro del metodo print è deprecata dalla versione dell’IDE 1.0 per cui volendo compilare lo sketch con una versione superiore ( io ho utilizzato la 1.6.4 ) ho apportato la modifica che leggi:

/*******************************************\
Pololu Micro Serial Servo Controller code
 
Control a Pololu Micro SSC using Pololu's 
protocol.
 
Rich Mattes
rjm5066@psu.edu
\********************************************/
 
#include <SoftwareSerial.h>
 
// Define pins you're using for serial communication
// Don't need to use pins 0 and 1
#define RXPIN 3
#define TXPIN 4

// Set up the new serial port to transmit data
// You only need to connect the TXPIN and Ground
// to the Pololu controller...RXPIN is unused
// (Pololu incapable of sending data back to arduino)
SoftwareSerial pololu(RXPIN, TXPIN);
 
void setup()
{
  // define pin modes for tx, rx:
  pinMode(RXPIN, INPUT);
  pinMode(TXPIN, OUTPUT);
 
  Serial.begin(9600);
  pololu.begin(40000);
}
 
void loop()
{
  // Use the SetPosition command to do stuff.
  // Takes values between 0 and 5000
  for (int i=20; i<160; i++) {
    int valDx = map( i, 0,180, 0,5000 );
    int valSx = map( i, 0,180, 5000,0 );
    
    SetServo(0, valDx);
    SetServo(1, valDx);
    SetServo(2, valDx);
    SetServo(3, valDx);

    SetServo(4, valSx);
    SetServo(5, valSx);
    SetServo(6, valSx);
    SetServo(7, valSx);
    
    delay(5);
    Serial.println( i );
  }
  
  for (int i=160; i>20; i--) {
    int valDx = map( i, 0,180, 0,5000 );
    int valSx = map( i, 0,180, 5000,0 );
    
    SetServo(0, valDx);
    SetServo(1, valDx);
    SetServo(2, valDx);
    SetServo(3, valDx);

    SetServo(4, valSx);
    SetServo(5, valSx);
    SetServo(6, valSx);
    SetServo(7, valSx);
    
    delay(5);
    Serial.println( i );
  }

}
 
// Set a servo position.  Returns 0 on success, -1 on error
// Uses the Pololu protocol, so the jumper has to be off of the controller
// Makes use of "Absolute Position" command
// VALID POSITION VALUES: 0-5000
int SetServo(int servo, int position)
{
  byte data1 = 0;
  byte data2 = 0;
 
  // Check to make sure the servo is right
  if (servo <0 || servo >8)
   return -1;
 
  // Check to make sure position is within bounds
  if (position < 0 || position > 5000)
    return -1;
 
  // Controller actually takes values between 500 and 5500,
  // so just add 500
  position+=500;
  // Calculate data bytes from position
  data2 = position & B01111111;
  data1 = position >> 7;
 
  
  pololu.write(0x80); // Start Byte
  pololu.write(0x01); // Device ID
  pololu.write(0x04); // Command: 0x04 is set absolute position
  pololu.write((byte)servo); // Servo number
  pololu.write(data1); // First data byte
  pololu.write(data2); // Second data byte
 
  // Everything seems ok, return 0
  return (0);
}

inizia dalla linea 11: includi la libreria SoftwareSerial.h che ti permette di utilizzare due pin digitali qualsiasi di arduino uno come canali Tx ed Rx seriali;

linee 15-16: definisci i pin Tx ed Rx da assegnare alla libreria inclusa alla linea 11;

linea 22: definisci una istanza pololu assegnandogli i pin Rx e Tx definiti alle linee 15 e 16;

linee 27-28: imposta la modalità dei pin Tx ed Rx come INPUT ed OUTPUT;

linea 30: imposta la velocità di comunicazione verso il monitor seriale a 9600 baud;

linea 31: imposta la velocità di comunicazione con il micro serial servo controller pololu a 40000 baud, velocità possibile da specifiche pololu e tra quelle che ho provato è quella con cui ho riscontrato meno anomalie;

linea 38: imposta un ciclo da 20 a 160, sono i gradi di movimento dei servo, ho notato infatti che passando da 0 a 180 ottenevo uno sforzo di finecorsa di ciascun servo nelle fasce 0-20 e 160-180, ho quindi ridotto i gradi in cui spostare i servo;

linea 39: per i servo di destra ri mappo i valori 0-180 su 0-5000, il range 0-5000 è quello previsto dal micro servo serial controller pololu per spostare i servo da 0 a 180°,

linea 40: esegui la medesima operazione della linea 39 invertendo i valori da inviare al controller in modo che i servo di sinistra si muovano al contrario rispetto a quelli posizionati a destra;

linee 42-50: richiama la funzione SetServo( servo, posizione ) passandole prima i servo posti a destra e successivamente i servo posti a sinistra ed il rispettivo valore di posizione a cui andare;

linea 52: attendi 5 millisecondi tra un ciclo for ed il successivo;

linea 53: scrivi sul monitor seriale l’angolo i a cui hai inviato i servo;

linee 56-72: esegui le medesime operazioni di controllo servo ma partendo da 160 e scorrendo fino a 20;

linea 080: definisci la funzione SetServo() richiamata più volte dalla loop();

linee 82-83: imposta due variabili di tipo byte ciascuna atta a contenere i valori da passare al micro serial servo controller pololu;

linee 86-87: controlla che il numero corrispondente al servo da posizionre non sia inferiore a 1 o superiore a 8, il micro serial servo controller può al massimo controllare 8 servo.

linee 90-91: allo stesso modo controlla che il valore da inviare al servo per la posizione non deve essere < di 0 o maggiore di 5000;

linea 95: dal manuale sai che i reali valori che puoi trasferire al servo come posizione sono 500-5500 per cui rispetto al range inviato 0-5000 sommi al valore +500 per spostare il valore nel range corretto;

linea 97: esegui una operazione di AND ( & )  con una maschera 01111111 per cui qualsiasi valore sia contenuto in position il valore di data2 conterrà gli ultimi 7 bit del valore;

linea 98: esegui lo shift del valore binario di position in modo che data1 contenga i primi bit del valore passato. Con le operazioni precedenti hai ottenuto che data1data2 contengano il valore binario di position che può essere al massimo 5500 ossia 1010101111100 quindi 13 bit;

linee 101-106: utilizzando il metodo write, al posto del metodo print che non supportando la costante BYTE non è adatto ad inviare byte attraverso la seriale, il metodo consigliato per l’invio di byte è write in cui non devi neppure specificare che vuoi trasferire il dato in byte, lo fa di default.

linea 109: se tutto il trasferimento dei dati al servo è andato a buon fine restituisci 0 alla funzione loop() che ti ha chiamato.

Il video, che hai già visto nel precedente articolo, mostra il funzionamento dello sketch: