#include <xc.h>
#include <pic18f452.h>
#include <stdio.h>

//#include <stdlib.h>
// CONFIG1H
#pragma config OSC=HS         // Oscillator Selection bits (HS oscillator)
#pragma config OSCS=ON        // Oscillator System Clock Switch Enable bit (Oscillator system clock switch option is enabled (oscillator switching is enabled))
// CONFIG2L
#pragma config PWRT=OFF       // Power-up Timer Enable bit (PWRT enabled)
#pragma config BOR=ON         // Brown-out Reset Enable bit (Brown-out Reset enabled)
#pragma config BORV=27        // Brown-out Reset Voltage bits (VBOR set to 2.0V)
// CONFIG2H
#pragma config WDT=OFF        // Watchdog Timer Enable bit (WDT disabled (control is placed on the SWDTEN bit))
#pragma config WDTPS=1        // Watchdog Timer Postscale Select bits (1:1)
// CONFIG3H
#pragma config CCP2MUX=ON     // CCP2 Mux bit (CCP2 input/output is multiplexed with RC1)
// CONFIG4L
#pragma config STVR=ON        // Stack Full/Underflow Reset Enable bit (Stack Full/Underflow will cause RESET)
#pragma config LVP=OFF        // Low Voltage ICSP Enable bit (Low Voltage ICSP disabled)
// CONFIG5L
#pragma config CP0=OFF        // Code Protection bit (Block 0 (000200-001FFFh) not code protected)
#pragma config CP1=OFF        // Code Protection bit (Block 1 (002000-003FFFh) not code protected)
#pragma config CP2=OFF        // Code Protection bit (Block 2 (004000-005FFFh) not code protected)
#pragma config CP3=OFF        // Code Protection bit (Block 3 (006000-007FFFh) not code protected)
// CONFIG5H
#pragma config CPB=OFF        // Boot Block Code Protection bit (Boot Block (000000-0001FFh) not code protected)
#pragma config CPD=OFF        // Data EEPROM Code Protection bit (Data EEPROM not code protected)
// CONFIG6L
#pragma config WRT0=OFF       // Write Protection bit (Block 0 (000200-001FFFh) not write protected)
#pragma config WRT1=OFF       // Write Protection bit (Block 1 (002000-003FFFh) not write protected)
#pragma config WRT2=OFF       // Write Protection bit (Block 2 (004000-005FFFh) not write protected)
#pragma config WRT3=OFF       // Write Protection bit (Block 3 (006000-007FFFh) not write protected)
// CONFIG6H
#pragma config WRTC=OFF       // Configuration Register Write Protection bit (Configuration registers (300000-3000FFh) not write protected)
#pragma config WRTB=OFF       // Boot Block Write Protection bit (Boot Block (000000-0001FFh) not write protected)
#pragma config WRTD=OFF       // Data EEPROM Write Protection bit (Data EEPROM not write protected)
// CONFIG7L
#pragma config EBTR0=OFF      // Table Read Protection bit (Block 0 (000200-001FFFh) not protected from Table Reads executed in other blocks)
#pragma config EBTR1=OFF      // Table Read Protection bit (Block 1 (002000-003FFFh) not protected from Table Reads executed in other blocks)
#pragma config EBTR2=OFF      // Table Read Protection bit (Block 2 (004000-005FFFh) not protected from Table Reads executed in other blocks)
#pragma config EBTR3=OFF      // Table Read Protection bit (Block 3 (006000-007FFFh) not protected from Table Reads executed in other blocks)
// CONFIG7H
#pragma config EBTRB=OFF      //Boot Block Table Read Protection bit (Boot Block (000000-0001FFh) not protected from Table Reads executed in other blocks)
#pragma warning disable 752
#pragma warning disable 520
// FREQUENCY CLOCK
#define _XTAL_FREQ 20000000
//VARIABLES
unsigned int Ax,Ay,Az,Gx,Gy,Gz,T;

//---------------------------- INTERRUPT ---------------------------------------
//IPEN=1
void __interrupt(high_priority) ISR_H(void)
      {
       //
       }

