//Arduino: braccio meccanico e palline colorate – robotic arm and colored balls //Servo, TCS230, color sensor, sensore di colore,

C1 - braccio meccanico e palline colorate – robotic arm and colored balls – (some notes at section end)

Figura 1

Arduino riconosce il colore di alcune palline (solo i colori blu, rosso e verde) e, tramite un braccio meccanico, le afferra e le deposita in contenitori specifici per ogni colore. Questo progetto nasce dall’unione di un sensore di colore TCS230 e di un braccio meccanico dotato di sei livelli di movimento. Si tratta di un progetto la cui parte meccanica presenta qualche livello di complessita’ per cui, qualora lo si volesse replicare, si consiglia di seguire le indicazioni di costruzione presenti nella scheda del progetto 46 braccio meccanico

In particolare e’ indispensabile:

utilizzare dei servomotori di grande potenza con ingranaggi in metallo, (tipo MG995 o, meglio, superiori)

disporre di una batteria da 6 volt in grado di erogare almeno 12 ampere,

azzerare la posizione dei servomotori prima di montarli (il programma di azzeramento e’ reperibile nella scheda del progetto 46, di cui sopra)

montare le parti meccaniche facendo attenzione a non ruotare accidentalmente il perno dei servomotori (per non spostarli dalla loro posizione zero) e tenendo presente che

gli stessi girano in senso antiorario. In linea di massima al termine della costruzione il braccio deve avere una posizione simile a quella in figura 1 e la pinza deve essere aperta.

Una volta costruito il braccio bisogna riprogrammare i movimenti in funzione della specifica situazione ambientale (essenzialmente per la posizione del rilevatore di colore e dei contenitori)

Per programmare i movimenti e’ necessario dapprima individuarli manualmente (tramite i pulsanti ed il display lcd) e poi trascriverli nella tabella tabprog inserendo, in sequenza le mosse che il braccio deve compiere partendo dalla posizione iniziale (posizione assunta automaticamente al termine di ogni ciclo di mosse e definita nelle righe di tabprog dal 44 in su) per:

afferrare la pallina,

spostarsi sul contenitore selezionato in base al colore

rilasciare la pallina

Per ogni mossa bisogna inserire, in tabprog:

il numero del servomotore che si intende muovere

i gradi che il servomotore deve raggiungere

il tempo che il servo deve impiegare per muoversi di un grado (questo tempo serve a rallentare l’azione, in modo da ridurre al minimo l’inerzia delle masse in movimento)

Al termine di ogni ciclo di mosse (al termine delle mosse per il colore blu, verde e rosso), bisogna inserire un elemento di chiusura e cioe’ una mossa con codice servomotore 99, gradi zero e tempo zero. Le mosse per il colore rosso iniziano dall’elemento zero della tabella, le mosse per il colore blu iniziano dall’elemento 10 mentre quelle per il colore verde iniziano dall’elemento 20. Attenzione: nel modificare tabprog non alterare il numero di virgole

Per quanto concerne il sensore di colore (il TCS230) valgono le raccomandazioni e le note costruttive presenti nel progetto 55 – sensore di colore. In particolare bisogna inizialmente eseguire la fase di calibrazione del sensore, ponendo sul sensore, quando chiesto dal programma e per qualche secondo ognuna, le palline di diverso colore. Nel caso dovessero apparire segnalazioni di fuoriscala bisognera’ rifare la calibrazione, attenuando un poco la luci ambientali.

Il filmato di questo progetto e’ reperibile qui: braccio meccanico e palline colorate

//Arduino: braccio meccanico e palline colorate – robotic arm and colored balls //Servo, TCS230, color sensor, sensore di colore,

Prima di compilare il programma bisogna, scaricare ed installare, se non gia’ fatto:

La libreria di gestione del display a cristalli liquidi, reperibile qui.

Per installare una libreria:

Scaricarla mantenendola in formato compresso

Andare in IDE-> sketch-> importa libreria-> add library->

Individuare la cartella di download->

Fare doppio click sulla libreria da installare

Verificarne la presenza in IDE-> sketch-> includes Library-> Contributed library

Nota: Questo esercizio e questa nota sono parte di una serie che vede protagonisti Arduino ed

alcuni dei componenti ad esso collegabili. Per la maggior parte degli esercizi e’ anche disponibile un filmato su youtube:

Esercizi facenti parte della raccolta

Filmati presenti su youtube

Informazioni su arduino e sui componenti collegabili (PDF scaricato nell’area di download)

Breve manuale di programmazione (PDF scaricato nell’area di download)

Per eventuali chiarimenti o suggerimenti sul contenuto di questa scheda scrivere a giocarduino@libero.it

Here some notes about this project, translated by google translator

Arduino recognizes color of some balls (only blue, red and green colors) and, through a robotik arm, grabs and deposits them in containers, specific for each color. This project was born from the union of a TCS230 color sensor and a robotik arm equipped with six servo motors. This is a project whose mechanical part has some level of complexity so, if you want to replicate it, it is advisable to follow the construction indications present in the project file 46 robotic arm

Figura 2

In particular you must:

use high power servomotors with metal gears, (like MG995 or higher)

have a 6-volt battery capable of delivering at least 12 amps,

positioning servomotors to zero before mounting them (the reset program can be found in project 46 file)

