Descrizione del codice per il movimento

Cominciamo a fare un po' di conti con la meccanica :

La ruota ha un diametro di 58mm, quindi una circonferenza di 182,212mm
L'encoder è su uno degli assi del gruppo riduttore e fa 20 giri ogni giro della ruota
questo ha 40 finestrelle che danno due interrupt ognuna (fronte di salita e di discesa)
quindi 40 x 2 x 20 = 1600 tick per giro (o CPR, count per revolution)
di conseguenza la risoluzione teorica è di 0,1138827336926 mm

Il MAIN è semplicemente uno scheduler che lancia le diverse routine con le giuste priorità.

Il programma lavora in una versione semplificata di "Real Time Operating System", sono state seguite quindi le seguenti regole per lo sviluppo di un "Sistema Operativo Multitasking Cooperativo":

-Ogni routine collabora con tutte le altre, ed in particolare con il main, per il buon funzionamento del sistema.
-Ogni routine occupa il sistema per il minor tempo possibile
-Non ci sono loop lunghi o ritardi a SW
-Il main si preoccupa di schedulare i vari task, le routine si chiamano raramente tra loro
-Lo scambio di informazioni tra task avviene tramite flag e variabili globali
-Le temporizzazioni sono realizzate usando come "Real Time Clock" l'interrupt overflow del TIMER0 che è il vero "hearth-beat" del sistema
-L'esecuzione dei vari task è condizionata dai relativi semafori, il main o un altro task
possono disabilitare una certa funzione

Per approfondimenti sull'argomento si può vedere la pagina dei miei primi studi sull'argomento, da allora qualcosa è cambiato ma i concetti di base sono gli stessi.

Il Main è sempre attivo e, visto che le operazioni di movimento sono molto lente rispetto ai tempi di esecuzione del PIC, per la maggior parte del tempo non farà niente.
L'evento che occupa di più il PIC è l'interrupt degli encoder: alla massima velocità ci sono circa 1600 impulsi per ogni encoder ogni secondo, nella condizione peggiore quindi 1 / 3200 = 1 impulso ogni 312,5 uSec che corrispondono comunque a 1562 istruzioni.

Il cuore di tutto è il timer (TMR0) che ogni 1mSec genera un interrupt.
Nella ISR vengono incrementati i timer (a 8, 16, 24 bit secondo la durata necessaria) che servono per le temporizzazioni delle varie routine.
Anche il computo dello spazio percorso tramite encoder ottici in quadratura, funziona ad interrupt,ognuno dei due encoder genera il proprio interrupt che, analizzato e confrontato con il flag di direzione, incrementa o decrementa le variabili di conteggio dello spazio.
Nella stessa ISR viene calcolata, ogni 50mSec, la velocità di avanzamento attuale,dividendo lo spazio percorso per il tempo.

PID

Una di queste è la MotSpeed che lavora in background e, ogni 50 mSec (dopo la misura della velocità), imposta il PWM (calcolato tramite un algoritmo PID) per fare in modo che la velocità attuale corrisponda con quella desiderata, .

/*MotSpeed ****************************************************************************
Controllo velocit‡ motori PID (Proportional + Integral + Derivative)
il ponte ad H Ë usato in modalit‡ "Locked Anti Phase" (LAP):
PWM = 128 -> motore fermo
PWM = 255 -> velocit‡ massima FWD (circa 200 mm/sec)
PWM = 0 -> velocit‡ massima REW (circa -200 mm/sec)
CCPR2L collegato al motore R
CCPR1L collegato al motore L
*/
void MotSpeed (void)
{
/* const divisore =((200-0)/(255-128)); rapporto tra range velocit‡ e range PWM = 1,57
viene moltiplicato per 100, il risultato finale Ë poi diviso per 100,
per mantenere la precisone di due decimali nei calcoli intermedi senza usare la virgola mobile
*/

Anche la routine PATH lavora in background e, se abilitata, controlla la sequenza di
operazioni da effetuare per seguire il percorso impostato da qualche altra routine che
decide il comportamento del bot.
La routine che decida di eseguire un percorso diverso dal normale, deve:
- impostare il flag di attivazione della routine Path
- scrivere nell'array PathSeq la sequenza di codici della manovra da effettuare
(0=fine sequenza)
- azzerare il puntatore della sequenza
Path comincer‡ allora a scandire l'array chiamando le routine in funzione del codice
impostato (cammina X cm, ruota Y gradi, cammina W mm/sec).
La routine chiamata (Walk, Turn) calcola lo spazio che deve percorrere ogni ruota prima
di ripassare il comando a Path.
A questo punto si Ë attivata Space2Run (schedulata da main) che si disattiva, ripassando
il controllo a Path, solo quando le ruote si sono mosse dello spazio desiderato.
Path controlla la sequenza successiva ripetendo le operazioni o restituendo il controllo
quando il codice Ë = 0.

/*Path ********************************************************************************
viene chiamata solo se (Space2RunFlag==0 & PathSeq[PathSeqPointer] != 0)
se Space2RunFlag = 1, sono ancora in esecuzione routine movimento e quindi non puÚ
passare allo step successivo
se PathSeq[PathSeqPointer]= 0 la sequenza Ë terminata e rilascia il controllo

routine che deve usare Path
-Imposta DesSpeedX = 0; // ferma motori
-imposta PathSeq con passi da fare in ordine (fine seq = 0)
-imposta PathSeqPointer = 0; // inizializza sequenza passi
-imposta Space2RunFlag = 0; // reset di qualsiasi routine di movimento

Se deve avanzare a velocit‡ costante si imposta semplicemente la velocit‡

*/

Calcola e inizializza le variabili per Space2Run, per camminare dello spazio desiderato

operazioni:

imposta: Space2RunXstart

Space2RunX è impostata dalla routine chiamante

imposta velocità: DesSpeedX

imposta: Space2RunFlag a 1

*/

controlla continuamente lo spazio percorso per pilotare i motori fino al raggiungimento

della posizione desiderata

operazioni:

controlla se spazio percorso >= spazio desiderato:

if yes, ferma motore corrispondente

se entrambi motori fermi: Space2RunFlag a 0 (si autodisabilita)

/* qualche routine ha abilitato il controllo dello spazio percorso,

ha impostato la velocità (DesSpeedX), lo spazio da percorrere (Space2RunX)

e la posizione di partenza (Space2RunXstart).

Questa routine sarà eseguita continuamente fino al raggiungimento della

posizione desiderata, dopodichè fermerà i motori e disabiliterà il flag

*/