Navigazione intelligente

Navigazione Intelligente

Applicabile a robot con Basic Stamp)

Stufo di veder gironzolare a caso, il robot per la casa, ho cercato di definire prima, e programmare poi, quella che chiamo navigazione Intelligente.
Ho lasciato perdere i vari concetti per eseguire una mappatura dell’ambiente, questa richiede troppo tempo per definirla e costringe il robot a navigare solo in ambienti noti.

Mi sono posto questi obbiettivo:
Definite le coordinate iniziali x,y, il robot deve essere in grado di raggiungere le coordinate di un obbiettivo x1,y1, nel modo più diretto, ovviamente aggirando tutti gli ostacoli che incontra sul percorso diretto.

Per ottenere questo risultato il robot deve:
    1)       Dopo ogni movimento eseguito, deve essere in grado di conoscere le coordinate di dove si trova.
    2)       Calcolare continuamente la direzione ottimale e la distanza dall’obbiettivo.
    3)       Il tutto realizzato via software. (In seguito si potrà implementare con eventuale bussola).

Definizione delle grandezze in gioco:
Ruote con servo motori.
1 impulso servo corrisponde a 2,8mm percorsi.
Lunghezza asse delle ruote = 148 mm.
Centro di riferimento della posizione: punto centrale dell’asse delle ruote.
5 byte per contenere le coordinate di posizionamento:
    1 word per l’asse X (può contenere sino a + - 32768 inpulsi, corrispondenti a 97,750 metri.)
    1 word per l’asse Y (stessi valori di X).
    1 byte per i gradi.
Altri 5 byte per le coordinate da raggiungere.

Apro subito una parentesi per i gradi: Il PBasic utilizza un solo byte per contenere i gradi, quindi l’angolo giro è di 256 gradi non di 360. Questo non porta alcuna difficoltà pratica, ma nei calcoli successivi, bisogna ricordare la conversione: 1 angolo Pbasic = angolo/360*256. (un angolo standard di 90 corrisponde ad un angolo PBasic di 64).

Abbiamo definito quindi che il ‘mondo’ esplorabile intelligentemente dal nostro Robot è un’area di 97,750+97,750 = 183,5 metri con le coordinate iniziali 0,0 poste nel punto iniziale di accensione del robot. La precisione all’interno di questa area sarà di 2,8 mm..

Prerequisiti:
Per utilizzare praticamente queste idee ed adattarle al vostro programma dovete:

Disporre di due microprocessori: io utilizzo un BS2 (sleave) per la lettura degli encoders ed il calcolo della posizione, ed un BS2E (master) per il calcolo della direzione e la gestione vere e propria dei motori delle ruote.
Lo spazio di programma necessario per le righe di istruzione per i calcoli di posizionamento e determinazione della posizione da raggiungere occupano più di uno slot di 2k, per cui è impossibile utilizzare un semplice BS2, senza considerare anche i problemi di tempificazione per il continuo monitoraggio degli encoders.
Le variabili da utilizzare sono molte e lo spazio del solo Bs2 non sono sufficienti.
Dovete avere già perfettamente funzionante un buon programma di navigazione, cioè un Robot che non urta, ma evita tutti gli ostacoli, frontali, laterali e posteriori. Non utilizzando una bussola, il posizionamento è calcolato solo via software e l’urto contro ostacoli, produrrebbe un errore sulle coordinate di posizionamento, dovuto allo slittamento delle ruote.

Calcolo Posizione (sul BS2 sleave)
Esistono tre diversi tipi di movimento, per ognuno dei quali si devono usare calcoli diversi:

Lineare, (direzione fissa senza alcuna rotazione)
Svolta, (una ruota gira e l’altra è ferma).
Rotazione (una ruota avanti e l’altra indietro).

Per ciascun movimento descrivo il calcolo teorico e come applicarlo alla programmazione in PBasic.

Movimento Lineare.
Le coordinate di partenza sono 0,0 ed a gradi 0. (la direzione di gradi 0 è verso le ore 12 dell’orologio).
Il movimento lineare è il più semplice, le coordinate al termine di un movimento sono date da:

Asse Y = Asse Y iniziale + distanza * cos(gradi)
Asse X = Asse X iniziale + distanza * sen(gradi)