assemble the mechanical parts taking care not to accidentally rotate the servomotor pin (to avoid moving it from zero position) and bearing in mind that it rotates counter clockwise. In general, at end of construction, arm must have a position similar to the one in figure 1 and clamps must be open.

Once arm is built, it will be necessary reprogram movements according to the specific environmental situation (essentially due to the position of the color sensor and the balls containers)

To program movements it is first necessary identify them manually (using buttons and LCD display) and then write them down in tabprog array, by entering, in sequence, moves that arm must perform starting from the initial position (position automatically assumed at the end of each cycle of moves and defined in tabprog from rows 44 and up) for:

grab ball,

move to selected container, according to ball color

release ball

//Arduino: braccio meccanico e palline colorate – robotic arm and colored balls //Servo, TCS230, color sensor, sensore di colore,

For each move you must enter, in tabprog:

the code of servomotor to be acted

the degrees that the servomotor must reach

the time (in milliseconds) that the servo must use to move one degree (is used to slow down the action, so as to minimize the inertia of moving masses).

At the end of each moves cycle (at the end of the blue, green and red moves), a closing element must be inserted, ie a move with servo code 99, zero degrees and zero time. About tabprog, The red moves start in the zero element, the blue moves start in element 10 and the green moves start from element 20. Warning: in modifying tabprog do not alter commas number and position.

As far as the color sensor is concerned (the TCS230) apply the recommendations and constructive notes in project 55 – color sensor, in particular the calibration phase must be performed placing, when requested by program and for a few seconds each, the balls on sensor. If on display appear fuoriscala (off-scale) it will be necessary to redo calibration by slightly attenuating ambient lights.

The movie of this program is linked here: robotic arm and colored balls

Before proceeding to program compilation must be installed, if not already done, the library:

LiquidCrystal_I2C.h found in: https://github.com/fmalpartida/New-LiquidCrystal

For library installation, see process shown in previous projects, and summarized in:

library download in compressed form;

Installation via IDE-> sketch-> includes Library-> add .zip library

After installation please verify the library. It must be present in IDE-> sketch-> includes Library-> Contributed library

Note: This project and this note is part of a series that sees, as main characters, Arduino and some of connectable components. For most projects there is also a video on youtube.

Projects collection

Movies on youtube

