Comportamento Robot

Come da regolamento della competizione, nella fase obbligatoria, il robot parte da una posizione ed un’angolazione iniziale note, da questa posizione il robot deve raggiungere l’angolo opposto del campo, di dimensioni 3m x 4m, prendere tutti gli oggetti e portarli nella zona col rispettivo colore; in mezzo al campo sono disposti degli ostacoli con posizione non nota e che devono essere evitati per non incorrere in penalità.

Di seguito abbiamo raggruppato le classi utilizzate per il nostro programma con i relativi attributi e metodi:

Andiamo a fare un Giro

Il nostro approccio, per lo più reattivo, è stato quello di far ruotare il robot fino ad una determinata angolazione tale da farlo direzionare verso la zona target e farlo riorientare se questo esce fuori da un certo range di traiettoria. Per far eseguire questo passo iniziale sfruttiamo il giroscopio; in particolare ci serviamo di una libreria scritta ad hoc per il nostro modulo MPU6050 e che ci restituisce valori negativi decrescenti per rotazioni in senso orario, mentre ci restituisce valori positivi crescenti per rotazioni in senso antiorario.

Abbiamo considerato come 0 del giroscopio il Nord del campo poiché il robot parte proprio in quella direzione, per cui abbiamo calcolato una rotazione oraria di 35° come angolo in cui si deve orientare inizialmente il nostro robot; questo si traduce in un -35° del giroscopio. Come accennato prima, il robot non può mai mantenersi in modo assolutamente preciso in un determinato angolo per cui consideriamo un range che per tutti i nostri angoli è ±10° rispetto all'angolo target.

Il giroscopio ha una classe propria MPU6050, presente nel file MPU6050.py, che implementa tutti i metodi necessari per effettuare l’indirizzamento del modulo ed il calcolo della yaw a partire dai dati grezzi acquisiti. La yaw rappresenta la rotazione che avviene per l’asse Z, ovvero quella che punta verso l’alto, dunque esso ci dà indicazione dell’angolo a cui è rivolto il robot rispetto allo 0 dato dall'orientamento iniziale.

La classe che si occupa di prendere le informazioni necessarie dalla classe MPU6050 è la classe SENSOR presente nel file sensor.py. Questa classe è thredizzata e ciò permette di eseguire tutti i suoi metodi indipendentemente dal resto del robot, per cui il giroscopio aggiornerà in continuazione la yaw e quindi all’occorrenza basta leggere il valore di quell’attributo per sapere in ogni momento l’angolazione del robot.

Poiché per rotazioni in senso orario abbiamo valori negativi e per rotazioni antiorarie valori positivi abbiamo fatto in modo che il range di angoli in cui si può trovare il robot non varia da 0 a 360 bensì da -179 a 180. In questo modo abbiamo risolto il problema del salto 0/360 ma si è introdotto il problema del salto -179/180, ma questo è stato gestito semplicemente assegnando valore -179 ad angoli compresi tra 170 e 180 e quelli compresi tra -169 e -179; lo stesso metodo è stato poi adottato per tutti gli altri angoli noti (0, 45, -45, 90, -90, 135, -135). È rimasto il problema che il robot certe volte esegue il giro lungo per arrivare ad un certo angolo, risolvibile gestendo meglio le rotazioni, ma il tempo non è stato abbastanza.

Pro e contro con la bussola

Abbiamo scelto di utilizzare questo modulo rispetto alla bussola HMC5883L poiché la classe per il giroscopio era già thredizzata molto bene e quindi possiamo avere in ogni momento il valore dell’angolo, inoltre la bussola risente molto degli influssi magnetici esterni e necessita inoltre di essere ogni volta ricalibrata. Di contro la bussola dà sempre angoli assoluti in riferimento al campo magnetico terrestre, mentre il giroscopio fissa lo 0 nel punto di partenza. È possibile un’implementazione ibrida in cui si prende come angolo di partenza quello della bussola e su quello si applica il giroscopio.

Come accennato precedentemente posizioniamo il robot a -35°, facendolo quindi rivolgere verso la zona target, ruotatosi verso quell'angolo il robot comincia a muoversi in avanti.

Come avvengono i movimenti?

