Facebook
Google
GitHub
Linkedin
Hi, first of all i'm sorry for my english i hope u can undestand what i'll say.
I'm having problem with my program, i'm trying to do a balancing robot with acc and gyro using PIC32 and I2C.
I tested this I2C routine and it seems to work but something mess up with the data that i read with my acc i've done a cycle to test the ±G switching by moving my platform,it seem to "fall" always by the same side.
The acc i'm using is MMA7455L.
And this is the data sheet link http://www.freescale.com/files/sensors/doc/data_sheet/MMA7455L.pdf
Sorry for some italian comment but i think the I2C routine doesn't need comment i've comment in english only Cmotory,Inizacc and Elabacc.
This is my Code:
I'm having problem with my program, i'm trying to do a balancing robot with acc and gyro using PIC32 and I2C.
I tested this I2C routine and it seems to work but something mess up with the data that i read with my acc i've done a cycle to test the ±G switching by moving my platform,it seem to "fall" always by the same side.
The acc i'm using is MMA7455L.
And this is the data sheet link http://www.freescale.com/files/sensors/doc/data_sheet/MMA7455L.pdf
Sorry for some italian comment but i think the I2C routine doesn't need comment i've comment in english only Cmotory,Inizacc and Elabacc.
This is my Code:
Rich (BB code):
#include<plib.h>
#include<peripheral/int.h>
#include<p32xxxx.h>
#pragma config POSCMOD=XT, FNOSC=PRIPLL
#pragma config FPLLIDIV=DIV_2, FPLLMUL=MUL_18, FPLLODIV=DIV_1
#pragma config FPBDIV=DIV_2, FWDTEN=OFF, CP=OFF, BWP=OFF
#define LCDDATA 1
#define LCDCMD 0
#define PMDATA PMDIN
int len,reg_addr,PER_ADDR,Dgyro,Dacc,Dgyroold,DgyroM,Direz;
unsigned char dato;
void configPIC ()
{
#define SYSTEM_CLK (80000000L)
SYSTEMConfigPerformance(SYSTEM_CLK);
mJTAGPortEnable(DEBUG_JTAGPORT_OFF); // JTAG disabled
TRISE=0x00; // Set TRISE to all output
LATE=0x00; // Init all the E pin to 0
}
void eepromInit(void)
{
I2CConfigure(I2C2, I2C_ENABLE_SLAVE_CLOCK_STRETCHING|I2C_ENABLE_HIGH_SPEED);
I2CSetFrequency(I2C2,80000000L,40000); //Set the F=4Khz
I2CEnable(I2C2, TRUE); //I2C Enable
}
void eepromRead(unsigned int PER_ADDR, unsigned int reg_addr,unsigned char *buf)
{
StartI2C2(); //Avvio L'I2C
IdleI2C2(); //Metto in idle aspettando un nuovo comando
MasterWriteI2C2(PER_ADDR | 0); //Chiamo la periferica desiderata in scrittura
IdleI2C2();
if (I2C2STATbits.ACKSTAT)return; //Controllo se è ritornato un ack se no rinizio la procedura
MasterWriteI2C2(reg_addr); //Chiamo il registro interno alla perifica
IdleI2C2();
if(I2C2STATbits.ACKSTAT) return; //Controllo se è ritornato un ack se no rinizio la procedura
RestartI2C2(); //Eseguo un Restart per poter chiamare la funzione in lettura altrimenti creerei un situazione di conflitto sui bus
IdleI2C2();
MasterWriteI2C2(PER_ADDR | 1); //Chiamo la periferica desiderata in lettura
IdleI2C2();
if(I2C2STATbits.ACKSTAT) return; //Controllo se è ritornato un ack se no rinizio la procedura
I2C2CONbits.RCEN = 1;
while(!DataRdyI2C2()); //Inizio ricezione dati
*buf = I2C2RCV; //Metto i dati nel buffer
I2C2CONbits.ACKDT = 1;
I2C2CONbits.ACKEN = 1;
while(I2C2CONbits.ACKEN==1); //inizio la procedura di ack per arrivare alla fine della trasmissione
StopI2C2();
IdleI2C2();
}
void eepromWrite(unsigned int PER_ADDR, unsigned int reg_addr, unsigned char *buf)
{
StartI2C2();
IdleI2C2();
MasterWriteI2C2(PER_ADDR | 0); //Chiamo la periferica in scrittura
IdleI2C2();
if (I2C2STATbits.ACKSTAT)return; //Richiedo un ack
MasterWriteI2C2(reg_addr); //Invio l'indirizzo del registro della periferica
IdleI2C2();
MasterWriteI2C2(*buf); //Scrivo il dato inserito nella variabile puntata da buf
IdleI2C2();
if (I2C2STATbits.ACKSTAT)return; //Richiedo un ack
StopI2C2();
IdleI2C2();
while(1)
{
StartI2C2();
IdleI2C2();
MasterWriteI2C2(PER_ADDR | 0); //Chiamo la periferica in scrittura
IdleI2C2();
if (I2C2STATbits.ACKSTAT==0) break; //Se ritorna un ack esco dal ciclo
StopI2C2();
IdleI2C2();
}
StopI2C2();
IdleI2C2();
}
void Inizacc ()
{
int indirizzo=0x3A;
dato=0b00000101;
eepromWrite(indirizzo,0x16,&dato); //2G mode
dato=0b00000000;
eepromWrite(indirizzo,0x18,&dato); //All axis enable
dato=0b00000000;
eepromWrite(indirizzo,0x19,&dato); //Motion Detector
}
void Elabacc ()
{
int indirizzo=0x3A;
eepromRead(indirizzo,0x08,&dato); //Z axis 8 bit, only one avaible in measurament mode
Dacc=dato;
}
void CMotori ()
{
int Stab;
Stab=Dacc; //Var to avoid activating motors while in stability
if(Stab=0) return;
Direz=Dacc && 0x80; //Mask MSB to compare it (MSB change with the G polarity +G=0 -G=1).
if (Direz = 0)
{
while(Direz=0) //while falling one that side.
{
LATEbits.LATE0=1; //Enable the motors supply and direction
LATEbits.LATE1=1;
LATEbits.LATE2=0;
Elabacc(); //Check the acc data
Direz=Dacc && 0x80; //Put new data in direz
}
}
else
{
while(Direz=0x80) //same as before but for other side
{
LATEbits.LATE0=1;
LATEbits.LATE1=0;
LATEbits.LATE2=1;
Elabacc();
Direz=Dacc && 0x80;
}
}
}
main (void)
{
configPIC();
eepromInit();
Inizgyro(); //function removed in this because i'm sure that's right and i don't use it in this example
Inizacc();
while(1)
{
Elabacc();
CMotori();
}
}