About Arduino and components (italian; pdf will be downloaded in your download area

Quick programming guide (almost english; pdf will be downloaded in your download area) For any questions or suggestions about this note (and on its english translation), please write to giocarduino@libero.it (simple words and short sentences, please)

Materiali

1 sensore di colore TCS230 6 servo tipo Futaba S3003 o MG995 o, meglio, altri piu’ potenti e con ingranaggi in metallo

1 kt per la cotruzione di un braccio meccanico, reperibile sui magazzini on line

3 pulsanti

3 resistenze da 10 kohm

1 display 1602 con driver I2C

//Arduino: braccio meccanico e palline colorate – robotic arm and colored balls //Servo, TCS230, color sensor, sensore di colore,

Schema

Programma /* Attenzione: facendo il copia/incolla dal PDF all’IDE si perde la formattazione del testo. Per

rendere piu’ facilmente leggibile il programma e’ opportuno formattarlo subito dopo il

trasferimento nell’IDE, premendo CTRL+T.

In questo programma un sensore di colore individusa il colore di una pallina ed un braccio

meccanico la afferra e la deposita in un contenitore specifico per il colore.

SEZIONE RELATIVA AL BRACCIO MECCANICO:

Questo programma puo' essere utilizzato anche per studiare il movimento dei servomotori di un

braccio meccanico. Il servomotore su cui agire e' selezionabile tramite un pulsante mentre

l'aumento o la diminuzione dei gradi e' pilotato da due pulsanti (uno per aumentare e l'altro per

Download del programma via Dropbox Program download via Dropbox

//Arduino: braccio meccanico e palline colorate – robotic arm and colored balls //Servo, TCS230, color sensor, sensore di colore,

diminuire). I gradi raggiunti sono visualizzati su di un display.

Il programma contiene anche una sezione di “mosse programmate” con le quali il braccio esegue

alcune azioni predefinite (e cioe’ prende la pallina e la dposita nel suo contonitore. La sezione

“mosse programmate” si attiva selezionando il servo numero 6. Le mosse programmate partono tre

secondi dopo la selezione del servo 6.

Per programmare i movimenti del braccio bisogna innanzitutto individuare ogni singolo movimento,

in termini di numero di servomotore da azionare e gradi da raggiungere, ed infine, una volta

individuati tutti i movimenti, inserirli nella tabella tabprog, facendo attenzione a non alterare

il numero totale di virgole.

Ogni mossa e' composta da tre dati: numero del servomotore da muovere, numero di gradi da

raggiungere, tempo di attesa per l'esecuzione di un grado. Ogni sequenza di mosse deve essere

chiusa dal codice "99" inserito nella zona dedicata al numero del servomotore.

Le mosse per la gestione delle palline di colore rosso devono essere inserite a partire dal

primo elemento della tabella; quelle di colore verde a partire dall'elemento 10 e quelle per il

blu a partire dall'elemento 20.

SEZIONE RELATIVA AL SENSORE DI COLORE:

Questo programma identifica il colore di un oggetto (rosso o verde o blu) posto davanti ad un

sensore di tipo TCS230.

Piu’ in dettaglio, la luce riflessa dall’oggetto viene “letta” dal sensore utilizzando, in rapida

sequenza, diodi sensibili al colore rosso, al colore verde ed al colore blu. Il sensore trasforma

ogni lettura in una serie di segnali digitali, la cui frequenza e’ inversamente proporzionale al

colore dell’oggetto in esame. Arduino converte questa frequenza (rilevata tramite l’istruzione

“pulseIn”) in un valore compreso tra 0 a 255 e poi analizza le frequenze proposte dalla “lettura”

effettuata dalle tre differenti tipologie di diodi e tenta di interpretarle.

Per ottenere valori stabili ed in qualche modo vicini ai reali valori RGB, i parametri di

mappatura(ciclo di while e istruzione map delle frequenze segnalate da ogni colore) devono essere

definiti di volta in volta, in funzione della luce ambientale e della intensita' del colore degli

oggetti campione.

Nella fase inziale il programma esegue quindi una serie di attivita' necessarie alla definzione

Dei parametri sia in assenza di oggetti che in presenza di oggetti colorati

Al termine della fase di inzializzazione il sistema avra' definito i parametri necessari al

riconoscimento di ogni colore.

Una volta stabiliti i parametri ( e cioe' i valori minimi e massimi di ogni fotorilevatore, in

presenza di colori il programma, sempre per rendere stabile e sicura l'individuazione del colore,

esegue 5 letture consecutive per ogni oggetto posto vicino al sensore ed alla fine espone

e considera la media dei valori rilevati.

------------------------------------------------------------------------------------------------

Warning: cut&paste from PDF to IDE loses formatting. Oo restore it press CTRL + T.

ROBOTIC ARM SECTION:

This section can be used as a case study for servo motors in a mechanical arm. The servo on which

to act is selectable via a button while the degrees increase or decrease, using other two buttons

(one to increase and the other to decrease) . The achieved degrees are displayed on a display.

The program also includes a section "programmed moves" in which the mechanical arm performs some

programmed movements, stored in an array. The "programmed moves" are selected choosing servo

number 6, and starts working after 3 seconds

To program arm moves, it is first necessary identify each movement, in terms of number of

servomotor to be operated and degrees to be achieved. Once all the movements have been

identified, insert them in the tabprog array, taking care not to alter the commas total number.

Each move consists of three data: number of servo to be act, number of degrees to reach and the

waiting time (in milliseconds) for one degree execution. Each sequence of moves must be closed

by the code "99" inserted in the area dedicated to the servomotor number.

The moves to manages the red balls must be entered starting from first element of the table;

those of green color starting from element 10 and those for blue from element 20.

COLOR SENSOR SECTION:

This section identifies a color of an object (red or green or blue) placed near a TCS230 sensor.

More in detail, the light reflected by object is "read" by sensor using, in rapid succession,

diodes sensitive to red, green and blue. Sensor transforms each reading into a series of digital

signals, whose frequency is inversely proportional tothe color of the object . Arduino converts

this frequency (detected by the "pulseIn" instruction) in a value between 0 and 255 and then

//Arduino: braccio meccanico e palline colorate – robotic arm and colored balls //Servo, TCS230, color sensor, sensore di colore,

analyzes frequencies proposed by the "reading" performed by the three different types of diodes,

and tries to interpret them.

To obtain stable values and close to the real RGB values, the mapping parameters ("while" cycle

and "map) statement) must be defined each time, depending on ambient light and color intensity

of the sample objects.

In the initial phase, program performs a series of activities for defining the parameters both in

absence of objects and in presence of colored objects.

At the end of the initialization phase, system will have defined the parameters necessary for the

recognition of each color.

Once the parameters have been established (and that is the minimum and maximum values reeased by

Each photodetector), program performs 5 consecutive readings for each object placed near the

sensor and then exposes and considers the average of the values measured.

*/

#define giu 16 // il pulsante giu e' collegato alla porta 16 (ex analog pin A2)- down button pin

#define su 15 // il pulsante su e' collegato alla porta 15 (ex analog pin A1) - up button pin

#define scelta 14 // pulsante selezione collegato alla porta 144 (ex A0) - servo select button pin

#define S0 10 // pin S0 del sensore di colore - TCS230 So pin

#define S1 9 // pin S1 del sesnore di colore - TCS230 s1 pin

#define S2 12 // pin S2 del sensore di colore - TCS230 S2 pin

#define S3 11 // pin S3 del sensore di colore - TCS230 S3 pin

#define sensore 13// PIN OUT del sesnore di colore - TCS230 OUT pin

#include <Wire.h> // libreria wire presente, di fault, nell'IDE

#include <LiquidCrystal_I2C.h> // libreria di gestione display lcd a due porte(vedi esecizio 18

// bis nel sito sopra indicato). Nota: i terminali SDA e SCL del display devono essere collegati,

// rispettivamente, alle porte analogiche 4 e 5

// . . . . . . . . . addr, en,rw,rs,d4,d5,d6,d7,bl,blpol

LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE); // definisce tipologia del display

// LiquidCrystal_I2C lcd(0x3F, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);

// ^ indirizzo alternativo, da attivare al posto della precedente istruzione se lcd non funziona

