Codice sorgente del programma scritto in BasicX
'*********************************************************************************
'*********************************************************************************
'**** ******
'**** Programma scritto da Zefferino Pavanelli ******
'**** ******
'*********************************************************************************
'*********************************************************************************
Option Explicit
Dim Pot1 As Single, Pot2 As Single, Pot3 As Single
Dim x2 As Single, y2 As Single, xp As Single, yp As Single
Dim k1 As Single, k2 As Single, k3 As Single
Dim angolod As Single, deltalfa As Single
Dim alfac As Single, alfad As Single, alfa As Single, beta As Single
Dim bool_x As Boolean, bool_y As Boolean, bool_a As Boolean
Const PinPot1 As Byte = 13
Const PinPot2 As Byte = 14
Const PinPot3 As Byte = 15
Const Pi As Single = 3.14159265
Public Sub Main()
debug.print
debug.print " Sub Main() "
debug.print
Call Costanti()
x2 = 10.0
y2 = 55.0
Call Funziona()
debug.print
debug.print " x2= "; cStr(x2); " y2= "; cStr(y2);
debug.print
x2 = -10.0
y2 = 4.0
Call Funziona()
debug.print
debug.print " x2= "; cStr(x2); " y2= "; cStr(y2);
debug.print
x2 = 15.0
y2 = 55.0
Call Funziona()
debug.print
debug.print " x2= "; cStr(x2); " y2= "; cStr(y2);
debug.print
x2 = -20.0
y2 = 32.0
Call Funziona()
x2 = 2.0
y2 = 31.0
Call Funziona()
End Sub
Public Sub Funziona()
bool_x = true
bool_y = true
do
do
Call Alfa_Carrello()
Call Xp_Yp()
Call Angolo_Direzione()
deltalfa = Abs(angolod - alfac)
If (angolod > alfac) Then
' debug.print " angolod > alfac "
Call Ruota_Carrello_S()
end if
If (angolod < alfac) Then
' debug.print " angolod < alfac "
Call Ruota_Carrello_D()
end if
loop While (deltalfa > 0.2)
Call Avanza_Tratto()
if (Abs (x2 - xp) < 5.0) then
bool_x = false
end if
if (Abs (y2 - yp) < 5.0) then
bool_y = false
end if
'debug.print " bool_x= "; cStr(bool_x); " bool_y= "; cStr(bool_y)
loop While (bool_x or bool_y)
End Sub
Sub Costanti()
Call GetAdc(PinPot1, Pot1)
Call GetAdc(PinPot2, Pot2)
Call GetAdc(PinPot3, Pot3)
k1 = Pot1 * 1.5
k2 = Pot3 * 1.5 - 0.5
k3 = Pot2 * 1.5 - 0.5
End Sub
Sub Alfa_Carrello()
Call GetAdc(PinPot1, Pot1)
Call GetAdc(PinPot2, Pot2)
Call GetAdc(PinPot3, Pot3)
alfad = (k1 - Pot1 * 1.5) * Pi
alfa = (Pot3 * 1.5 - k2) * Pi
beta = (Pot2 * 1.5 - k3) * Pi
alfac = Pi - alfa - beta - alfad
'Debug.Print " Pot1= "; cStr(Pot1); " Pot2= "; cStr(Pot2); " Pot3= "; cStr(Pot3)
'Debug.Print "alfac= "; cStr(alfac); " alfad= "; cStr(alfad); " alfa= "; cStr(alfa);
" beta= "; cStr(beta)
End Sub
Sub Xp_Yp()
xp = 32.0 * (1.0 + cos(alfa + beta) - cos(alfa))
yp = 32.0 * (sin(alfa) - sin(alfa + beta))
'Debug.Print " xp= "; cStr(xp); " yp= "; cStr(yp)
End Sub
Sub Angolo_Direzione()
If (x2 <> xp) Then
angolod = Atn ((y2 - yp)/(x2 - xp))
else
if (yp > y2) then
angolod = Pi / 2.0
end if
if (yp < y2) then
angolod = -Pi/2.0
end if
End If
'Debug.Print
'Debug.Print " angolod= "; cStr(angolod)
'Debug.Print
End Sub
Sub Ruota_Carrello_S()
Call PutPin(9, bxOutputLow) 'MD avanti
Call PutPin(10, bxOutputHigh)
Call PutPin(11, bxOutputHigh) 'MS indietro
Call PutPin(12, bxOutputLow)
Delay(0.6 * deltalfa)
Call PutPin(9, bxOutputLow) 'MD fermo
Call PutPin(10, bxOutputLow)
Call PutPin(11, bxOutputLow) 'MS fermo
Call PutPin(12, bxOutputLow)
End Sub
Sub Ruota_Carrello_D()
Call PutPin(9, bxOutputHigh) 'MD indietro
Call PutPin(10, bxOutputLow)
Call PutPin(11, bxOutputLow) 'MS avanti
Call PutPin(12, bxOutputHigh)
Delay(0.6 * deltalfa)
Call PutPin(9, bxOutputLow) 'MD fermo
Call PutPin(10, bxOutputLow)
Call PutPin(11, bxOutputLow) 'MS fermo
Call PutPin(12, bxOutputLow)
End Sub
Sub Avanza_Tratto()
'debug.print " Avanza_Tratto "
'debug.print
If (x2 < xp) then
Call PutPin(9, bxOutputLow) 'MD indietro
Call PutPin(10, bxOutputHigh)
Call PutPin(11, bxOutputLow) 'MS indietro
Call PutPin(12, bxOutputHigh)
Delay(0.1)
Call PutPin(9, bxOutputLow) 'MD fermo
Call PutPin(10, bxOutputLow)
Call PutPin(11, bxOutputLow) 'MS fermo
Call PutPin(12, bxOutputLow)
end if
If (x2 > xp) then
Call PutPin(9, bxOutputHigh) 'MD avanti
Call PutPin(10, bxOutputLow)
Call PutPin(11, bxOutputHigh) 'MS avanti
Call PutPin(12, bxOutputLow)
Delay(0.1)
Call PutPin(9, bxOutputLow) 'MD fermo
Call PutPin(10, bxOutputLow)
Call PutPin(11, bxOutputLow) 'MS fermo
Call PutPin(12, bxOutputLow)
end if
End Sub
Il carrello va posto nella posizione iniziale come mostrato nella visione di insieme; a partire da questa posizione, di coordinate iniziali (0, 32) in centimetri, questo programma fa raggiungere al carrello successivamente la posizione (10,55) in alto a destra del piano di movimento, quindi la posizione (-10,4) in basso a sinistra, quindi torna in alto a destra nella posizione (15,55) e successivamente, con due manovre, retrocede alla posizione (-20,32) ed avanza alla posizione (2,31) ponendosi praticamente nella stessa posizione iniziale di partenza.
Volendo la stampa di alcuni dati che caratterizzano lo stato del sistema è sufficiente togliere il carattere ' davanti alla corrispondente istruzione di Debug.Print; in questo modo da semplice commento viene trasformato in istruzione che comporta la stampa della variabili volute dullo schermo del computer sul quale è installato il compilatore BasicX; questo comporta anche un rallentamento della velocità di controllo del carrello.