//--------------------------- CONFIGURACIÃ’N PIC --------------------------------
void Config_PIC(void)
     {
      TRISA=0b00000000;
      TRISB=0b00000000;
      TRISC=0b00011000;
      TRISD=0b00000000;
      TRISE=0b000;
      LATA=0b000000000;
      PORTA=0b00000000;
      PORTB=0b00000000;
      PORTC=0b00000000;
      PORTD=0b00000000;
      PORTE=0b000;
      LATB=0b00000000;
      LATC=0b00000000;
      LATD=0b00000000;
      LATE=0b000;
      RCONbits.IPEN=0;
      INTCON=0b00000000;
      INTCON2=0b10000000;
      INTCON3=0b00000000;
      PIR1=0b00000000;
      PIR2=0b00000000;
      PIE1=0b00000000;
      PIE2=0b00000000;
      IPR1=0b00000000;
      IPR2=0b00000000;
      ADCON0=0b00000000;
      ADCON1=0b00000110;
     }

//--------------------------- GESTION USART ------------------------------------
void Config_UART()
     {
      SYNC=0;
      SPBRG=32;
      BRGH=0;
      TXSTAbits.CSRC=0;
      TXSTAbits.TX9=0;
      TXSTAbits.TXEN=1;
      TXSTAbits.SYNC=0;
      TXSTAbits.TX9D=0;
      RCSTAbits.SPEN=1;
      RCSTAbits.RX9=0;
      RCSTAbits.SREN=0;
      RCSTAbits.CREN=0;
      RCSTAbits.ADDEN=0;
      RCSTAbits.RX9D=0;
     }

void Tx_UART(char Dato)
     {
      while(!TRMT);
      TXREG=Dato;
     }

char Tx_UART_Empty()
     {
      return TRMT;
     }

void Tx_strUART(const char *Caracter,unsigned short Escape)
     {
      while(*Caracter)
           {
            Tx_UART(*Caracter);
            Caracter++;
           }
      if(Escape==1)Tx_UART('\n');
     }

//--------------------------- GESTIÃ’N I2C --------------------------------------
void Config_I2C()
     {
      SSPSTATbits.SMP=1;                        //Controllo della velocità disabilitato per la modalità velocità standard (100 kHz e 1 MHz)
      SSPADD=49;
      SSPCON1bits.WCOL=0;
      SSPCON1bits.SSPOV=0;
      SSPCON1bits.SSPEN=1;
      SSPCON1bits.CKP=0;
      SSPCON1bits.SSPM0=0;                       //I2C Master,Clock=FOSC/(4 * (SSPADD+1))
      SSPCON1bits.SSPM1=0;
      SSPCON1bits.SSPM2=0;
      SSPCON1bits.SSPM3=1;
      PIR1bits.SSPIF=0;
      PIR2bits.BCLIF=0;
     }

void W_MSSP_I2C(void)
     {
      while((SSPCON2&0x1F)||(SSPSTAT&0x04));
     }

void Start_I2C(void)
     {
      SEN=1;
      while(SEN);
     }

void ReStart_I2C(void)
     {
      RSEN=1;
      while(RSEN);
     }

void Stop_I2C(void)
     {
      PEN=1;
      while(PEN);
     }

void Tx_I2C(unsigned char SendByte)
     {
      SSPBUF=SendByte;
      while(BF);
      W_MSSP_I2C();
     }

unsigned char Rx_I2C(unsigned char vACK)
     {
      unsigned char ReadByte;
      RCEN=1;
      while(!BF);
      ReadByte=SSPBUF;
      if(vACK==0)
        {
         ACKDT=0;
        }
      else
        {
         ACKDT=1;
        }
      ACKEN=1;
      W_MSSP_I2C();
      return ReadByte;
     }

void Wr_I2C(unsigned char IDAddress,unsigned short Address,unsigned short Dato)
     {
      Start_I2C();
      Tx_I2C(IDAddress);
      Tx_I2C(Address);
      Tx_I2C(Dato);
      Stop_I2C();
      __delay_ms(25);
     }