// ^^ Alternative address, to activate in place of previous instruction if LCD doesn't work

#include <Servo.h> // richiama la libreria di gestione dei servomotori

Servo servobase; // crea i servo oggetti per ogni servo del braccio meccanico.

Servo servobraccio;

Servo servoavambraccio;

Servo servopolso;

Servo servogirapolso;

Servo servomano;

int statogiu = 0; // variabile di memorizzazione dello stato del pulsante giu

int statosu = 0; // Variabile di memorizzazione dello stato del pulsante su

int statosele = 0; // variabile di memorizzazione dello stato del pulsante "selezione"

int indix = 0; // indice utilizzato in diversi cicli di for

long tempoinizio = 0; // memorizza il momento della selezione 6 (selta sequenza programmata)

long tempoattuale = 0; // memorizza il momento di verifica del tempo trascorso dalla selezione 6

int mossa = 0; // numero di mossa della sequenza programmata

int mossainiziale = 0; // numero della mossa di partenza, in tabella tabprog

int tempo = 0; // tempo (in millisecondi)da impegnare per lo spostamento di 1 grado

int movimento = 1; // movimento di un grado sul servomotore selezionato. Puo' essere positivo

//(1) oppure negativo (-1) a seconda che i gradi da raggiungere siano superiori o inferiori ai gradi

// correnti. *** Movement of 1 degree on the selected servo motor. Can 'be positive (1) or

//negative (-1) depending if degrees to be achieved are above or below the current degrees

byte sele = 0; // selezione del servomotore da 0 a 5, in sequenza: base, braccio,

// avambraccio,polso, girapolso, mano – servomotor selected and working (from 0 to 5)

byte selezione = 0; // selezione operata tramite pulsanti

char descservo [7][12] = // descrizione dei servomotori - servomotor description

{

{"base "}, // base

{"braccio "}, // arm

{"avambraccio"}, // forearm

{"polso "}, // wrist

{"girapolso "}, // wrist turn

{"mano "}, // hand

{"seq.program"}, // programmed sequence

};

int tabgradi [8] =

{0, 0, 0, 0, 0, 0, 0, 0}; // memorizzazione gradi di ogni servo – current degrees for each servo

byte tabprog [151] = // tabella di memorizzazione sequenza di mosse programmate (max 50).

// ogni mossa prevede tre parametri: il numero del servomotre (da 0 a 5), i gradi che deve

// raggiungere ed il tempo per compiere il movimento di un grado. il servomotore "99" chiude la

// serie di mosse programmate. *** storage array for programmed moves (max 50). Each move requires

// three parameters: the servo number (0 to 5), degrees that must reached and time for a one

// degree movement. Servo "99" closes the series of planned moves

//servo, gradi, velocita’ (tempo per il movimento di un grado)

// servo, degrees, speed (time for one degree movement)

{

//Arduino: braccio meccanico e palline colorate – robotic arm and colored balls //Servo, TCS230, color sensor, sensore di colore,

0, 63, 10, // mossa 0 move #0

2, 12, 20, // mossa 1 move #1

1, 33, 10, // mossa 2 ...

3, 1, 10, // mossa 3

5, 35, 10, // mossa 4

1, 20, 20, // mossa 5

0, 40, 10, // mossa 6

5, 0, 10, // mossa 7

99, 0, 0, // mossa 8

0, 0, 0, // mossa 9

0, 63, 10, // mossa 10

2, 12, 20, // mossa 11

1, 33, 20, // mossa 12

5, 35, 10, // mossa 13

1, 20, 10, // mossa 14

0, 90, 20, // mossa 15

5, 0, 10, // mossa 16

99, 0, 0, // mossa 17

0, 0, 0, // mossa 18

0, 0, 0, // mossa 19

0, 63, 10, // mossa 20

2, 12, 20, // mossa 21

1, 33, 20, // mossa 22

5, 35, 10, // mossa 23

1, 20, 20, // mossa 24

0, 15, 20, // mossa 24

5, 0, 10, // mossa 26

99, 0, 10, // mossa 27

0, 0, 0, // mossa 28

0, 0, 0, // mossa 29

0, 0, 0, // mossa 30

0, 0, 0, // mossa 31

0, 0, 0, // mossa 32

0, 0, 0, // mossa 33

0, 0, 0, // mossa 34

0, 0, 0, // mossa 35

0, 0, 0, // mossa 36

0, 0, 0, // mossa 37

0, 0, 0, // mossa 38

0, 0, 0, // mossa 39

0, 0, 0, // mossa 40

0, 0, 0, // mossa 41

0, 0, 0, // mossa 42

0, 0, 0, // mossa 43

// le seguenti mosse sono utilizzate per fare assumere al braccio la posizione iniziale

// The following moves are used to set arm in starting position

5, 0, 10, // mossa 44

4, 55, 10, // mossa 45

3, 13, 10, // mossa 46

1, 5, 20, // mossa 47

2, 1, 20, // mossa 48

0, 63, 20, // mossa 49

};

int frequenza = 0;

int freqrosso = 0;

int freqverde = 0;

int freqblu = 0;

int rosso = 0;

int blu = 0;

int verde = 0;

int indice = 0;

byte tabvalori [751]; // memorizzazione delle ripetizioni delle frequenze di ogni colore;