Dove distanza è = il numero di impulsi mandati al servo * 2,8 mm., se il movimento ruote è all’indietro la distanza è negativa. Gradi sono i gradi di posizionamento rispetto alle ore 12.

Traduzione in PBasic.
Le considerazioni da tenere sempre presenti sono molte:

Il PBasic non gestisce i decimali
Bisogna sempre tenere conto della massima capacità della variabile che si usano.
Eseguire moltiplicazioni e divisioni con i numeri assoluti ed aggiungere il segno alla fine.
L’angolo giro è di 256 gradi.
I valori di seno e coseno sono espressi in numeri da -127 a +127 e non da -1 a +1.

Le due semplici espressioni per il movimento lineare vengono, purtroppo così tradotte in PBasic:

sign = DeltaAsseY.BIT15
dist = DeltaAsseX

DeltaAsseY = COS(gradi)
sign = DeltaAsseY.BIT15^dist.BIT15
DeltaAsseY = ABS dist*DeltaAsseY
DeltaAsseY = ABS DeltaAsseY/127 '*28/10
IF sign=1 THEN DeltaAsseY=-DeltaAsseY

DeltaAsseX = SIN(gradi)
sign = DeltaAsseX.BIT15^dist.BIT15
DeltaAsseX = ABS dist*DeltaAsseX
DeltaAsseX = ABS DeltaAsseX/127 '*28/10
IF sign=1 THEN DeltaAsseX=-DeltaAsseX

AsseX = AsseX + DeltaAsseX
AsseY = AsseY + DeltaAsseY

Svolta (una ruota gira e l’altra è ferma)
E’ come percorrere un arco di cerchio avente il raggio uguale all’asse tra le due ruote.
Questi i calcoli da considerare:
Impulsi = numero di impulsi al servo.
Circonferenza = 2 * PI * raggio = 2* 6,28* 148 = 930
Arco = Impulsi * 2,8
formule complete:
DeltaGradi=256*dist/circ
Gradi=Gradi+DeltaGradi
DeltaAsseX = rag/2 * sen(Gradi-DeltaGradi) - rag/2 * cos(Gradi) 'svolta Sx
DeltaAsseY = rag/2 * sen(Gradi-DeltaGradi) - rag/2 * sen(Gradi)
DeltaAsseX e DeltaAsseY invertiti per svolta Dx (dx centro)
AsseX = AsseX + DeltaAsseX
AsseY = AsseY + DeltaAsseY

Traduzione in PBasic:

DirSvoltaSx:
    dist = DeltaAsseX
    GOTO DirSvolta

DirSvoltaDx:
    dist = DeltaAsseY
    GOTO DirSvolta

DirSvolta:
    sign = dist.BIT15
    DeltaGradi = ABS dist*/$E4 '256/circ = 256/228 = 0,8888 = $00e4
    IF sign=1 THEN DeltaGradi=256-DeltaGradi
    IF moto=MAvantiSx OR moto=MIndietroSx THEN DeltaGradi=256-DeltaGradi 'inverte gradiV per avanti Sx
    Gradi = Gradi+DeltaGradi

    DeltaAsseX = (hraggio*COS(Gradi-DeltaGradi)) - (hraggio*COS(gradi))
    sign=DeltaAsseX.BIT15: DeltaAsseX=ABS DeltaAsseX/127 :IF sign=1 THEN DeltaAsseX=-DeltaAsseX

    DeltaAsseY = (hraggio*SIN(gradi)) - (hraggio*SIN(Gradi-DeltaGradi))
    sign=DeltaAsseY.BIT15: DeltaAsseY=ABS DeltaAsseY/127: IF sign=1 THEN DeltaAsseY=-DeltaAsseY

    IF moto=MAvantiSx THEN DeltaAsseX=-DeltaAsseX: DeltaAsseY=-DeltaAsseY
    AsseX = AsseX + DeltaAsseX
    AsseY = AsseY + DeltaAsseY

Rotazione (una ruota avanti e l’altra indietro)

formule complete:
dist=(DeltaAsseY-DeltaAsseX)/2
DeltaGradi=256*dist/gcirc
Gradi=Gradi+DeltaGradi