long Rd_I2C(unsigned char IDAddress,unsigned short Address)
     {
      long Respuesta;
      Start_I2C();
      Tx_I2C(IDAddress);
      Tx_I2C(Address);
      ReStart_I2C();
      Tx_I2C(IDAddress+1);
      Respuesta=Rx_I2C(1);
      Stop_I2C();
      __delay_ms(5);
      return Respuesta;
     }

//--------------------------- MPU6050 ------------------------------------------
void Config_MPU6050()
     { 
      Start_I2C();
      Tx_I2C(0xD0);                            
      Tx_I2C(0x6B);
      Tx_I2C(0x00);
      Stop_I2C();
      __delay_ms(5);
     }

unsigned char WHO_AM_I()
     {
      unsigned char Respuesta;
      Start_I2C();
      Tx_I2C(0xD0);
      Tx_I2C(0x75);
      ReStart_I2C();
      Tx_I2C(0xD1);
      Respuesta=Rx_I2C(1);
      return Respuesta;
      __delay_ms(5);
     }

void Lectura_MPU6050(void)
     {
      Start_I2C();
      Tx_I2C(0xD0);
      //Tx_I2C(0x3B);
      Tx_I2C(0x3B);
      ReStart_I2C();
      Tx_I2C(0xD1);
      Ax=Rx_I2C(0)<<8|Rx_I2C(0);
      Ay=Rx_I2C(0)<<8|Rx_I2C(0);
      Az=Rx_I2C(0)<<8|Rx_I2C(0);    
      T=Rx_I2C(0)<<8|Rx_I2C(0);
      Gx=Rx_I2C(0)<<8|Rx_I2C(0);
      Gy=Rx_I2C(0)<<8|Rx_I2C(0);
      Gz=Rx_I2C(0)<<8|Rx_I2C(1);
      Stop_I2C();
      __delay_ms(5);
     }

//--------------------------- CICLO --------------------------------------------
void main(void)
     {
      char Dato[20];
      Config_PIC();
      Config_I2C();
      Config_UART();
      Config_MPU6050();
      __delay_ms(200);
      while(1)
           {
            Lectura_MPU6050();
            sprintf(Dato,"WHO_AM_I = %x",WHO_AM_I());
            Tx_strUART(Dato,1);
            sprintf(Dato,"Ax = %d",Ax);
            Tx_strUART(Dato,0);
            sprintf(Dato," | Ay = %d",Ay);
            Tx_strUART(Dato,0);
            sprintf(Dato," | Az = %d",Az);
            Tx_strUART(Dato,1);
            sprintf(Dato,"Gx = %d",Gx);
            Tx_strUART(Dato,1);
            sprintf(Dato,"Gy = %d",Gy);
            Tx_strUART(Dato,1);
            sprintf(Dato,"Gz = %d",Gz);
            Tx_strUART(Dato,1);
            sprintf(Dato,"Temperatura = %d",T);
            Tx_strUART("------------------",1);
            __delay_ms(1000);
           }
     } 

#include "p33FJ128MC802.h"          //fusebit fastRC wPLL e tutto libero
_FOSCSEL (FNOSC_FRCPLL & IESO_ON)      //Oscillatore FastRC 7,37 MHZ with PLL 
#define ledr _LATA4                    // RA4 pin12 led rosso
#define ledv _LATB4                    // RB4 pin11 led verde
#define ph1 PWM1CON1bits.PEN1H      //PWM1chan1 pin25 RB14 high side enable
#define pl1 PWM1CON1bits.PEN1L      //PWM1chan1 pin26 RB15 low side enable
#define ph2 PWM1CON1bits.PEN2H      //PWM1chan2 pin23 RB12 high side enable
#define pl2 PWM1CON1bits.PEN2L      //PWM1chan2 pin24 RB13 low side enable
        