Per i movimenti del robot utilizziamo 2 motori DC a 6V; la classe che contiene i metodi necessari a gestire i movimenti è la classe MOTORI. Questa classe contiene innanzitutto 4 attributi che indicano i Pin a cui sono collegati i 4 ingressi del driver, 2 per ogni motore. Utilizziamo la PWM (Pulse With Modulation) che permette di determinare la frequenza di funzionamento dei motori, nel nostro caso 500Hz. Abbiamo dunque i 4 metodi (avanti, indietro, destra, sinistra) che gestiscono separatamente i movimenti delle due ruote per far muovere il robot nel modo desiderato passando un parametro che indica la percentuale di voltaggio che arriva ai motori, solitamente noi usiamo 50 e poiché ai motori arriva una tensione di circa 3,4V a noi girano a circa 1,7V, tranne in alcuni casi in cui usiamo 60 o 70.

Passo dopo passo

Da esperimenti effettuati abbiamo notato che il robot in 0.5 secondi percorreva sempre circa 10cm, per cui abbiamo determinato che il robot viaggiasse ad una velocità lineare di 20cm/s. Questo parametro ci ha permesso di costruire una specie di odometria o più precisamente un cosiddetto dead reckoning che ci ha permesso di determinare in maniera più o meno precisa gli spostamenti del robot e quindi indirettamente possiamo determinare la posizione del robot.

Conoscendo le dimensioni del campo (300cm x 400cm) e conoscendo velocità del robot e l’angolo a cui si trova possiamo trattare queste due ultime informazioni come modulo e fase e quindi comandare al robot dove fermarsi per passare allo stato successivo.

Essendo che il nostro lavora come una macchina a stati è necessario cambiare di volta in volta lo stato; inizialmente avremo state=start e new_state=target.

Ostacoli

In mezzo al campo però sono presenti degli ostacoli e questo complica un po’ le cose poiché il robot deve poter evitare gli ostacoli e riorientarsi verso la zona target.

Per evitare gli ostacoli ci siamo affidati agli infrarossi. Ogni volta che il robot si dirige da qualsiasi stato verso la zona target utilizziamo soltanto i 4 infrarossi posizionati in alto e quindi a seconda di quale infrarosso viene suscitato viene indicata la presenza di un ostacolo in quella direzione e di conseguenza si avrà una rotazione verso una zona priva di ostacoli poiché non segnalati dagli infrarossi. Quando invece siamo diretti verso l’area relativa all’oggetto preso si attivano i 2 infrarossi presenti davanti nella presa. L’altro infrarosso posizionato in basso invece serve per determinare quando l’oggetto può essere preso. La classe infrarossi è IR ed in questa sono semplicemente presenti gli attributi con i Pin a cui sono collegati tutti gli infrarossi ed i metodi setup della GPIO. Ogni infrarosso restituisce 0 se incontra un ostacolo e 1 altrimenti.

È quindi presente la funzione evitaostacoli che gestisce il modo in cui vengono evitati gli ostacoli a seconda dello stato in cui si trova il robot.

Evitati gli ostacoli mi rioriento verso la direzione obiettivo.

Quando fermarsi?

La classe Move è fondamentale in quanto gestisce tutti i movimenti del robot ed effettua i cambi di stato a seconda degli obiettivi raggiunti, inoltre è in un thread a se e quindi è indipendente dal resto. In questa classe è presente il metodo goto che ad ogni stato associa un modulo ed una fase che il robot deve raggiungere. Nel caso iniziale il robot ha una fase di -35° ed un modulo di 220. Raggiunto quel modulo il robot esce dalla fase di goto e passa ad una nuova fase, la cattura.

Cattura degli oggetti

Alla base di questo stato vi è la camera. Questa si accende e si riaccende ad ogni stato consentendo di resettare i parametri precedentemente settati.

La classe che gestisce la camera si chiama Camera ed utilizza la libreria cv2 per eseguire la cattura dei frame e le dovute elaborazioni di ogni frame.

Finita la goto il robot si trova in prossimità della zona target, dunque con gli oggetti abbastanza vicini, quindi la camera può determinare l’oggetto più vicino, questo viene determinato calcolando il minimo tra le y degl’altri oggetti individuati.