// l'indice della tabella e' la frequenza, e le frequenze analizzate vanno da 0 a 250 per tre colori

// matrix to storing frequencies repetitions for each color; the matrix index is the frequency, and

// the analyzed frequencies go from 0 to 250, for three colors

//

float tempoiniz = 0; // momento di inizio calibrazione

float tempocorr = 0; // momento attuale

byte minaccett = 0; // Occorrenze minime accettabili (il 2 % del numero totale di occorrenze)

byte minrosso = 250;

byte maxrosso = 0;

byte minverde = 250;

byte maxverde = 0;

byte minblu = 250;

byte maxblu = 0;

int contatore = 0;

byte minrosson = 250;

byte maxrosson = 0;

//Arduino: braccio meccanico e palline colorate – robotic arm and colored balls //Servo, TCS230, color sensor, sensore di colore,

byte minverden = 250;

byte maxverden = 0;

byte minblun = 250;

byte maxblun = 0;

byte semaforo = 0; // semaforo di gestione del braccio (1 = seq. programmata; 0 = gest.manuale)

//

//**** routine di lettura delle frequenze in uscita dal sensore TCS230 ****

//***************** TCS230 output reading routine ***********************

//

void lettura ()

{

//********* rileva la frequenza emanata dai fotorilevatori sensibili al rosso *************

// ***************detects frequency emitted by red sensitive photodetectors ***************

digitalWrite(S2, LOW);

digitalWrite(S3, LOW);

frequenza = pulseIn(sensore, LOW);

if (frequenza > 250)

{

lcd.clear ();

lcd.print F("fuoriscala rosso"); // red frequencies out of limits

lcd.print (frequenza);

}

else

freqrosso = frequenza;

delay (30);

//********* rileva la frequenza emanata dai fotorilevatori sensibili al verde ***************

//***************detects frequency emitted by green sensitive photodetectors ****************

digitalWrite(S2, HIGH);

digitalWrite(S3, HIGH);

frequenza = pulseIn(sensore, LOW);

if (frequenza > 250)

{

lcd.clear ();

lcd.print F("fuoriscala verde"); // green frequencies out of limits

lcd.print (frequenza);

}

else

freqverde = frequenza;

delay (30);

//********* rileva la frequenza emanata dai fotorilevatori sensibili al blu **************

//***************detects frequency emitted by blue sensitive photodetectors **************

digitalWrite(S2, LOW);

digitalWrite(S3, HIGH);

frequenza = pulseIn(sensore, LOW);

if (frequenza > 250)

{

lcd.clear ();

lcd.print F("fuoriscala blu "); // blue frequencies out of limits

lcd.print (frequenza);

}

else

freqblu = frequenza;

delay (30);

}

//

//******** routine di gestione della sequenza programmata **********************

//************ preprogrammed sequence execution ***************************

//

void sequenzaprogrammata (void)

{

for (mossa = mossainiziale; mossa <= 49; mossa ++) // loop di mosse programmate

{

if (tabprog [mossa * 3] == 99) // se ha raggiunto la fine della programmazione

break; // esce dal loop di mosse programmate

sele = tabprog [mossa * 3]; // memorizza il numero del servomotore sul quale deve agire

movimento = 1;

if (tabprog [mossa * 3 + 1] < tabgradi [sele]) // se posizione da raggiungere < pos. corrente

movimento = -1; // cambia il segno del movimento

tempo = tabprog [mossa * 3 + 2];

while (!(tabgradi [sele] == tabprog [mossa * 3 + 1]))

{

tabgradi [sele] = tabgradi [sele] + movimento; // cambia di un grado (in + o in -, a seconda

// dei casi)la posizione del servomotore in esame

visualizzaimpulsi ();

posizionaservo ();

}

}

selezione = 0; // forza la selezione del servomotore 0, per riprendere il normale ciclo di loop

//Arduino: braccio meccanico e palline colorate – robotic arm and colored balls //Servo, TCS230, color sensor, sensore di colore,

}

//

//********************** routine di movimento dei servo motori ************************

//****************************** servo move routine *******************************

//

void posizionaservo (void)

{

servobase.write (tabgradi [0]);

servobraccio.write (tabgradi [1]);

servoavambraccio.write (tabgradi [2]);

servopolso.write (tabgradi [3]);

servogirapolso.write (tabgradi [4]);

servomano.write (tabgradi [5]);

delay (tempo);

}

//

// ******** routine di gestione dei gradi di inclinazione *********

// *************** acquiring degrees **************************

//

void acquisiscigradi (void)

{

if (tabgradi [sele] == 0)

tabgradi [sele]++; // pone ad 1 i gradi nel caso abbiano gia' raggiunto il minimo)

if (tabgradi [sele] == 180)

tabgradi[sele]--; //pone a 179 i gradi nel caso abbiano gia' raggiunto il massimo

statogiu = digitalRead (giu); // acquisisce lo stato del pulsante giu

if (statogiu == 1) // se e' stato premuto il pulsante giu

tabgradi [sele]--; // diminuisce di 1 il valore dei gradi

statosu = digitalRead (su); // acquisisce lo stato del pulsante su

if (statosu == 1) // se e' stato premuto il pulsante su

tabgradi[sele]++; // aumenta di 1 i gradi

}

