Figura A.2: accelerometro 1, asse y
Figura A.4: accelerometro 2, asse y
Figura A.6: accelerometro 3, asse y
B. Schemi Matlab
Vengono ora riportati gli schemi Simulink di tutti i blocchi utilizzati nello schema di controllo finale, già ottimizzati con gli accorgimenti citati nel paragrafo 5.3.1
Figura B.1: schema di controllo ottimizzato
Figura B.3: schema dei blocchi V-Acc
Il valore del blocco gain vale -9.81/sens, dove sens indica la sensitività dell’accelerometro.
Figura B.4: schema dei blocchi ROT_X
Il valore dei blocchi gain sono cos(x) e sin(x) (con x gli angoli misurati nel capitolo 2) .
Figura B.5: schema del blocco Initial Rotation I valori dei guadagni sono 1/9.81 e -1/9.81.
Figura B.6: schema del blocco Floating Mean
I blocchi DF2T implementano un filtro FIR a media mobile a 100 campioni.
Figura B.8: schema del blocco Calibration
Figura B.9 : schema del blocco STATIC
Figura B.11: schema del blocco DYNAMIC
Figura B.12: schema del blocco LF
I blocchi Calcola dV, e le chart sono già stati descritte nel paragrafo 3.4.
Figura B.13 : schema del blocco Motion integrator Il blocco Controllo è già stato descritto nel paragrafo 2.3.5.
Figura B.14: schema del blocco Integ
Figura B.15: schema del blocco switch
I blocchi Continue hanno il compito di mantenere la posizione costante tra i cambi di modalità di navigazione.
3x 11 2x 1z 32 1y 3x 1z 2 3x 1x 2 2 1z 3x 21 2x 1z 3x 2 2x 2 3 1z x 3 2 2x 2 2 1z 2 3x 2x 2 2 1z 2 3x 1x 2 3 1z 2 x 3 2x 2 3 1z 2y 32 2x 1z 2 1z 3x 2z 2 1 2y 32 3x 1z 2 1z 3x 3z 2 1 2 z 1 3x 3z 2 2 2z 3x 2x 3 1 1z 2 3x 11 1z 2z 2 3x 3 1 1z 2z 3y 3 2 3z 1x 2 2z 2 1 3z 1x 1z 3y 3 2 3z 1x 2z 2y 3 2 3z 1x 1z 2y 3 2 3z 1x 2 1z 3x 23 2 1z 3x 2x 3 1 2 1 1y 2x 3y 1z 2y 3x 1 2 2x 1z 2y 2 3x 1 2 1z 1y 2x 2 3 3y 1z 2 z 1 3x 3 3 2y 3x 2 3 3y 1z 2y 2x 2 3 3y 1z 32 1y 2x 1z 2y 2 1 3x 3y 1z 2 3x 21 1z 2y 2 1 2x 3y 1z 2z 1x 3z 2 1 1z 2z 3x 3z 2 2 1z 2z 3x 3z 2 1 1z 2z 1x 3z 2 2 1z 2 3x 1y 1 2 1z 2 1z 2 3x 3 1 2 1 3x 2z 2 2 3x 1y 1 2 2x 1z 2z 3 33 1z 2y 3x 2 1z 2 3 1y 2x 3z 1x 2 1 2y 3x 3z 1y 2 1 1z 33 3z 1x 2z 33 3z 1x 2z 2y 3x 1y 2 1 2y 2x 3z 1y 2 1 1z 3y 3x 1y 2 1 2y 3x 3z 1x 2 1 2z 3y 3x y 1 2 1 2y 3x 1z 1y 2 3 2y 3x 1z 1y 2 1 2y 2x 3z 1x 2 1 1y 3x 3z 1x 2 1 2x 2 3 3x 3z 1x 2x 2 2 3x 3z 1x 3x 2 2z 2 2 1z 1z 2y 3y 2 1 1x 1z 2y 3y 2 3 1x 2z 2y 3y 2 1 1x 2x 2 3 3x 1z 1x 2z 2y 3y 2 3 1x 1z 3y 32 1x 2z 23 3z 1x 2x 2 2 3x 1z 1x 1z 23 3z 1x 2z 1 3 3x 3z 1x 1z 2 3z 2 2 1x 1z 1 3 3x 3z 1x 2 2 2 2x 3z 1x 2 2z 2 2 3z 1x 2z 1 3 2x 3z 1x 1z 1 3 2x 3z 1x 2z 3x 23 1z 1z 2 3y 2 3 1x 3y 3x 2 1z 2 3 2z 2y 3x 1z 2 3 2z 2 3y 2 3 1x 1z 2y 32 1x 2z 3y 3x 1z 2 3 2z 2y 32 1x 2z 2 3z 2 2 1x 2z 3y 32 1x 2x 3z 2 1y 2 3 2y 3x 3z 1y 2 3 2z 2y 3x 1y 2 3 2y 2x 3z 1y 2 3 2z 3y 3x 1y 2 3 1z 3y 3x 1y 2 3 1z 2 3y 2 1 1x 2z 2 3y 2 1 1x 1z 2 3z 12 1x 2z 2 3z 2 1 1x 2 3 2 2x 3z 1x 2 1x 2 3 3x 3z 2 1x 2 2 3x 3z 2 1x 2 3 2x 3z 2 1x 2 2 2x 3z 21 3x 3z 1x 11 3x 3z 1x 21 2x 3z 1x 11 2x 3z 1x 2x 3z 2 1y 2 1 3x 3z 2 1y 2 1 3x 3z 2 1y 2 3 2y 3x z 1 2 1 1y 3x 3z 12 2z 3y 3x 12 1y 2x 3z 12 2y 2x 3z 12 2z 2y 3x 12 1z 3y 3x 12 2 2z 2 1 1z 3x 3z 12 r a r r a r r r -r r r -r a r r -r r r -r r r -r r r r r r -r r r r a r r -r r r r a r r r r r -r r r -r r r 2r r a r -r r 2r -r r r 2r r r r r r r 2r -r r r 2r -r r r 2r r r a r r r 2 -r r r r r r r 2r r r 2r -r r r r r r a -r r r r r r r r -a r r r r r r r r a r r r r r -r r r r -r r r r r r r r r r r r -r r 2r r r 2 r r r r r r 2r -xr r a r r r r 2 -r r r r 2 r r r r -r a r r r a r r -r r r r -r r r r r r r r 2 -r r r r 2 r r r r r r r r r r r r r r r r 2 -r r r r 2 -r r r r -r r r r -r r r -r r r r -r r r r -r r r r r r r r r r r r r r a r r a r r r r r r r a r r -r r r 2r r r r r r r 2r -r r r r r r r r r 2r -r r r 2r r r a r -r r r r r r 2 r r r r 2 r r r -r r a r -r r r r 2 -r r a r r r r -r r a r -r r r -r r r r -r r r r -r r r r r r r r r r r r 2 -r r r r r r -r r r r r r -r r r r r r r r r r r r -r r r -a r r r -a r r r a r r r a r r r -r r r -r r r r r r r r r a r r r a r r r a r r r a -r r r a r r r a -r r r a -r r r -r r r a N2 2y 3x + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + = ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω z 1y 3x 3z 2y 2x 1z 1y 2x 1z 2y 3x z 1 1y 3x 1z 2z 3y 1x 1z 3y 1x 2z 2y 1x 1z 2y 1x 2z 3y x 3 1z 3y 3x 2z 2y 3x 2y 2x 3z 1y 2x 3z 2y 3x 3z r r r r r r -r r r r r 2r r r r -r r r -r r r r r r r r r -r r r r r r -r r r -r r r r r r -r r r D2 + + + + + + + + =
) r r r r -r r r r -r r r r r r r r r a r r -r r r r 2 -r r r 2 r r r r r r r r -r r r r -r r r r -r r r r 2 r r r r r r r r - r r r r r r r r r r r r -r r r r -r r r -r r 2 -r r r r r r r r r r 2 -r r r r 2 r r r 2 r a r r r r r r r -r r r r a r r r a r r -r r r r r r -r r r -r r r r -r r a r r r a r -r r r r r r r -r r 2r r r r r r r r r r r r r r 2r -r r r r r r r r r r -r r r -r a r r r r r r r a r -r r r -r a r r -r r a r r r r -r r r 2r r r a r -r r a r r r r -r r r r r r r a r r r r r -r r r -r r r -r r r 2r -r r r 2r r r r 2r -r a r -r r a -r r r a -r r r a r r r a r r r a -r r r a -r r r a r r r a r r r a -r r a r r r r 2 1y 3x 2 3 2y 3y 2 1 1y 3x 2y 3y 1y 2x 2 3 2y 3y 2 1 1y 2x 2y 3y 3y 12 1y 2x 1y 2x 3z 3y 3 2 2y 1z 2 3x 3 1 1z 2y 1x 3z 2 1 2z 2y 1x 3z 2 1 1y 2x 1z 3z 2 1 2y 3x 1z 3z 2 1 2y 2x 3z 3y 3 2 2y 2x 1z 3z 2 1 2z 2y 1x 3z 2 2 1z 2y 1x 3z 2 2 2y 2x 1z 3z 2 2 1y 2x z 1 3z 2 2 2y 3x 1z 3z 2 2 2y 2 3x 1x 2 3 2 3x 2 2y 1 2 2y 2 3x 2x 2 3 2y 2 3x 2x 2 2 y 2 2x 1z 3y 3 2 1y 2x 1z 3y 3 2 2y 2 3x 1y 1 2 2 2y 32 3x 2 2y 3y 2 1 3x 2y 2 3x 1x 2 2 2y 2 1y 2 1 3x 2y 12 1y 3x 2y 32 1y 3x 2y 2 1x 2 3 3x 2 2y 1y 2 1 3x 2 2y 1y 2 3 3x 2y 2x 1x 2 2 3x 2y 1x 11 3x 2y 1x 21 3x 2y 2 1y 2 3 3x 2y 2x 1x 2 3 3x 1x 2 2y 1 2 3x 3y 1x 2x 2 2 3x 2 2y 3y 2 3 3x 2y 2 1x 2 2 3x 2y 1x 1y 1 2 3x 3y 1x 2x 2 3 3x 3y 1x 2 2x 2 2 3y 3x 2 2x 2 2 3y 3x 2 2x 2 3 3y 12 2y 2x y 3 1x 2 2x 2 3 3y 3x 21 x 2 2 3y 2 3 y 2 2x 3y 32 2y 2x 3y 3x 11 2x 2 3y 2 1 2y x 2 3y 3x 2y 1 2 2x 3y 1x 11 2x 3y 1x 21 2x 3y 2 1y 2 1 2x 2 3y 2 3 1y 2x 2 3y 2 1 1y 2x 3y 32 1y 2x 3y 2 1y 2 3 2x 3y 2 1x 2 2 2x 3y 2 1x 2 3 2x 3y 3x 1y 1 2 2x 3y 1x 1y 1 2 x 2 3y 1x 2y 1 2 2x 2 2y 12 3x 2y 2 3x 11 2z 3y 3x 23 1z 3y 3x 23 2y 3x 3z 23 1y 3x 3z 23 2y 3x 1z 23 1y 3x 1z 23 2z 3y 1x 23 z 1 3y 1x 23 2y 2 3x 21 2z 3y 1x 2x 3 1 ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1y 3x 3z 2y 2x 1z 1y 2x 1z 2y 3x 1z 1y 3x 1z 2z 3y 1x 1z 3y 1x 2z 2y 1x 1z 2y 1x 2z 3y 3x 1z 3y 3x 2z 2y 3x 2y 2x 3z 1y 2x 3z 2y 3x 3z r r r r r r -r r r r r 2r r r r -r r r -r r r r r r r r r -r r r r r r -r r r -r r r r r r -r r r - D3 + + + + + + + + =
1y 2x 3z 3 2 1y 3x 1z 3 2 2z 2 3x 3 1 1z 1x 2z 2 2 2z 3x 33 3x 2 2z 2 2 2z 1x 33 1z 1x 33 1z 3x 33 1z 1x 3x 3 1 2z 1x 3x 3 1 2 3x 21 2 1x 2 3 2x 2z 3x 23 1z 3x 23 2z 1x 23 1z 1x 23 3x 2 2z 2 1 1x 2 2z 2 1 1z 1x 2y 3 2 1x 2 2z 2 2 2z 3x 2y 3 2 2z 1x 2y 3 2 2z 1x 2x 3 1 1z 1x 2x 3 1 1z x 3 2x 3 1 2z 3x 2x 3 1 2 1x 2 2 2x 3x 1y 1 2 2x 1x y 1 1 2 2x 1x 2y 1 2 2x 2 3x 1y 1 2 1z 3x 2z 2 1 12 1y 2x 2 3x 2x 2 3 2 3x 1x 2 3 2 3x 2x 2 2 1x 2y 1 2 3x 1x 21 3x 2 1y 2 1 3x 12 1y 3x 2 1x 2 2 3x 2 1x 2 3 3x 1x 2 2x 2 2 3x 2 2x 2 2 r r r -r r r -r r 2 -r r r -r r a r r -r r a -r r a r r a -r r r 2 -r r r 2 r a r r -r r a -r r a r r a r r a -r r -r r r r r r r r r r r r r -r r r 2 -r r r 2 r r r 2 -r r r 2 r r -r r 2r -r r 2r r r 2r -r r 2 r r r a r r -r r r r -r r r r 2r r a r -r r a r r r r r r r r r r ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1y 3x 3z 2y 2x 1z 1y 2x 1z 2y x 3 1z 1y x 3 1z 2z 3y 1x 1z 3y 1x 2z 2y 1x 1z 2y 1x 2z 3y 3x 1z 3y 3x 2z 2y 3x 2y 2x 3z 1y 2x 3z 2y 3x 3z r r r r r r -r r r r r 2r r r r -r r r -r r r r r r r r r -r r r r r r -r r r -r r r r r r -r r r D4 + + + + + + + + = ) a r r r r r a r -a r r a r r -r r r r r r r r -r r r a r r r r r a r r r r r -r r r -r * r r -r r r r r r r 2r r r r -r r r -r r r -r r r r r r r r 2r -r r r -r r 2r -r r 2r r r r r r r r r 2r r r -r r -r r r 2 -r r r r r 2 -r r a -r r a r r r 2 r r r 2 r r 2 r r r r r r -r r r r r r 2 -r r -r r r r r a r r r r r r r r a r -r a r r r r r r r r r -r r r -r r r r r r -r r -r r -r r -r r 2r -r r 2r r r r r r r -r r r r r r -a r r -r r a -r r a r r r 2r -r r a -r r a r r r 2 r r 2 -r a -r r r r r r -r r r r r r -r a r r -r r -r r a -r r a r r r r r r r r r -r r r -r r -r a r -a r r r r -r a r a r r -r r r ( N5 23 1y 1z 2 3z 2 2 1y 3y 12 1y 33 3z 1y 23 3z 1y 2 2y 3y 2 3 3y 1x 2x 2 3 2y 3x 1x 2 2 3y 3x 1x 2 2 33 1y 1z 3z 2 1 2y 1z 33 2y 1z 2z 2y 2 2 1z 2z 2y 2 1 1z 3z 2 2 1y 1z 2z 2 2 1y 1z 2z 1y 2 1 1z 2 2y 3 2 1z 3y 3x 1 3 1z 1y 2x 1 3 1z 2y 1x 1 3 1z 2y 2x 1 3 1z 3y 1x 1 3 1z 2y 3 2 1y 1z 3z 2 1 1y 1z 3y 3 2 2y 1z 3y 3 2 1y 1z 3z 2 2 2y 1z 1y 3x 1 3 1z y 3 3 2 3z 2y y 3 2 1x 2 2 3y 2 1x 2 3 2y 1x 1y 1 2 2y 2 1x 2 2 3y 3x 1y 1 2 2y 1x 21 3y 1x 21 3y 1x 1y 1 2 2y 3x 1y 1 2 1x 2 2y 1 2 3y 1x 2x 2 2 1y 3x 3z 1 3 2z 2y 1x 1 3 3y 1x 2y 1 2 3y 2 1y 2 1 2y 2 1y 2 3 2y 2 1y 2 1 2y 12 1y 2 3z 2 1 1y 2 3y 2 3 y 1 2 3y 2 1 1y 2y 32 1y 3y 32 1y 2y 2 1x 2 3 3y 3x 1x 2 3 2y 2x 1x 2 3 2y 3x 1x 2 3 1y 2x 3z 1 3 2z 3y 1x 1 3 3y 2 1y 2 3 2 2y 1y 2 1 2 2y 1y 2 3 3y 3 2 3z 1y 2y 3 2 3z 1y 3z 2 1 2y 2z 3z 2 2 1y 2z 3z 2 2 2y 2z 3z 2 1 1y 2z 23 2y 1z 3y 1x 11 2y 1x 11 2 2y 3y 2 1 2 2y 3 2 3z 2y 3x 11 3y 3x 11 3y 3x 2y 1 2 3x 2 2y 1 2 2 2y 12 2y 3x 2x 2 3 3y 3x 2x 2 2 2y 3x 2x 2 2 3y 3x 2x 2 3 2 2y 32 2 3y 2 1 2y 2 3z 2 1 2y 3y 3x 21 2y 3x 21 2y 3x 3z 1 3 2z 3y 3x 1 3 2y 2x 3z 1 3 2z 2y 3x 1 3 2 3y 2 3 2y 3y 32 2y 23 3z 2y 2 3z 2 2 2y 3y 12 2y 33 3z 2y 2y 2x 1x 2 2 + + + + + + + − + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + = ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω
2 2z 2 1 1z 3y 3 2 2 1z 2z 2 1 2 1z 2 2z 2 2 1z 2z 2 2 2 1z 3z 2 1 2 1z 2z 33 1z 2z 23 1z 3z 2 2 2 1z 2 2z 2 1 3z 2 2z 2 2 3z 1z 3y 12 2z 3y 1x 2 1 2z 2y 1y 2 1 2z 3y 3 2 1z 2z 33 3z 2z 2y 1x 2 1 1z 2y 1x 2 1 1z 2y 1y 2 1 2z 2y 3 2 3z 1z 33 3z 1z 2y 3 2 3z 2z 3y 3x 2 1 2z 3y 12 1z 3y 1x 2 1 1z 3y 3 2 3z 2z 3y 3 2 3z 2y 3 2 2 1z 2z 2y 3x 2 1 2z 2y 3 2 1z 1z 3y 3x 2 1 2z 23 3z 1z 23 3z 1z 2 3z 2 2 1z 2y 1y 2 3 2z 3y 1y 2 1 2 2 2 2x 3z 1z 1 3 2x 3z 1y 1 2 3x 3z 2z 1 3 2x 3z 2y 1 2 2x 3z 1z 3y 1y 2 1 2y 1 2 3x 3z 1z 1 3 3x 3z 1y 1 2 2x 1z 2z 1 3 3x 3z 2z 3 2x 1z 2y 2x 1z 2x 2 2 3x 1z 1y 1 2 3x 1z 2z 1 3 3x 1z 2z 3y 1y 2 3 2z 3y 32 1z 3y 32 1z 3y 1y 2 3 2z 2 3z 2 2 2z 2y 1y 2 3 1x 2 2 2x 1z 1x 2 3 2x 1z 1 3 2x 2 1z 1x 2 2 3x 1z 1x 2 3 3x 1z 1 3 3x 2 1z 2x 2 3 3x 1z 2z 2y 3y 2 3 1z 2y 3y 2 3 2z 2y 3y 2 1 1z 2y 3y 2 1 1x 2 2 2x 3z 1x 2 3 2x 3z 1x 2 2 3x 3z 1x 2 3 3x 3z 2x 2 2 3x 3z 2x 2 3 3x 3z 2z 2y 32 1z 2y 32 2z 2 3y 2 3 1z 2 3y 2 3 r r r 2r -r r -r r r r -r r r a r -r a r r r r r -r r -r r a r r r r r r r r 2r r a r r r r -r r r r r r -r r 2r r a r -r r 2r -r r r -r r a -r r r -r r 2r r r 2r -r 2r r r r r r 2r -r r r r a r -r a r r r -r r r -r r r -r r -r r 2r -r r r r r 2r r r r r r r r r r -r r 2r r r r r r 2r -1r r 2r -1r 2 r r -r r r -r r r -r r 2r r r r -r r a r r a -r r r r r r r r r r r -r r r -r 2r r r r r r r r 2r -r r r -r r r -r r r r r r -r r r r r r r r r r r r -r r r -r r r r r r r r a -r r a r r r r ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1y 3x 3z 2y 2x 1z 1y 2x 1z 2y 3x 1z 1y 3x 1z 2z 3y 1x 1z 3y 1x 2z 2y 1x 1z 2y 1x 2z 3y 3x 1z 3y 3x 2z 2y 3x 2y 2x 3z 1y 2x 3z 2y 3x 3z r r r r r r -r r r r r 2r r r r -r r r -r r r r r r r r r -r r r r r r -r r r -r r r r r r -r r r D6 + + + + + + + =
D. Codici C
Di seguito riportiamo i codici C dei file ert_main.h ed del file ert_main.c.
ert_main.c
#include <pic18.h>
#include <stdio.h>
#include "tmwtypes.h" /* MathWorks types */ #include "simstruc_types.h" /* Simulink types */ #include "IO.h" /* Model's header file */ #include "ert_main.h"
IO_step(0);
/* Get model outputs here */ OverrunFlag--;
}
struct packet_data{
unsigned char StartP1; unsigned char StartP2; short int x;
short int y;
unsigned char CheckSum; unsigned char EndP; } ;
struct packet_in { short int AD0; short int AD1; short int AD2; short int AD3; };
struct packet_in Pin;
struct packet_data P1D = {0xFF, 0xFF, 0, 0, 0xFE, 0}; unsigned char * PP1D = (char *)&P1D;
char *index; static char chantime; TMR0IF = 0;
/* Leggo il sensore analogico */
if (chantime==0) index=(char *)&(Pin.AD3); else index = (char *)&(Pin.AD0)+(chantime-2); switch(chantime) {
case 1: case 3: case 5: case 7: case 9: case 11: // Chan Convert
GODONE = 1;
break;
case 0: case 2: case 4: case 6: case 8: case 10: // Chan X Read Next Chan Set index[0] = ADRESL ; index[1] = ADRESH ; if (chantime == 6) ADCChan(0); else ADCChan((chantime>>1)+1); break; }
chantime = (chantime+1)% MODULOCHAN; }
void packet_send(void) {
static unsigned char value; TXREG = PP1D[value];
value = (value+1)% (unsigned char)sizeof(struct packet_data); }
void main( ) { /* inizialize PIC */ ADCSetup(); SERIAL_INTERRUPT(); OSC(); TIMER0(); /* Initialize model */ IO_initialize(1); while(1) { rt_OneStep(); IO_U.Az1 = 5*((float)Pin.AD0)/1023; IO_U.Ay1 = 5*((float)Pin.AD1)/1023; IO_U.Ax1 = 5*((float)Pin.AD2)/1023; P1D.x = (short int)(1000*IO_Y.X); P1D.y = (short int)(1000*IO_Y.Y); IO_U.INITIAL= 1-RB1; IO_U.MODE = RB0; IO_U.RESET = RB2; } return 0; }
#define BAUD 2400 #define FOSC 40000000L
#define NINE_BITS 0x00 /* Use 8 bits communication */ //#define NINE_BITS 0x40 /* Use 9 Bits communication */ #define TCONV 0x80 // tempo di conversione FOSC/32 #define CHANNEL 0
/* if DIVIDER is >255 use: HIGH_SPEED 0 */ #define HIGH_SPEED 0
#if HIGH_SPEED == 1
#define DIVIDER ((int)(FOSC/(16UL * BAUD) -1)) #define SPEED 0x4
#else
#define DIVIDER ((int)(FOSC/(64UL * BAUD) -1)) #define SPEED 0 #endif #define InitComms() \ SPBRG = DIVIDER; \ TXSTA = (SPEED|NINE_BITS|0x20); \ RCSTA = (NINE_BITS|0x90);\ BAUDCON = 0x40\
// enable global and peripheral interrupt // enable usart interrupt
T0CON = 0xC3;\ TMR0IF = 0;\ TMR0IE = 1; #define ADCChan(X)\ ADCON0 = 0x01 | X<<2 #define ADCSetup()\ ADCON1=0x00;\ ADCON2=0x91\