Anche la camera, come la Move, ha un thread a se; la camera si occupa di aggiornare delle variabili globali che danno informazioni sul centro e la y dell’oggetto individuato, queste variabili globali vengono lette dalla Move che ne genera un comando opportuno per consentire la cattura o il rilascio dell’oggetto.

Si è subito pensato di gestire l’accesso concorrente a queste variabili e per fare ciò si utilizzano i metodi lock() e release() sempre della libreria threading che consentono l’accesso autonomo alle variabili evitando concorrenza.

Il robot dunque con le informazioni provenienti dalla camera si muoverà in modo tale da mantenere sempre l’oggetto al centro e fintantoché non si illumina l’infrarosso sottostante al primo piano. Non appena le condizioni vengono soddisfatte viene azionata la cattura gestita dal metodo capturePhase(). Questa avviene per mezzo del servo motore, gestito dalla classe SERVO. Il metodo cattura() di questa classe si occupa di far abbassare la presa. Come accade per i motori anche in questa classe abbiamo i Pin a cui sono collegati i due servo motori e i metodi per gestire la PWM, in questo caso 50Hz.

Catturato un oggetto verranno resettate tutte le variabili globali in modo da consentire di ripartire senza informazioni precedenti. Inoltre verrà settata a False la variabile relativa al colore dell’oggetto catturato in modo che questo non venga più cercato successivamente, diminuendo quindi il carico computazionale per la camera.

Dove vado?

Dopo aver catturato un oggetto è fondamentale sapere dove andare per consegnare l’oggetto. Come per lo step precedente, ma con stati cambiati infatti ora lo stato corrente state sarà target mentre il nuovo stato new_state sarà il colore dell’oggetto catturato. Avendo queste informazioni la goto genererà automaticamente modulo e fase e farà sì che il robot si orienti subito verso l’angolo generato e prosegua fintantoché non raggiunge il modulo stabilito; ovviamente come per lo stato iniziale anche qui il robot dovrà evitare gli ostacoli e riorientarsi, ma questa volta aiutato anche dagli infrarossi posti sulla presa.

Rilascio oggetti

Terminata la goto il robot dovrà cercare l’area goal. Di questo si occupa il metodo releasePhase(), sempre della classe Move. Se nessun area di quel colore viene individuata allora il robot si muoverà lentamente a destra finché non la trova; dopo di ciò, sfruttando sempre le informazioni del centro e della Y provenienti dalla camera, il robot si avvicinerà all’area finché non verrà suscitato almeno uno dei due infrarossi posti sulla presa, in quel momento il robot, mediante il metodo libera() della classe SERVO, alzerà la presa e libererà l’oggetto nella corretta posizione resettando al solito tutte le variabili.

Ancora e ancora

Arrivati a questo punto il giro si ripete. Infatti il robot non dovrà fare altro che cambiare stato mettendo come stato corrente il new_state precedente e aggiornare questo mettendo target, quindi ritornare alla zona target e continuare con gli oggetti rimanenti nella stessa identica maniera raccontata sopra, almeno per quanto riguarda i 3 oggetti (parallelepipedo rosso, piramide blu e L gialla).

…e la pillola va giù

Per come sono disposti gli oggetti nella zona target si hanno i 3 oggetti menzionati sopra messi davanti un oggetto a forma di pillola blu posta inizialmente in posizione verticale su di un piedistallo spesso circa 1cm. Catturare questo oggetto è un po’ più complicato in quanto adottando lo stesso metodo di cattura usato per gli altri oggetti potremmo far cadere la pillola fuori dalla presa nel momento della cattura o addirittura non riuscire a prenderla proprio perché fatta cadere troppo lontana prima della cattura.

Per questo motivo abbiamo adottato un metodo di cattura leggermente differente. Esso consiste nell’avvicinarsi finché la testa della pillola non scenda sotto una certa soglia, in quell’istante abbassa la presa, tira un po’ indietro e ruota leggermente a destra per far cadere la pillola all’interno della presa.

Fatto ciò il robot trasporterà la pillola nella zona blu nella stesso modo con cui ha portato l’oggetto blu.