//

// ******** routine di selezione del servomotore *********

// ***************** selecting servo *****************

//

void selezionaservo (void)

{

statosele = digitalRead (scelta); // acquisisce lo stato del pulsante seleziona servo

if (statosele == 1) // se e' stato premuto il pulsante di selezione

{

selezione++; // passa al servomotore successivo

if (selezione == 7)

selezione = 0; // pone a 0 il servomotore in selezione quando la selezione va oltre il 6

delay (300);

}

}

//

//************ routine di visualizzazione servo attivo ed impulsi ************************

//*****************displaying data on LCD **********************************************

//

void visualizzaimpulsi (void)

{

lcd.setCursor(0, 0); // posiziona il cursore all'inizio della prima riga

lcd.print (sele);

lcd.print (" - ");

lcd.setCursor (4, 0);

lcd.print (descservo [sele]);

lcd.setCursor (0, 1);

lcd.print F("gradi = ");

lcd.print (tabgradi [sele]);

lcd.print F(" ");

}

//

// ************** routine di riconoscimento del colore *******************

// ********************color detection routine **************************

//

void riconoscecolore (void)

{

rosso = 0;

verde = 0;

blu = 0;

tempoiniz = millis (); // memorizza il momento di inizio del loop delle 5 letture

for (indice = 1; indice < 6; indice = indice) // esegue 5 letture e poi utilizza la media di

// per individuare il colore *** performs 5 readings and then uses the average to identify color

{

tempocorr = millis (); // memorizza il momento della rilevazione

if ((tempocorr - tempoiniz ) > 3000) // se sono passati piu' di tre secondi da inizio

//Arduino: braccio meccanico e palline colorate – robotic arm and colored balls //Servo, TCS230, color sensor, sensore di colore,

// rilevazione significa che il sensore non vede alcun colore e quindi esce dalla routine di

// rilevazione e torna alla gestione manuale del braccio - If are more than 3 second from detection

// beginning, it means that sensor does not see any color and therefore resets data and come back to

// arm manual handling

{

indice = 6;

semaforo = 0;

}

else // se sta leggendo un colore - if is reading a color

{

lettura (); // lancia la routine di lettura dei colori if (((( freqrosso < minrosson) || (freqrosso > maxrosson)) && ((freqverde < minverden) || (freqverde > maxverden)) && ((freqblu > minblun) || (freqblu < maxblun))))

{ // se la lettura e' valida (se il sensore ha individuato un oggetto) - if reading i valid

indice++;

rosso = rosso + map(freqrosso, minrosso, maxrosso, 255, 0); // rimappa la frequenza per

// conventirla in valori RGB

verde = verde + map(freqverde, minverde, maxverde, 255, 0); // rimappa la frequenza per

// conventirla in valori RGB

blu = blu + map(freqblu, minblu, maxblu, 255, 0); // rimappa la frequenza per

// conventirla in valori RGB

}

}

}

// ********** individua il colore *********

// ********* compute a color **************

if (semaforo == 1)

{

lcd.clear ();

Serial.print F("R = ");

Serial.print (rosso / 5);

Serial.print F(" ");

Serial.print F("G = ");

Serial.print (verde / 5);

Serial.print F(" ");

Serial.print F("B = ");

Serial.print (blu / 5);

Serial.println F(" ");

if ((blu > verde) && (blu > rosso))

{

lcd.print F("blu"); // blue

mossainiziale = 20;

}

if ((verde > blu) && (verde > rosso))

{

lcd.print F("verde"); // green

mossainiziale = 10;

}

if ((rosso > verde) && (rosso > blu))

{

lcd.print F("rosso"); // red

mossainiziale = 0;

}

delay (2000);

}

}

//

//

void setup()