long int xgr, ygr, xgc, ygc;           //variabili riguardanti il giroscopio
int xg, yg, xgp, ygp, stato, lbyte, hbyte;
float gx, gy, gyp;
int bufa, seg;                         //variabili riguardanti l'accelerometro
float ax, ay, yar, xar, xa, ya, xac, yac, xap, yap, kx; 
int avades, avasin, asp, adp;          //variabili riguardanti il duty cycle per il PWM ai motori
float g, tilt, tilr, tilrr;            //gravità e inclinazioni
float fus, fup, fupp, auf, a, den, spa;//fusioni, -accelerazione e altre variabili 
int aux, ii;

int main (void) {             //Main Program, configurazioni 
PLLFBDbits.PLLDIV=41;         // M=43 PLLDIV=41 clock oscillator Tcy=25 ns 
CLKDIVbits.PLLPOST=0;         // N1=2  N2=2(default) 7.37*M/N1/N2=79Mhz~40MIPS 
TRISA = 0b1111111111101111;   //OUT RA4 pin 12 LED rosso
TRISB = 0b0000111111101111;   //OUT RB4 pin 11 LED verde
I2C1BRG=100;           //BoudRate I2C =400.000 ; (1/400.000-0,000000120)x39.600.000-2=92,3 
AD1CHS0bits.CH0SA = 0; //AN0 pin 2 acceler.x verso alto (CH0SA=1 AN1 pin 3 acceler.y verso avanti)
AD1CON1bits.SSRC=7;    //ADC1 module auto-convert
AD1CON1bits.ASAM=1;    //ADC1 module auto-start
AD1CON1bits.AD12B=1;   //ADC1 module 12 bit
AD1CON1bits.FORM=1;    //ADC1 module signed integer
AD1CON3bits.SAMC = 31; //SAMPLE TIME 31 CK 
AD1CON3bits.ADCS = 63; //ADC CK 40/(63+1)MHz 14conv+31samp 13,8kS/s 72,7us
AD1PCFGL = 0b0000000111111100; //analog input AN0pin2 x, AN1pin3 y 
IEC0bits.AD1IE = 1;    //ADC int.ENABLE 
AD1CON1bits.ADON=1;    //ADC1 module ON
T1CONbits.TCKPS = 3;   //timer1 prescale 256 6,4 uS/count 0,4 sec/rollover per attese
P1TCONbits.PTCKPS = 2; //PWM1 clock prescale 4
P1TCONbits.PTMOD = 2;  //PWM1 up/down mode P1DCx = 0-1000 indietro, 1000-2000 avanti
P1DC1=0; P1DC2=2000;//motori inizialmente fermi
P1TPER = 999;          //PWM1 time base half period/presc.Tcy PWM 5 kHz motori
T1CONbits.TON = 1;     //timer1 run ON

while (TMR1 <32500) ;  //attesa circa 0,2 sec per assestamento alimentazioni 
I2C1CONbits.I2CEN=1;   //abilitazione modulo per comunicazione I2C
IFS1bits.MI2C1IF = 0;  //serve!
IEC1bits.MI2C1IE = 1;  //Master events enable bit
stato=81;              //inizio configurazione accelerometro
I2C1CONbits.SEN=1;     //start bit per attivare la catena di interrupt

TMR1=0; while (TMR1 <65500) ;      //attesa circa 0,4 sec per assestamento manuale e filtri
for (ii=0; ii<64; ii++) {          //cumula 64 letture
TMR1=0; while (TMR1 <5000) ;       //intervallate di 5000 x 0,0064 = 32 msec
yar=yar+ya; xar=xar+xa; ygr=ygr+yg;}
yar=yar/64; xar=xar/64; ygr=ygr>>6;//fa la media delle 64 letture 

PWM1CON2bits.IUE = 1;  //PWM1 duty cycle immediate update enable
P1TCONbits.PTEN = 1;   //PWM1 time base enabled

// risultati della calibrazione:
//massima x=410 con y=-40 e robot verticale
//massima y=382 con x=-10 e robot inclinato 90 gradi indietro   
//con livello a bolla, accelerometro verticale: x=410,  y=-43 
//         accelerometro inclinato 90 indietro: y=382,  x=-7
//         accelerometro inclinato 90 avanti:   y=-466, x=-8
//         accelerometro inclinato 180      :   x=-425, y=-42

kx=(382+466)/(410+425);    //coefficiente per rendere eguali le sensibilità degli assi x e y
ax=-(xar+7)*kx; ay=-yar-42;//letture calibrate e rovesciate espresse in count digitali
g=sqrtf(ax*ax+ay*ay);      //gravità espressa in count digitali
//g=(382+466)/2;           //gravità fissa presa dalla calibrazione=424 count digitali
tilr=ay*1000/g; tilrr=tilr;//inclinazione di riferimento e iniziale espresse in milliradianti

rip: TMR1=0;               //loop di controllo ogni millisecondo
ax=-(xa+7)*kx; ay=-ya-42;  //letture calibrate e rovesciate
aux=yg-ygr;
gy=aux;
gy=gy*0.305;//velocità angolare dal giroscopio in mrad/sec (fondo scala=28.570 count per 500 gradi/sec) 
den=(ax*ax+ay*ay);         //denominatore della formula
auf=den-g*g;
if (auf>0) a=sqrtf(auf);   //formula della -accelerazione in count 
else a=0;                  //impedisce eccezioni software per radice di un numero negativo
aux=avades-adp+avasin-asp; //accelerazione ricavata dall'ultima variazione del duty cycle
if (aux>0) seg=-1;         //ne inverte il segno
if (aux<0) seg=1;
if (seg==-1)a=-a;          //applica il segno alla -accelerazione a
adp=avades; asp=avasin;    //memorizza i duty cycle
tilt=(1000*(ay*g+a*ax))/den;//inclinazione da accelerometro in mrad trascurando arcsen
//tilt=(tilt+1000*ay/g)/2; //fa una media con l'inclinazione approssimata che trascura l'accelerazione
tilt=tilt-tilr;            //inclinazione da acelerometro rispetto all'inclinazione assunta come riferimento

fus=tilt+gy;               //fusione delle variabili da accelerometro e da giroscopio
fus=fup+(fus-fup)*.001;    //filtro passa-basso con costante di tempo 1 msec / .001 = 1 secondo
auf=fus*60+(fus-fup)*1500; //sommiamo la derivata con coefficienti empirici 
auf=auf+(fus-2*fup+fupp)*.3;//sommiamo la derivata seconda con coefficienti empirici
auf=auf+(gy-gyp)*.7; gyp=gy;//ricavandola in maggior parte dall'ultima variazione del giroscopio
fupp=fup; fup=fus;         //memorizzaziamo i valori per calcolarne le derivate

auf=auf*0.75;//circa 0,5 count/mrad * guadagno

if(auf>0.0)auf=auf+500;       //duty cycle minimo per vincere l'attrito al distacco
if(auf<0.0)auf=auf-500;       //con trasmissione molto scorrevole (era 400)
if (auf<-1499.7) auf=-1499.7;  //limiti del duty cycle
if (auf>1499.7) auf=1499.7; 
aux=auf;
avades=aux;                //duty cycle destro con segno
avasin=aux;                //duty cycle sinistro con segno

if(aux<0) { pl1=0;ph1=1;pl2=0;ph2=1;//ponte H abilitato high side    
aux=-aux; P1DC1=aux;P1DC2=aux; goto dopo;}//duty cycle da 0 a 2000

if(aux>0) { ph1=0;pl1=1;ph2=0;pl2=1;    //ponte H abilitato low side
aux=2000-aux; P1DC1=aux;P1DC2=aux; goto dopo;}//duty cycle da 2000 a 0

if(aux==0) {ph1=0;pl1=0;P1DC1=1000;ph2=0;pl2=0;P1DC2=1000;}////duty cycle zero
dopo:
if (fus>0) ledv=1; else ledv=0;//usiamo i Led come ci pare
if (fus<0) ledr=1; else ledr=0;
//P1DC1=avasin+1000;         //duty cycle sinistro 0-2000 senza segno per unità PWM1 
//P1DC2=avades+1000;         //duty cycle destro 0-2000 senza segno per unità PWM1 

spa=spa+auf*0.003;       //cumula i segnali di errore già considerati che grossomodo danno lo spazio percorso (era spa+fus)  
tilr=tilrr-spa*.005; //e usa il cumulo per ricorreggere il riferimento della regolazione (era 0.000667)
tilr=tilr-auf*.05; //aggiungendo l'ultima variazione a titolo di derivata (era 0.0286)

while (TMR1 <160) ;        //attesa 160*0.0064=1msec 
goto rip; }                //fine di 1 ciclo di regolazione, ricomincia daccapo