Necessita di discriminare pillola da oggetto blu, per fare ciò abbiamo utilizzato due range di valori diversi in quanto la pillola risulta visivamente più scura rispetto all'oggetto.

La palla

Un altro oggetto da portare è una palla verde. La difficoltà maggiore del catturare questo oggetto sta nel fatto che non si trova nella zona target con tutti gli altri oggetti. Essa infatti è posizionata in mezzo al campo in una posizione non nota, ma questo oggetto può essere trasportato in una qualsiasi delle altre aree.

La nostra strategia presuppone che questo oggetto venga lasciato per ultimo e quindi dopo che tutti gli altri oggetti sono andati a buon fine.

Inizialmente avevamo costruito un mondo a griglia, con celle di dimensioni pari allo spazio percorso dal robot in mezzo secondo, ovvero 10 cm. Ne era venuta fuori una matrice 30x40 su cui eravamo riusciti a tracciare, in maniera più o meno approssimativa, i movimenti del robot. Con questo tipo di approccio, potendo restituire di volta in volta X e Y correnti tramite il metodo currPos() di Move e avendo la camera sempre accesa, avremmo potuto determinare e memorizzare sulla mappa X, Y e angolo della palla se incontrata lungo il tragitto e alla fine saremmo potuti tornare in quella posizione per prendere la palla. Non abbiamo più continuato per questa via in quanto questo avrebbe richiesto più tempo e molti più test e la data della presentazione del progetto era sempre più vicina. Sono comunque presenti nel codice dei blocchi riguardanti il mondo a griglia e la determinazione della posizione corrente; possono servire per sviluppi futuri.

Il metodo adottato alla fine è stato qualcosa un po’ alla carlona. Dopo aver consegnato l’ultimo oggetto il robot esegue la goto verso la zona target; arrivato alla zona target si attiva un’altra goto che lo porta verso una zona centrale del campo, da quel punto inizia a girare sperando di trovare la famigerata palla, se esse viene vista allora si dirige per catturarla altrimenti prosegue avanti e continua a cercarla.

In questo momento il robot si trova in uno stato undefined quindi ammesso che trovi la palla poi dovrà saper cercare la porta. Abbiamo dato preferenza alle aree verde-blu, quindi il robot da quello stato dovrà dirigersi verso quelle due aree poste vicine e rilasciare l’ultimo oggetto.

Se alla fine verrà consegnata pure la palla il robot eseguirà una simpatica esultanza, e probabilmente pure noi.

Metodo tralasciato A*

Per individuare gli ostacoli utilizzavamo una combinazione di dati ricevuti da sonar e infrarossi. Il sonar ci dava un’informazione circa la distanza dall'ostacolo e questo ci permetteva di mapparlo in una data cella, ma poiché il sonar dà risultati errati se posto non perpendicolare all'ostacolo utilizzavamo gli infrarossi come campanellino d’allarme se il robot si fosse avvicinato troppo all'ostacolo a causa di una lettura errata del sonar.

Il robot in questa maniera avrebbe perso un po’ di tempo all'inizio per scandagliare il campo prima di arrivare alla zona target o alle aree goal, ma se i dati fossero stati inseriti correttamente avrebbe impiegato molto meno tempo per ritornare poiché avrebbe già saputo le mosse da fare per evitare gli ostacoli.

Questo metodo è stato lasciato incompleto, seppur eravamo riusciti a far mappare gli ostacoli ad angolazioni diverse, poiché è molto rischioso e non avendo modo di misurare in maniera abbastanza precisa gli spostamenti del robot avremmo avuto problemi di accumulo di errore.

Sicuramente questo metodo è molto più elaborato di quello da noi utilizzato e non si escludono esperimenti futuri al riguardo.

Risultati ottenuti

I risultati ottenuti da alcuni nostri test svolti ci hanno dato solitamente risultati soddisfacenti a testimonianza del fatto che il robot era in grado di raggiungere gli obbiettivi descritti.

Di seguito una tabella riepilogativa di alcuni test:

In media quindi il nostro robot è riuscito a trasportare almeno 4 oggetti su 5 mantenendosi sotto la soglia di tempo previsto, sfruttando talvolta i timeout per correggere qualche errore.