{

Serial.begin (9600);

lcd.begin(16, 2); // inizializza il display (16 caratteri per due righe)

lcd.print F("buongiorno");

servobase.attach(2); // assegna il servo oggetto “servobase” alla porta 2

servobraccio.attach(3); // assegna il servo oggetto “servobraccio” alla porta 3

servoavambraccio.attach(4); // assegna il servo oggetto “servoavambraccio” alla porta 4

servopolso.attach(5); // assegna il servo oggetto “servopolso” alla porta 5

servogirapolso.attach(6); // assegna il servo oggetto “servogirapolso” alla porta 6

servomano.attach(7); // assegna il servo oggetto “servomano” alla porta 7

pinMode (giu, INPUT); // definisce il pulsante giu (pin2) come unita' di input

pinMode (su, INPUT); // definisce il pulsante su (pin3) come unita' di input

pinMode (scelta, INPUT); // definisce il pulsante scelta (pin 4) come unita' di input

pinMode(S0, OUTPUT); // inizializza il pin S0 del TCS230

pinMode(S1, OUTPUT); // inizializza il pin S1 del TCS230

pinMode(S2, OUTPUT); // inizializza il pin S2 del TCS230

pinMode(S3, OUTPUT); // inizializza il pin S3 del TCS230

pinMode(sensore, INPUT); // inizializza il pin OUT del TCS230

posizionaservo (); // porta i servomotri in posizione zero

mossainiziale = 44; // predispone l'indice sulla sequenza di inizio lavoro

sequenzaprogrammata (); // Mette il braccio in posizione di inizio lavoro

//Arduino: braccio meccanico e palline colorate – robotic arm and colored balls //Servo, TCS230, color sensor, sensore di colore,

mossainiziale = 0;

//

// sets the TCS230 for an output a range equal to 20% of the frequencies

digitalWrite(S0, HIGH); // setta il sensore (pin s0 ed s1) in modo che fornisca un segnale

digitalWrite(S1, LOW); // la cui frequenza sia pari al 20& della frequenza massima fornibile

//

// ********** fase di calibrazione sulle frequenze minime e massime **********

// *************** calibration phase on minimum and maximum frequencies ******

//

// *********** calibrazione in assenza di colori ************************************

// *******************calibration without colors *************************************

//

Serial.println F("calibrazione in assenza di colori......"); // calibration without colors...

for (indice = 1; indice < 50; indice ++)

{

lettura (); // lancia la routine di lettura delle frequenze rilasciate dal sensore

tabvalori [freqrosso * 3] = tabvalori[freqrosso * 3] + 1; // conteggia la frequenza del rosso

tabvalori [(freqverde * 3) + 1] = tabvalori[(freqverde * 3) + 1] + 1; // conteggia freq. verde

tabvalori [(freqblu * 3) + 2] = tabvalori[(freqblu * 3) + 2] + 1; // conteggia frequenza blu

}

//

// *************** uscito dalla fase di rilevazione, analizza i dati memorizzati ***************

// *********************** analyzes stored data **********************************************

//

minaccett = 1; // definisce la minima occorrenza accettabile (il 2% del numero di cicli)

for (indice = 1; indice < 250; indice ++) // analizza la tabella delle frequenze per individuare

// la frequenza minima e massima rilevata dai fotorilevatori di ogni colore

{

if (tabvalori [indice * 3] > minaccett) // se il numero di occorrenze del rosso e' superiore al

// minimo accettabile *** if occurrences are greater than minimum acceptable

{

if (indice < minrosso)

minrosso = indice; // memorizza la frequenza (l'indice e' la frequenza, mentre in tabella

// ci sono le occorrenze) memorizes frequency (index is the frequency, while in the array there are

// occurrences)

if (indice > maxrosso)

maxrosso = indice;

}

if (tabvalori [(indice * 3) + 1] > minaccett) // se numero occorrenze verde > al minimo accett.

{

if (indice < minverde)

minverde = indice; // memorizza la frequenza (l'indice e' la frequenza, mentre in tabella

// ci sono le occorrenze)

if (indice > maxverde)

maxverde = indice;

}

if (tabvalori [(indice * 3) + 2] > minaccett) // se numero occorrenze blu > minimo accettabile

{

if (indice < minblu)

minblu = indice; // memorizza la frequenza (l'indice e' la frequenza, mentre in tabella

// ci sono le occorrenze)

if (indice > maxblu)

maxblu = indice;

}

}

Serial.println F("frequenze minime e massime:"); // minimum and maximum frequencies

Serial.print F("rosso: "); // red

Serial.print (minrosso);

Serial.print F(" ");

Serial.println (maxrosso);

Serial.print F("verde: "); // green

Serial.print (minverde);

Serial.print F(" ");

Serial.println (maxverde);

Serial.print F("blu: "); // blue

Serial.print (minblu);

Serial.print F(" ");

Serial.println (maxblu);

for (indice = 0; indice < 750; indice ++)

tabvalori [indice] = 0; // azzera la tabella tabvalori

// memorizza le frequenze minime e massime in assenza di colore (aumentati o diminuiti di 2 unita)

// memorizes minimum and maximum frequencies with no colors (increased or decreased by 2 units)

minrosson = minrosso - 2;

maxrosson = maxrosso + 2;

minverden = minverde - 2;

maxverden = maxverde + 2;

minblun = minblu - 2;

maxblun = maxblu + 2;

//Arduino: braccio meccanico e palline colorate – robotic arm and colored balls //Servo, TCS230, color sensor, sensore di colore,

// ripristina i valori per una nuova calibrazione - restores values for a new calibration

minrosso = 250;

maxrosso = 0;

minverde = 250;

maxverde = 0;

minblu = 250;

maxblu = 0;

Serial.println F("calibrazione con i colori: "); // colors calibration:

Serial.println F("al via hai 15 secondi per posizionare, uno dopo l'altro, gli oggetti colorati sul sensore");

// at "via" you have 15 seconds to place, one after another, the colored objects near sensor

lcd.clear ();

lcd.print F("calibrazione....");

lcd.setCursor (0, 1);

lcd.print F("al via posiziona");

delay (3000);

lcd.clear();

lcd.print F("le palline sul ");

lcd.setCursor (0, 1);

lcd.print F("sensore - 15 sec");

delay (3000);

lcd.clear ();

lcd.print F("via!");

Serial.println F("Via! Calibrazione in corso...."); // "via! calibration in progress...

tempoiniz = millis ();

tempocorr = millis ();

while ((tempocorr - tempoiniz) < 15000) // se non sono passati 15 secondi

{

tempocorr = millis (); // memorizza il momento corrente

lettura (); // lancia la routine di lettura delle frequenze rilasciate dal sensore if (((( freqrosso < minrosson) || (freqrosso > maxrosson)) && ((freqverde < minverden) || (freqverde > maxverden)) && ((freqblu > minblun) || (freqblu < maxblun))))

{ // se la lettura e' valida (se il sensore ha letto un colore) - if reading is ok (if sensor

// has read a color)

tabvalori [freqrosso * 3] = tabvalori[freqrosso * 3] + 1; // conteggia la frequenza del rosso

// - add 1 to red frequency occurencies

tabvalori [(freqverde * 3) + 1] = tabvalori[(freqverde * 3) + 1] + 1; // conteggia la

//frequenza del verde - add 1 to green frequency occurencies

tabvalori [(freqblu * 3) + 2] = tabvalori[(freqblu * 3) + 2] + 1; // conteggia la

// frequenza del blu - add 1 to blue frequency occurencies

contatore ++; // conta le letture valide - counts valid reads

}

}

//

// *************** uscito dalla fase di rilevazione, analizza i dati memorizzati ***************

// *********************** analyzes stored data **********************************************

//

Serial.print F("letture valide :"); // valid reads:

Serial.println (contatore);

minaccett = contatore / 50; // definisce come minimo accettabile il 2% del numero di cicli

for (indice = 1; indice < 250; indice ++) // analizza la tabella delle frequenze per individuare

// la frequenza minima e massima rilevata dai fotorilevatori di ogni colore

{

if (tabvalori [indice * 3] > minaccett) // se le occorrenze rosso > minimo accettabile

{ // if red occurrences is greater than the minimum acceptable

if (indice < minrosso)

minrosso = indice; // memorizza la frequenza (l'indice e' la frequenza, mentre in

// tabella sono presenti le occorrenze memorizes frequency (index is the frequency, while in the

// array there are occurrences)

if (indice > maxrosso)

maxrosso = indice;

}

if (tabvalori [(indice * 3) + 1] > minaccett) // se occorrenze verde >l minimo accettabile

{

if (indice < minverde)

minverde = indice; // memorizza la frequenza (l'indice e' la frequenza, mentre in

// tabella sono presenti le occorrenze

if (indice > maxverde)

maxverde = indice;

}

if (tabvalori [(indice * 3) + 2] > minaccett) // se occorrenze blu > minimo accettabile

{

if (indice < minblu)

minblu = indice; // memorizza la frequenza (l'indice e' la frequenza, mentre in tabella

// sono presenti le occorrenze

if (indice > maxblu)

maxblu = indice;

}

}

// allarga il range delle frequenze minime e massime utilizzate per individuare i colori

// widens range for minimum and maximum frequencies used to identify each color

//Arduino: braccio meccanico e palline colorate – robotic arm and colored balls //Servo, TCS230, color sensor, sensore di colore,

if (minrosso > 3)

minrosso = minrosso - 3;

else

minrosso = 0;

if (maxrosso < 247)

maxrosso = maxrosso + 3;

else

maxrosso = 250;

if (minverde > 3)

minverde = minverde - 3;

else

minverde = 0;

if (maxverde < 247)

maxverde = maxverde + 3;

else

maxverde = 250;

if (minblu > 3)

minblu = minblu - 3;

else

minblu = 0;

if (maxrosso < 247)

maxblu = maxblu + 3;

else

maxblu = 250;

Serial.println F("frequenze minime e massime:");// minimum and maximum frequencies for each color

Serial.print F("rosso: ");

Serial.print (minrosso);

Serial.print F(" ");

Serial.println (maxrosso);

Serial.print F("verde: ");

Serial.print (minverde);

Serial.print F(" ");

Serial.println (maxverde);

Serial.print F("blu: ");

Serial.print (minblu);

Serial.print F(" ");

Serial.println (maxblu);

Serial.println F("sistema pronto; poni un oggetto colorato vicino al sensore"); // system ready;

// put a colored object near sensor

lcd.clear ();

lcd.print F("sistema pronto ");

lcd.setCursor (0, 1);

lcd.print F("selez. servo 6 ");

delay (3000);

lcd.clear ();

Serial.println F("finesetup");

}

//

//

void loop ()

{

tempoinizio = millis();

selezionaservo ();

sele = selezione;

visualizzaimpulsi ();

while (selezione == 6)

{

tempoattuale = millis ();

if ((tempoattuale - tempoinizio) > 3000) // attende 3 secondi per iniziare seq. programmata

{

semaforo = 1; // pone ad 1 il semaforo di sequenza programmata

while (semaforo == 1)

{

riconoscecolore (); //individua il colore della pallina posizionata sul TCS320

sequenzaprogrammata (); // esegue la sequenza programmata per il colore individuato

mossainiziale = 44; // predispone l'indice sulla sequenza di inizio lavoro

sequenzaprogrammata (); // Mette il braccio in posizione di inizio lavoro

}

mossainiziale = 0; // quando non ci sono piu' palline sul lettore

selezione = 0; // forza l'uscita dal loop di sequenza programmata

}

selezionaservo (); // consente l'uscita dal loop di sequenza programmata, nel caso non la si

// voglia eseguire

}

acquisiscigradi ();

visualizzaimpulsi ();

posizionaservo ();

}

//Arduino: braccio meccanico e palline colorate – robotic arm and colored balls //Servo, TCS230, color sensor, sensore di colore,