void __attribute__((no_auto_psv)) _ISR _ADC1Interrupt(void) //ADC ogni 73 uS alternando le due letture 
{if (AD1CHS0bits.CH0SA == 0) {            //se lo ADC ha letto il canale x 
bufa=ADC1BUF0;                            //lo salva nel buffer
AD1CHS0bits.CH0SA = 1;                    //e predispone la lettura del canale y
xa=bufa;                           //lettura asse x in formato floating (finezze)
//xac=31*xap+xa; xa=xac/32;//passa basso abbastanza inutile con costante di tempo 0,073x2x32=4,7 msec
//xap=xa;                                   //memorizza il valore filtrato   
goto ex; }                                //esce dalla ISR

bufa=ADC1BUF0;                            //se invece lo ADC ha letto il canale y, lo salva nel buffer
AD1CHS0bits.CH0SA = 0;                    //e predispone la lettura del canale x
ya=bufa;                           //idem per asse y
//yac=31*yap+ya; ya=yac/32; 
//yap=ya; 
ex: IFS0bits.AD1IF = 0; }                 //la InterruptServicingRoutine esce

void __attribute__((no_auto_psv)) _ISRFAST _MI2C1Interrupt(void) { //ISR I2C master ciclo letture ogni 265 microsec 
if (stato == 0)     //inizio
{if (I2C1STATbits.P==1)stato++; else goto res;//se era ozioso (idle) fa start altrimenti resetta
 I2C1CONbits.SEN=1;    //start bit
 goto ex; }  
if (stato == 1)     //dopo start
{if (I2C1STATbits.S==1)stato++; else goto res;  //se fatto start altrimenti resetta
I2C1TRN=0b11010110;  //slave address giroscopio pin SA0 adafruit al positivo D6=trasm D7=ric  
goto ex; }  
 if (stato == 2)     //se ha scritto slave address
 {if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se ACK scrive sub address registro slave
  I2C1TRN=0b10101000; //28h=OUT_X_L, MSBit=1 per lettura multipla
goto ex; }  
 if (stato == 3)     //se ha scritto indirizzo registro slave
 {if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se ACK ripete start
 I2C1CONbits.RSEN=1;  //repeated start
 goto ex; }  
if (stato == 4)     //dopo start
{if (I2C1STATbits.S==1)stato++; else goto res;  //se fatto start altrimenti resetta
I2C1TRN=0b11010111;  //slave address giroscopio pin SA0 adafruit al positivo D6=trasm D7=ric  
goto ex; }  
if (stato == 5)     //se era indirizzo slave lettura
 {if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se ACK
 I2C1CONbits.RCEN=1;  //abilitazione lettura 
goto ex; }  
if (stato == 6)     //
 {if (I2C1STATbits.RBF == 1)stato++; else goto res;//se letto primo byte fa ACK
lbyte=I2C1RCV;  //e memorizza lettura 
I2C1CONbits.ACKDT=0;//predispone Ack da master
I2C1CONbits.ACKEN=1;//Ack
goto ex; }  
if (stato == 7)     //
 {if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se  ACK
I2C1CONbits.RCEN=1;  //abilitazione lettura
goto ex; }  
if (stato == 8)     //
 {if (I2C1STATbits.RBF == 1)stato++; else goto res;//se letto secondo byte fa ACK
hbyte=I2C1RCV;  //e memorizza lettura 
xg=-((hbyte<<8)+lbyte);//lettura asse x giroscopio non utilizzata a perdere
xgc=(long int)7*xgp+xg; xg=(long int)xgc>>3;
xgp=xg;//filtro 2120 uS a perdere 
I2C1CONbits.ACKEN=1;//Ack
goto ex; }  
if (stato == 9)     //se era secondo byte
 {if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se ACK
 I2C1CONbits.RCEN=1;  //abilitazione lettura altro asse 
goto ex; }  
if (stato == 10)     //
 {if (I2C1STATbits.RBF == 1)stato++; else goto res;//se letto primo byte fa ACK
lbyte=I2C1RCV;  //e memorizza lettura 
I2C1CONbits.ACKEN=1;//Ack
goto ex; }  
if (stato == 11)     //
 {if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se  ACK
I2C1CONbits.RCEN=1;  //abilitazione lettura
goto ex; }  
if (stato == 12)     //
 {if (I2C1STATbits.RBF == 1)stato++; else goto res;//se letto secondo byte manda NACK
hbyte=I2C1RCV;  //e memorizza lettura 
I2C1CONbits.ACKDT=1;//predispone NAck da master
yg=((hbyte<<8)+lbyte);
//ygc=(long int)7*ygp+yg; yg=(long int)ygc>>3;
//ygp=yg;//filtro 2120 uS  
//yg=yg<<1;
I2C1CONbits.ACKEN=1;//NAck
goto ex; }  
if (stato == 13)     //dopo Nack mandato da master
 {stato=0;
I2C1CONbits.PEN=1;//Stop
goto ex; }  

if (stato == 81)     //configurazione dopo start
{if (I2C1STATbits.S==1)stato++; else goto res;  //se fatto start altrimenti resetta
  I2C1TRN=0b11010110;  //slave address giroscopio pin SA0 adafruit al positivo D6=trasm D7=ric
goto ex; }  
if (stato == 82)     //se ha scritto slave address
 {if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res; //se ACK scrive indirizzo registro slave
  I2C1TRN=0b00100000; //sub address registro giroscopio 20h CTRL1 per dato singolo
goto ex; }  
if (stato == 83)     //se ha scritto sub address registro slave
 {if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res; //se ACK manda configurazione registro acc.
  I2C1TRN=0b11111111; //giroscopio ON OutDataRate 1000 Hz cutoff 100 Hz XYZ axis enabled
goto ex; } 
if (stato == 84)     //se ha mandato configurazione
 {if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res; //se ACK stop poi configura fondo scala
 I2C1CONbits.PEN=1;//Stop
 goto ex; } 
if (stato == 85)     //prosegue configurazione giroscopio
{if (I2C1STATbits.P==1)stato++; else goto res;//se era ozioso (idle) fa start altrimenti resetta
 I2C1CONbits.SEN=1;    //start bit
 goto ex; }  
if (stato == 86)     //dopo start
{if (I2C1STATbits.S==1)stato++; else goto res;  //se fatto start altrimenti resetta
I2C1TRN=0b11010110;  //slave address giroscopio pin SA0 adafruit al positivo D6=trasm D7=ric
goto ex; }  
if (stato == 87)     //se ha scritto slave address
 {if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res; //se ACK scrive sub-address registro slave
  I2C1TRN=0b00100011; //sub address registro giroscopio 23h CTRL4 per dato singolo
goto ex; }  
if (stato == 88)     //se ha scritto sub address registro slave
 {if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res; //se ACK manda configurazione registro gir.sin.
I2C1TRN=0b00010000; //giroscopio fondo scala 500 dps
goto ex; } 
if (stato == 89)     //se ha mandato configurazione
 {if (I2C1STATbits.ACKSTAT == 0)stato=0; else goto res; //se ACK fa stop
 I2C1CONbits.PEN=1;//Stop
 goto ex; } 
goto ex;
res:I2C1CONbits.SEN=0; //reset
I2C1CONbits.RSEN=0; I2C1CONbits.RCEN=0;
I2C1CONbits.ACKEN=0;
I2C1CONbits.PEN=1;//Stop 
if (I2C1STATbits.P != 1)goto res;//accerta bus ozioso
stato=1;
I2C1CONbits.SEN=1;    //start bit per riattivare?
ex: IFS1bits.MI2C1IF = 0; } // fine ISR