Solci Riccardo
Classe 5 TA
06/06/2012
RELAZIONE PROGRAMMAZIONE BRACCIO ROBOTICO
-Materiali:
Il braccio robotico “AL5C” della Lynxmotion e' realizzato in alluminio anodizzato, con componenti in plastica. permette il
movimento di cinque servomotori: della base, della spalla, del gomito, del polso e della pinza. Questi servomotori
lavorano su cuscinetti che diminuiscono lo sforzo del sistema, permettendo movimenti precisi.
Il braccio robotico può essere utilizzato con il software RIOS (Robotic arm Interactive Operating System) tramite il servo
controller SSC-32. Questo programma, utilizza gli ingressi digitali e analogici per progetti che prevedono un controllo ad
anello chiuso. Vengono supportati in Basic i cicli If-then, for-next e do-while;
Per la nostra prova abbiamo utilizzato Visual Basic per la realizzazione del programma; tramite il cavo seriale abbiamo
immesso il programma creato nella scheda SSC-32, la quale fornisce le istruzioni da eseguire al braccio robotico.
Grazie al motore per la cinematica inversa, il braccio raggiunge facilmente l’obiettivo prestabilito.
-Board di controllo:
La scheda SSC-32, e' un controller seriale per servomotori, con alcune eccezionali caratteristiche, Ha una elevata
risoluzione (1uS) per un posizionamento accurato e dei movimenti fluidi. Il range e' da 0.75uS a 2.25uS per una
escursione di ben 170°. Il controllo del movimento puo' essere: risposta immediata, controllata in velocita', temporizzata,
o una combinazione. Una delle principali caratteristiche e' il 'Group Move' permette di comandare un gruppo di servi in
modo
sincronizzato,
anche
se
questi
servi
devono
muoversi
per
distanze
differenti.
Ogni uscita puo' essere utilizzata anche come uscita TTL. Ci sono 4 porte di Input che possono funzionare anche in
modalita' Latched, quindi non c'e' il rischio di perdere un evento di breve durata. Ci sono 3 connettori per l'alimentazione,
un connettore DB9F per la connessione diretta alla porta del PC.
-Interfaccia seriale:
L’interfaccia seriale EIA RS-232 asincrona è utilizzata per lo scambio di dati tra dispositivi digitali a bassa velocità.
Collegando due apparecchiature elettroniche con porta RS-232 è possibile realizzare delle comunicazioni tra di loro.
Il termine seriale significa che i bit che costituiscono l’informazione sono trasmessi uno alla volta su di un solo filo. Al
contrario della trasmissione in parallelo, dove i dati sono trasmessi nello stesso istante su più fili.
La velocità della trasmissione dei dati dipende dalle tecnologie adottate: per esempio in una fibra ottica, in un
cavo ethernet o USB i dati viaggiano a velocità elevate. Asincrono significa che i dati sono trasmessi un byte alla volta
anche non consecutivamente e senza un segnale per la sincronizzazione della trasmissione con la ricezione. Per
interpretare i dati, sia il trasmettitore sia il ricevitore devono avere un segnale di clock sincronizzato alla prima transizione
della linea dati.
-Obiettivi della prova:
1) Lo scopo principale della prima parte della prova è quello di far raggiungere al braccio robotico un punto stabilito,
indicando, mediante il programma scritto in Basic, le coordinate X, Y del punto stesso, senza però, modificare la
posizione della pinza.
Per fare ciò è necessario essere a conoscenza di due teoremi:
-Il teorema di Pitagora:
-In ogni triangolo rettangolo, l'area del quadrato costruito sull'ipotenusa è equivalente alla somma delle aree dei quadrati
costruiti sui cateti.
-Il teorema di Carnot:
-In un triangolo qualunque, il quadrato della misura di un lato è uguale alla somma dei quadrati degli altri due diminuita
del doppio prodotto di questi due per il coseno dell'angolo opposto al primo lato.
Questi teoremi vengono applicati nel seguente modo:
Il teorema di pitagora serve per calcolare la distanza dell’oggetto dal centro della struttura, cioè dal punto 0.
Il teorema di Carnot permette di trovare gli angoli tra i lati che formano la struttura, conoscendo le misure dei lati stessi.
L’angolo alfa è l’angolo presente tra L1 e il piano; l’angolo beta è l’angolo tra L1 e L2; l’angolo gamma è l’angolo tra L3 e
il piano che, però, non viene calcolato perché inutile per raggiungere il punto indicato.
Bisogna, inoltre, calcolare l’angolo che si formerà rispetto l’asse Y, quando il braccio raggiungerà le coordinate indicate.
Questo angolo indica la rotazione che deve effettuare la base per raggiungere il punto stabilito.
2) Lo scopo della seconda parte della prova, invece, è quello di far sì che, modificando il programma scritto seguendo gli
obiettivi della prima parte, il braccio si disponga in modo tale da lasciare lo spazio dal piano di appoggio affinché la pinza
abbia la possibilità di muoversi liberamente per afferrare un eventuale oggetto.
3) Nell’ultima parte della prova si giunge al reale scopo dell’intera esercitazione: cioè l’acquisizione e il riposizionamento
di uno o più oggetti. Questo obiettivo consiste nell’utilizzare una webcam che acquisisce l’immagine del piano d’appoggio
e degli oggetti da riposizionare, dopodiché l’immagine ricavata viene immessa nel Form attraverso l’utilizzo di una
PictureBox; a questo punto il programma, seguendo le istruzioni che gli abbiamo dato, esegue un controllo di ogni pixel
dell’immagine, riuscendo a riconoscere il colore degli oggetti da afferrare e quindi a riposizionarli nell’apposita locazione.
-Schema del braccio:
-Tramite il Teorema di Pitagora si possono calcolare i valori di d e d1;
-i valori di Gamma1, beta e alfa sono ricavati applicando il teorema di Carnot (Vedi sopra);
-il valore di Gamma2 si ottiene utilizzando le formule trigonometriche.
-Cinematica inversa:
Tramite la cinematica inversa si può raggiungere un punto stabilito, solo in base al posizionamento delle estremità. È
molto importante in robotica, dove le braccia meccaniche vengono controllate tramite angoli delle articolazioni, senza
guidare manualmente ogni articolazione del robot.
Un braccio robotico è formato da una serie di segmenti rigidi connessi tra loro per mezzo di articolazioni. Al variare degli
angoli delle articolazioni si ottengono diverse configurazioni. Il problema principale sta nel trovare gli angoli giusti per
ottenere l'impostazione che si desidera dare al robot.
.
-Struttura sistema:
Il braccio è formato da cinque servomotori che servono per muovere le sue cinque parti mobili, che sono:
Base
Spalla
Gomito
Polso
Pinza
I servomotori sono dispositivi che si muovono grazie ad un segnale PWM (Pulse Width Modulation), cioè ruotano di n
gradi in base alla lunghezza dell’impulso, espresso in mS. La taratura effettuata durante la prima parte della prova è
servita, appunto, per conoscere la lunghezza dell’impulso in base al quale il servomotore si posiziona ad una certa
angolazione.
I collegamenti tra le varie parti mobili ( chiamati link), sono lunghi 15 cm. Di fronte al braccio è posto, ad una distanza di
9 cm dal centro della base, un foglio sul quale vi è disegnata una scacchiera, utile a verificare se le coordinate che ha
raggiunto il braccio durante l’esecuzione del programma, sono uguali a quelle immesse nel Form.
Il Form del programma creato è composto da tre pulsanti e cinque Textbox:
Per la prima parte di programma vengono utilizzati due pulsanti, uno per eseguire il movimento, e l’altro per far
tornare il braccio alla posizione di riposo, e tre Textbox, uno in cui verrà indicato il motore da muovere, uno per
indicare il numero di impulsi che occorrono per muovere il motore e l’ultimo per indicare il tempo di esecuzione
del programma.
Per la seconda parte del programma viene, invece utilizzato un solo pulsante, per eseguire il movimento e due
Textbox, uno per indicare la coordinata X e l’altro per indicare la coordinata Y.
Infine, per immettere il programma nella Board del braccio robotico, viene utilizzato un cavo seriale che collega il
computer al braccio e nel Form viene impostata la velocità della Porta a 115200.
-Braccio robotico AL5C
-Descrizione del programma:
Public Class Form1
Dim angolo01, angolo02, impulso01,
Dim angolo11, angolo12, impulso11,
Dim angolo21, angolo22, impulso21,
Dim angolo31, angolo32, impulso31,
Dim M0 As Integer
Dim AB, BC, AC As Integer
Dim impulso As Integer
Dim Fi As Single
Dim angolo As Integer
impulso02
impulso12
impulso22
impulso32
As
As
As
As
Integer
Integer
Integer ‘Dichiarazione dei valori usati
Integer
Private Sub Btnmuovi_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Btnmuovi.Click
Dim comando As String
angolo01 = -90
angolo02 = 90
impulso01 = 2465
impulso02 = 670
angolo11 = 0
angolo12 = 90
impulso11 = 640
impulso12 = 1480
‘Valori della taratura, cioè il numero di impulsi necessari
affinchè il motore selezionato si sposti, per esempio, di 90
gradi
angolo21 = 90
angolo22 = 180
impulso21 = 1540
impulso22 = 600
angolo31 = -90
angolo32 = 90
impulso31 = 2410
impulso32 = 643
angolo = Convert.ToInt32(txtangolo.Text)
If txtmotore.Text = "0" Then
M0 = (impulso02 - impulso01) / (angolo02 - angolo01)
impulso = impulso01 + M0 * (angolo - angolo01)
End If
If txtmotore.Text = "1" Then
M0 = (impulso12 - impulso11) / (angolo12 - angolo11)
impulso = impulso11 + M0 * (angolo - angolo11)
End If
‘In base al motore selezionato e
numero di impulsi immesso, il
braccio si sposta in base alle
tarature.
If txtmotore.Text = "2" Then
M0 = (impulso22 - impulso21) / (angolo22 - angolo21)
impulso = impulso21 + M0 * (angolo - angolo21)
End If
If txtmotore.Text = "3" Then
M0 = (impulso32 - impulso31) / (angolo32 - angolo31)
impulso = impulso31 + M0 * (angolo - angolo31)
End If
Porta.Open() ‘Apre la porta per la scrittura del programma
comando = "#" & txtmotore.Text & "P" & Convert.ToString(impulso) & " T1000" & vbCr
‘Comando da eseguire
Porta.Write(comando) ‘Scrittura del programma sulla porta
Porta.Close()
‘Chiusura dela porta
End Sub
Private Sub btnriposo_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles btnriposo.Click
Dim comando As String
Porta.Open()
comando = "#0 P1560 T1000" & vbCr
Porta.Write(comando)
comando = "#1 P1500 T1000" & vbCr
Porta.Write(comando)
comando = "#2 P2000 T1000" & vbCr
Porta.Write(comando)
comando = "#3 P2100 T1000" & vbCr
Porta.Write(comando)
comando = "#4 P2600 T1000" & vbCr
Porta.Write(comando)
Porta.Close()
End Sub
‘Istruzioni che portano il braccio alla posizione
di riposo
Private Sub Form1_Load(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles MyBase.Load
angolo01 = -90
angolo02 = 90
impulso01 = 2465
impulso02 = 670
angolo11 = 0
angolo12 = 90
impulso11 = 640
impulso12 = 1480
angolo21 = 90
angolo22 = 180
impulso21 = 1540
impulso22 = 600
angolo31 = -90
angolo32 = 90
impulso31 = 2410
impulso32 = 643
End Sub
Private Sub Button3_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button3.Click
Dim comando As String
Dim cosa, cosb As Single
Dim alfa, beta As Single
Dim x, y As Single
Dim disfoglio As Single
Dim alfadeg, betadeg, fideg As Single
AB = 15
BC = 15
disfoglio = 9
‘Lunghezza dei bracci AB e BC, e distanza del foglio dalla base
y = Convert.ToSingle(txty.Text)
x = Convert.ToSingle(txtx.Text)
AC = Math.Sqrt((y + disfoglio) ^ 2 + x ^ 2)
cosa = ((BC) ^ 2 - (AC) ^ 2 - (AB) ^ 2) / (-2 * (AB) * (AC))
cosb = ((AC) ^ 2 - (BC) ^ 2 - (AB) ^ 2) / (-2 * (AB) * (BC))
‘Applicazione del teorema
di Carnot
Fi = Math.Tan(x / (y + disfoglio))
Fi = Math.Atan(Fi)
alfa = Math.Acos(cosa)
beta = Math.Acos(cosb)
alfadeg = 360 * alfa / 6.28
betadeg = 360 * beta / 6.28
fideg = 360 * Fi / 6.28
Porta.Open()
'angolo della spalla
'angolo del gomito
‘Conversione degli angoli in gradi
‘Apertura della porta per la scrittura del programma
M0 = (impulso02 - impulso01) / (angolo02 - angolo01) ‘Istruzioni per il movimento della
impulso = impulso01 + M0 * (fideg - angolo01)
base
comando = "#" & "0" & "P" & Convert.ToString(impulso) & " T1000" & vbCr
Porta.Write(comando) ‘scrittura sulla porta del programma da eseguire
M0 = (impulso12 - impulso11) / (angolo12 - angolo11) ‘Istruzioni per il movimento della
impulso = impulso11 + M0 * (alfadeg - angolo11)
spalla
comando = "#" & "1" & "P" & Convert.ToString(impulso) & " T1000" & vbCr
Porta.Write(comando)
M0 = (impulso22 - impulso21) / (angolo22 - angolo21) ‘Istruzioni per il movimento del
impulso = impulso21 + M0 * (betadeg - angolo21)
gomito
comando = "#" & "2" & "P" & Convert.ToString(impulso) & " T1000" & vbCr
Porta.Write(comando)
Porta.Close() ‘Chiusura della porta
End Sub
‘Termine delle istruzioni del programma scritto
‘Tramite la precedente serie di istruzioni si riesce a posizionare il braccio in modo che
corrisponda alle coordinate x e y immesse nei TextBox del Form
‘In seguito vengono dichiarate le variabili per l’acquisizione delle immagini
Const
Const
Const
Const
WM_CAP As Short = &H400S
WM_CAP_DRIVER_CONNECT As Integer = WM_CAP + 10
WM_CAP_DRIVER_DISCONNECT As Integer = WM_CAP + 11
WM_CAP_EDIT_COPY As Integer = WM_CAP + 30
Const
Const
Const
Const
Const
Const
Const
Const
Const
WM_CAP_SET_PREVIEW As Integer = WM_CAP + 50
WM_CAP_SET_PREVIEWRATE As Integer = WM_CAP + 52
WM_CAP_SET_SCALE As Integer = WM_CAP + 53
WS_CHILD As Integer = &H40000000
WS_VISIBLE As Integer = &H10000000
SWP_NOMOVE As Short = &H2S
SWP_NOSIZE As Short = 1
SWP_NOZORDER As Short = &H4S
HWND_BOTTOM As Short = 1
Dim iDevice As Integer = 0
Dim hHwnd As Integer
Declare Function SendMessage Lib "user32" Alias "SendMessageA" (ByVal hwnd As Integer,
ByVal wMsg As Integer, ByVal wParam As Integer, ByVal lParam As Object) As Integer
Declare Function SetWindowPos Lib "user32" Alias "SetWindowPos" (ByVal hwnd As
Integer, ByVal hWndInsertAfter As Integer, ByVal x As Integer, ByVal y As Integer, ByVal
cx As Integer, ByVal cy As Integer, ByVal wFlags As Integer) As Integer
Declare Function DestroyWindow Lib "user32" (ByVal hndw As Integer) As Boolean
Declare Function capCreateCaptureWindowA Lib "avicap32.dll" (ByVal lpszWindowName As
String, ByVal dwStyle As Integer, ByVal x As Integer, ByVal y As Integer, ByVal nWidth As
Integer, ByVal nHeight As Short, ByVal hWndParent As Integer, ByVal nID As Integer) As
Integer
Declare Function capGetDriverDescriptionA Lib "avicap32.dll" (ByVal wDriver As Short,
ByVal lpszName As String, ByVal cbName As Integer, ByVal lpszVer As String, ByVal cbVer
As Integer) As Boolean
Private Sub LoadDeviceList()
Dim strName As String = Space(100)
Dim strVer As String = Space(100)
Dim bReturn As Boolean
Dim x As Integer = 0
Do
bReturn = capGetDriverDescriptionA(x, strName, 100, strVer, 100)
If bReturn Then lstDevices.Items.Add(strName.Trim)
x += 1
Loop Until bReturn = False
End Sub
‘Tramite le seguenti istruzioni si effettua l’acquisizione dell’immagine
Private Sub OpenPreviewWindow()
Dim iHeight As Integer = PicCapture.Height
Dim iWidth As Integer = PicCapture.Width
hHwnd = capCreateCaptureWindowA(iDevice, WS_VISIBLE Or WS_CHILD, 0, 0, 640, 480,
PicCapture.Handle.ToInt32, 0)
If SendMessage(hHwnd, WM_CAP_DRIVER_CONNECT, iDevice, 0) Then
SendMessage(hHwnd, WM_CAP_SET_SCALE, False, 0)
SendMessage(hHwnd, WM_CAP_SET_PREVIEWRATE, 66, 0)
SendMessage(hHwnd, WM_CAP_SET_PREVIEW, True, 0)
SetWindowPos(hHwnd, HWND_BOTTOM, 0, 0, PicCapture.Width, PicCapture.Height,
SWP_NOMOVE Or SWP_NOZORDER)
Else
DestroyWindow(hHwnd)
End If
Private Sub PicCopia_MouseMove(ByVal sender As Object, ByVal e As
System.Windows.Forms.MouseEventArgs) Handles PicCopia.MouseMove
On Error GoTo errore
Dim nomevar As New Bitmap(PicCopia.Image)
Dim colore As Color
Dim rosso As Integer
Dim verde As Integer
Dim blu As Integer
colore = (nomevar.GetPixel(e.X, e.Y))
rosso = colore.R
verde = colore.G
blu = colore.B
LblR.Text = rosso
LblG.Text = verde
LblB.Text = blu
‘L’immagine viene acquisita e posizionata
in due PictureBox, una rappresenta l’immagine
in tempo reale, mentre l’altra proietta
l’immagine aggiornata ogni secondo.
Dall’immagine nelle PictureBox,
il programma controlla il
colore di ogni pixel.
lblMouseX.Text = e.X
lblMouseY.Text = e.Y
Exit Sub
Resume uscita
End Sub
Private Sub btnDisegna1_Click(ByVal sender As System.Object, ByVal e As
System.EventArgs)
Dim PennaRossa As New Pen(Color.Red, 1)
Dim g As Graphics
g = PicCopia.CreateGraphics
g.FillRectangle(Brushes.Blue, 10, 10, 100, 100)
End Sub
End Class
‘Indica il termine di tutte le istruzioni, quindi del programma.
-Conclusioni:
Grazie a questa prova siamo riusciti a capire meglio il funzionamento del braccio robotico e cosa si può ottenere da esso
con la giusta programmazione. Inoltre abbiamo raggiunto le basi per realizzare in futuro programmi più complessi e
raggiungere obiettivi più importanti.
Durante la realizzazione dell’esperienza ci sono state alcune difficoltà, superate, comunque, senza grandi problemi.