Traduzione in PBasic:

dist = ABS (DeltaAsseY-DeltaAsseX)/2
DeltaGradi = ABS dist*/$01c7 '256/gcirc = 256/144 = 1,7777 = $01c7
IF moto = MGiraSx THEN DeltaGradi=-DeltaGradi
Gradi = Gradi+DeltaGradi

Calcolo Direzione ottimale da prendere (calcolo eseguito sul BS2E master)

trova distanza & gradi
formula ideale: distanza=SQR(asseX^2+asseY^2)
formula ridotta: distanza = maggiore differenza tra assi
formula ideale: gradi=ASIN(assY/distanza)

Traduzione in PBasic:

'Trova differenza X
    GET MemAsseXLow, AsseX.LOWBYTE
    GET MemAsseXHigh, AsseX.HIGHBYTE
    GET MemPuntoXLow, work4.LOWBYTE
    GET MemPuntoXHigh, work4.HIGHBYTE
    asseX = work4-AsseX

    'Trova differenza Y
    GET MemAsseYLow, AsseY.LOWBYTE
    GET MemAsseYHigh, AsseY.HIGHBYTE
    GET MemPuntoYLow, work4.LOWBYTE
    GET MemPuntoYHigh, work4.HIGHBYTE
    asseY = work4-AsseY
    IF Flag.BIT0 = 1 THEN 'Trace on (debug)
        DEBUG "Delta Ob. x,y: ", SDEC asseX, ",", SDEC asseY
    ENDIF

CalcoloDirezione:
    IF ABS asseX<ABS asseY THEN
        Distanza = ABS AsseY
        DeltaGradi=ABS asseX*32/ABS asseY
    ELSE
        Distanza = ABS asseX
        DeltaGradi=ABS asseY*32/ABS asseX
        DeltaGradi=64-DeltaGradi
    ENDIF

    'trova quadrante (1/4)
    stato=0
    stato.BIT0 = asseY.BIT15
    stato.BIT1 = asseX.BIT15
    LOOKUP stato, [1,2,4,3], quadrante
    SELECT quadrante
    CASE 2
        DeltaGradi=96
    CASE 3
        DeltaGradi=160
    CASE 4
        IF ABS asseX<ABS asseY THEN
            DeltaGradi=ABS asseX*32/ABS asseY
            DeltaGradi=256-DeltaGradi
        ELSE
            DeltaGradi=ABS asseY*32/ABS asseX
            DeltaGradi=196+DeltaGradi
        ENDIF
    ENDSELECT
    DeltaGradi = DeltaGradi-gradi
    PUT MemDistanzaLow,Distanza.BYTE0: PUT MemDistanzaHigh,Distanza.BYTE1
    PUT MemPuntoGradi, DeltaGradi
    IF Flag.BIT0 = 1 THEN     'debug
        DEBUG " Dist. Ob.: ", DEC Distanza,", gradiOb: ", DEC DeltaGradi, " quad: ", DEC quadrante,CR,CR
    ENDIF
    IF Distanza < 500 THEN CntSt = 10
    'Verifica se l'obbiettivo è stato raggiunto
    IF Distanza > 150 THEN DirCkGradi
        DEBUG "Obbiettivo raggiunto a: ",DEC Distanza,CR
    FlagObbiettivi=FlagObbiettivi-1
    'GOSUB suonoBip: GOSUB suonoBip:
    defmoto = MFerma
    PUT MemRtnGo, $46 :RUN 4 'suona roulette

DirCkGradi:
    IF DeltaGradi < 65 AND DeltaGradi > 20 THEN defmoto=MAvantiDx: RETURN
    IF DeltaGradi < 129 AND DeltaGradi > 64 THEN defmoto=MgiraDx: RETURN
    IF DeltaGradi < 236 AND DeltaGradi > 191 THEN defmoto=MAvantiSx: RETURN
    IF DeltaGradi < 192 AND DeltaGradi > 128 THEN defmoto=MgiraSx: RETURN
    'debug "Direz gradi OK: ",dec gradiV,cr
    defmoto=MAvanti
    RETURN

Chiaro ? ....

(comunque il progetto è finito e funziona!!!)