Self-balancing robot

Progetto per il corso di Sistemi Embedded e Real-Time

Giulia Calanca
Stefano Dottore
Un po’ di teoria

● Il problema simile a quello del pendolo inverso, un

sistema instabile per definizione
● L’equilibrio si ottiene mantenendo il centro di massa del
pendolo esattamente sopra il suo fulcro (𝜃=0)
● Bisogna conoscere: la direzione, l’inclinazione e la
velocità di caduta del robot per muovere i motori e
ottenere l’equilibrio
● Per ricavare questi dati si utilizzano l’accelerometro e il
giroscopio presenti sulla board
Lettura dei valori
● LSM6DSL è la componente (system-in-package) che integra al suo interno
accelerometro e giroscopio
● Per mettere in comunicazione LSM6DSL con la board (STM32L475) abbiamo
utilizzato il protocollo I2C
● Attraverso I2C è possibile leggere e scrivere registri di LSM6DSL per
configurare i sensori ed ottenerne i valori letti a seguito dei campionamenti.
● La frequenza di campionamento utilizzata di default è 52Hz.
Giroscopio

● Misura la velocità angolare lungo i tre assi

● La velocità viene riportata in termini di gradi
per secondo (degrees per second, dps)
● Si calcola l’angolo di inclinazione rispetto
all’asse x integrando le misurazioni ottenute
rispetto al tempo di campionamento:

gyro_roll = gyro_roll + gyro.x ⋅ dt

Accelerometro
● Misura la distribuzione
dell’ accelerazione di
gravità lungo i tre assi
● Per calcolare l’angolo di
inclinazione rispetto
all’asse x:

acc_roll = atan(acc.y / acc.z)

 Complementary filter
● Necessità di valutare entrambi i sensori
● Il giroscopio è molto affidabile nel breve termine, ma nel lungo termine tende a
divergere (fenomeno di drift)
● L’accelerometro è molto affidabile in condizioni statiche, ma misurando tutte le
forze, oltre a quella di gravità, non può essere ritenuto affidabile per misurare
variazioni nel breve termine
● Combinando i valori dei sensori usando un complementary filter si ottiene una
misurazione stabile dell’angolo di inclinazione sia nel breve che nel lungo termine
angle = α ⋅ (angle + gyro.x ⋅ dt) + (1 - α) ⋅ (acc_roll)

con α = τ / (τ + dt)

(α ≃ 0.98)
PID controller
● Regolatore proporzionale, integrativo e derivativo
● Combina tre azioni di controllo
○ azione proporzionale all'errore
○ azione proporzionale all'integrale dell'errore (basata sugli errori
commessi nel passato)
○ azione proporzionale alla derivata dell'errore (predittivo del
comportamento futuro)
● L’errore e(t) è l’angolo di inclinazione del robot al tempo t
● L’output viene usato per comandare i motori stepper

output = Kp ⋅ e(t) + Ki⎰e(t)⋅ dt + Kd ⋅ d/dt e(t)

Struttura del sistema
● Le funzionalità del sistema sono gestite da 6 task:
1. StartStopTask - attende il segnale di start/stop (comando o interrupt generato
dal pulsante) e notifica InputProcessorTask
2. InputProcessorTask - attende l’input dei sensori, lo processa calcolando
attraverso il PID l’output da fornire ai motori e sveglia MotorControllerTask
3. MotorControllerTask - attende di essere svegliato, calcola lo step_period e
controlla i motori stepper fornendo un impulso ogni step_period fino ad un
prossimo aggiornamento dell’output
4. UITask - fa lampeggiare i led in base alla direzione di rotazione dei motori
5. SerialRxTask - legge dalla porta seriale e processa i comandi letti
6. SerialTxFlushTask - svuota il buffer dei messaggi e li stampa sulla seriale
FreeRTOS e canali I/O

● CMSIS_V1 : wrapper per le API di FreeRTOS

● Primitive di sincronizzazione: task notification - alternativa light-weight ai
semafori
● Notifica inviata direttamente al task (invece che attraverso intermediari),
viene settato lo stato a pending e il task si sblocca
● Canali di input: bottoni blu e nero, porta seriale (UART)
● Canali di output: due LED, due motori