void InitMPU6050(unsigned char I2Caddr)
{
LDByteWriteI2C(I2Caddr, 0x6B, 0x01); // Wakeup MPU6050 with PLL to xaxis gyro
LDByteWriteI2C(I2Caddr, 0x19, 0x07); // Set sample rate to 1kHZ
/* ACCEL_CONFIG 0x1C selftest and Full Scale Range
0b00000000 -> 2g
0b00001000 -> 4g
0b00010000 -> 8g
0b00011000 -> 16g */
LDByteWriteI2C(I2Caddr, 0x1C, 0b00000000); // FSR 2g
/* GYRO_CONFIG 0x1B selftest and Full Scale Range
0b00000000 -> 250 deg/s
0b00001000 -> 500 deg/s
0b00010000 -> 1000 deg/s
0b00011000 -> 2000 deg/s */
LDByteWriteI2C(I2Caddr, 0x1B, 0b00000000); // FSR 250 deg/s
/* GYRO_CONFIG 0x1A Low Pass Filter
* 0 -> 0.0 ms
* 1 -> 2.0 ms
* 2 -> 3.0 ms
* 3 -> 4.9 ms
* 4 -> 8.5 ms
* 5 -> 13.8 ms
* 6 -> 19.0 ms */
LDByteWriteI2C(I2Caddr, 0x1A, 3); // 0.0 ms and disable FSYNC
}
void InitHMC5883L(void)
{
LDByteWriteI2C(0x3C, 0x00, 0b01111000); // Configuration Register A
/* 8 averages, 75 Hz, Normal measurement(no bias) */
LDByteWriteI2C(0x3C, 0x01, 0b00100000); // Configuration Register B
/* Gain setting 1.3Gauss range */
LDByteWriteI2C(0x3C, 0x02, 0b00000000); // Mode Register
/* Set in continuos mesurement mode */
}
int main (void)
{
unsigned int n;
InitPorts();
InitI2C();
ControlByte = 0xA0;
LowAdd = 0x0C;
HighAdd = 0x5A;
Data = 0xAB;
Length = 0x05;
for(n = 0; n < PAGESIZE; n++)
{
PageString[n] = n;
}
while(1)
{
LATAbits.LATA0 = 1;
LDByteWriteI2C(ControlByte, LowAdd, Data);
HDByteWriteI2C(ControlByte, HighAdd, LowAdd, Data);
HDPageWriteI2C(ControlByte, HighAdd, LowAdd, PageString);
LDByteReadI2C(ControlByte, LowAdd, &Data, 1);
HDByteReadI2C(ControlByte, HighAdd, LowAdd, &Data, 1);
HDSequentialReadI2C(ControlByte, HighAdd, LowAdd, PageString, PAGESIZE);
Nop();
LATAbits.LATA0 = 0;
}
}
int main(void) {
// Disable Watch Dog Timer
RCONbits.SWDTEN = 0;
// for LED
ODCAbits.ODA6 = 0;
TRISAbits.TRISA6 = 0;
TRISAbits.TRISA4 = 0;
TRISAbits.TRISA0 = 0;
//Enable channel
OpenI2C1(I2C_ON, I2C_BRG);
//Setup_MPU6050();
//LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0x00);
SPI1Init();
SPI2Init();
LCDinit();
LCDProcessEvents();
//_05ms = false;
//TimerInit();
unsigned char fifocount[2];
while (1) {
if (updateScreen) {
LCDProcessEvents();
updateScreen = false;
}
if (getFifo) {
LDByteReadI2C(MPU6050_ADDRESS, MPU6050_RA_FIFO_COUNTH, &fifocount[0], 2);
if (((((unsigned int) fifocount[0]) << 8) | fifocount[1]) == 1024) {
//need to reset
unsigned char temp = 0;
LDByteReadI2C(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, &temp, 1);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0b00000100 | temp);
/*
int i = 0;
for(i = data_p; i < data_p+CHUNK_SIZE; i++){
rpiData.imu[i] = 0;
}*/
__builtin_btg((unsigned int *) &LATA, 0);
} else if (((((unsigned int) fifocount[0]) << 8) | fifocount[1]) >= 42) {
LDByteReadI2C(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, &rpiData.imu[data_p], DMP_PACKET_SIZE);
__builtin_btg((unsigned int *) &LATA, 4);
}
getFifo = false;
}
__builtin_btg((unsigned int *) &LATA, 6);
}
return 0;
}
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void)
{
unsigned int *chptr; // For sending a float value
unsigned int command; // For sending commands to Axis Converter
double gyroXrate, gyroYrate;
char nRFstatus;
unsigned char rfCommands[32]; // Commands received from remote
unsigned char nRFregisters[10];
int iii;
long long xSpeedFilterSum = 0;
long long ySpeedFilterSum = 0;
// Toggle LED
_LATA4 = 1;
/** Read sensors ******************************************************/
readSensorData();
__delay32(1000); // Without this delay, the I2C command acts funny...
accXangle = ((atan2(accel[0], accel[2]))*RAD_TO_DEG)-AngleOffset[0];
accYangle = ((atan2(accel[1], accel[2]))*RAD_TO_DEG)-AngleOffset[1];
gyroXrate = ((double)gyro[0]-(double)GyroOffset[0])/131;
gyroYrate = -(((double)gyro[1]-(double)GyroOffset[1])/131);
/** Feedback Loop *****************************************************/
compAngleX = (compFilterGyro*(compAngleX+(gyroYrate*(double)SAMPLE_TIME)))+(compFilterAccel*accXangle); // Calculate the angle using a Complimentary filter
compAngleY = (compFilterGyro*(compAngleY+(gyroXrate*(double)SAMPLE_TIME)))+(compFilterAccel*accYangle);
if (enableSteppers == 1)
{
iCompAngleY += compAngleY;
iCompAngleX += compAngleX;
}
else
{
iCompAngleY = 0.0;
iCompAngleX = 0.0;
}
ySpeed = -((compAngleY)*(double)pGAIN+(compAngleY-compAngleYlast)*(double)dGAIN+gyroXrate*(double)iGAIN);
xSpeed = -((compAngleX-sGAIN)*(double)pGAIN+(compAngleX-compAngleXlast)*(double)dGAIN+gyroYrate*(double)iGAIN);
xSpeedFilter[SpeedFilterCounter] = xSpeed;
ySpeedFilter[SpeedFilterCounter] = ySpeed;
for(iii = 0; iii<=SpeedFilterLength;iii++)
{
xSpeedFilterSum += xSpeedFilter[iii];
ySpeedFilterSum += ySpeedFilter[iii];
}
xSpeed = (long long)xSpeedFilterSum/(SpeedFilterLength+1);
ySpeed = (long long)ySpeedFilterSum/(SpeedFilterLength+1);
// ySpeed = -((compAngleY)*(double)pGAIN+(compAngleY-compAngleYlast)*(double)dGAIN);
// xSpeed = -((compAngleX)*(double)pGAIN+(compAngleX-compAngleXlast)*(double)dGAIN);
// if (xSpeed<0)
// xSpeed = -(xSpeed*xSpeed);
// else
// xSpeed = xSpeed*xSpeed;
// if (ySpeed<0)
// ySpeed = -(ySpeed*ySpeed);
// else
// ySpeed = ySpeed*ySpeed;
compAngleXlast = compAngleX;
compAngleYlast = compAngleY;
// xSpeed = gyroXrate *1000;
// ySpeed = gyroYrate *1000;
/** Build command string **********************************************/
command = 0b1000000000000000;
if (enableSteppers == 1)
command = command | 0b0100000000000000;
/* Send data to Axis Controller ***************************************/
chptr = (unsigned char *) ∠ // For sending a float value
CS_Axis = 0;
/* Command: MSB
* 15: 0 = NO Speed Values
* 14: 1 = Enable Stepper Driver
* 13:
*/
WriteSPI2(command); // Command: MSB
WriteSPI2(xSpeed); // Send x-velocity vector
WriteSPI2(ySpeed); // Send y-velocity vector
WriteSPI2(*chptr++); // Sending first part of float value
WriteSPI2(*chptr); // Sending second part of float value
CS_Axis = 1;
/** Prepare telemetry *************************************************/
setRawData2string();
/**** Telemetry string format for command=0x42 ************************/
/** command, axh, axl, ayh, ayl, azh, azl, th, tl, 9byte **/
/** gxh, gxl, gyh, gyl, gzh, gzl, cxh, cxl, cyh, cyl, czh, czl,12byte**/
/** motorStatus 1byte**/
/** --Total 22byte **/
serString[16] = xSpeed & 0xFF; // Temporary sending additional telemetry
serString[15] = xSpeed >> 8;
serString[18] = ySpeed & 0xFF;
serString[17] = ySpeed >> 8;
// serString[18] = iax & 0xFF;
// serString[17] = iax >> 8;
// serString[20] = igy & 0xFF;
// serString[19] = igy >> 8;
serString[21] = enableSteppers;
SpeedFilterCounter++;
if (SpeedFilterCounter> SpeedFilterLength)
SpeedFilterCounter = 0;
/** Receive RF commands if any ********************************************/
// LDByteWriteSPI(0x20, 0b00001010);
nRFstatus = LDBytePollnRF();
for(iii=0; iii<10;iii++) // For debug
{
nRFregisters[iii] = readnRFbyte(iii);
}
if (nRFstatus & 0b01000000) // Data Ready RX FIFO
{
// _LATB4 = 1;
//// Delay10TCYx(100);
LDByteReadSPI(0x61, rfCommands, 21); // Read RX FIFO
LDByteWriteSPI(0x27 , 0b01000000); // Clear RX FIFO interrupt bit
//ByteWriteSPI(0xE2 , 0xFF); //Flush RX FIFO
if(rfCommands[0] == 0x82)
{
if(enableSteppers)
{
enableSteppers = 0;
buttonState = 0;
//_LATB4 = 0;
} else {
enableSteppers = 1;
buttonState = 2;
//_LATB4 = 1;
}
} else if (rfCommands[0] == 0x83)
{
_LATB4 = 1;
// Set the received values to variables
for(iii=1;iii<19;iii += 3){
if(rfCommands[iii]<9)
setValueCode[rfCommands[iii]] = (rfCommands[iii+1]<<8) + rfCommands[iii+2];
}
pGAIN = setValueCode[1];
dGAIN = setValueCode[2];
compFilterGyro = (float)setValueCode[4]/(float)1000;
compFilterAccel = 1.0 - compFilterGyro;
iGAIN = (float)setValueCode[3]/100.0;
//angle = (setValueCode[6]-180)* PI /180.0;
//SpeedFilterLength = setValueCode[5];
LDByteWriteI2C(0x00D0, 0x1A, setValueCode[5]); // 0.0 ms and disable FSYNC
sGAIN = (float)(setValueCode[6]-1000)/100.0;
}
// //LATDbits.LATD3 = 0;
//
}
// LDByteWriteSPI(0x20, 0b00001010); //
// LDByteWriteSPI(0x26, 0b00000010); // Set RC_CH to reset retransmit counter
LDCommandWriteSPI(0xE2); // Flush RX buffer (not during ACK)
/** Send telemetry ********************************************************/
// _LATB15 = 0; // Set CE low to stop receiving data
// LDByteWriteSPI(0x20 , 0b00001010); // Set to send mode
// __delay32(100);
LDByteWriteSPI(0x27, 0b00010000); // clear MAX_RT (max retries)
LDCommandWriteSPI(0xE1); // Flush TX buffer (tx buffer contains last failed transmission)
LDByteWriteSPI(0x27, 0b00100000); // clear TX_DS (ACK received)
sendnRFstring( serString, 22);
/** Set nRF to recive mode ************************************************/
// __delay32(1000);
// LDByteWriteSPI(0x27, 0b00100000); // clear TX_DS (ACK received)
// LDByteWriteSPI(0x20 , 0b00001011); // Set to receive mode
// _LATB15 = 1; // Set CE high to start receiving data
/* Done with interrupt ****************************************************/
_LATA4 = 0;
_T1IF = 0; // Clear Timer 1 interrupt flag
}
void Setup_MPU6050(void)
{
int loop;
unsigned char TheReg;
// I need code space. Just create a table with register to clear
const unsigned char MPU6050RegTable[]= {
MPU6050_RA_FF_THR, //Freefall threshold of |0mg| LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
MPU6050_RA_FF_DUR, //Freefall duration limit of 0 LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
MPU6050_RA_MOT_THR, //Motion threshold of 0mg LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
MPU6050_RA_MOT_DUR, //Motion duration of 0s LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
MPU6050_RA_ZRMOT_THR, //Zero motion threshold LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
MPU6050_RA_ZRMOT_DUR, //Zero motion duration threshold LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
MPU6050_RA_FIFO_EN, //Disable sensor output to FIFO buffer LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
MPU6050_RA_I2C_MST_CTRL, //AUX I2C setup //Sets AUX I2C to single master control, plus other config LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
MPU6050_RA_I2C_SLV0_ADDR, //Setup AUX I2C slaves LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
MPU6050_RA_I2C_SLV0_REG, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
MPU6050_RA_I2C_SLV0_CTRL, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
MPU6050_RA_I2C_SLV1_ADDR, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
MPU6050_RA_I2C_SLV1_REG, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
MPU6050_RA_I2C_SLV1_CTRL, //LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
MPU6050_RA_I2C_SLV2_ADDR, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
MPU6050_RA_I2C_SLV2_REG, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
MPU6050_RA_I2C_SLV2_CTRL, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
MPU6050_RA_I2C_SLV3_ADDR, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
MPU6050_RA_I2C_SLV3_REG, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
MPU6050_RA_I2C_SLV3_CTRL, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
MPU6050_RA_I2C_SLV4_ADDR, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
MPU6050_RA_I2C_SLV4_REG, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
MPU6050_RA_I2C_SLV4_DO, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
MPU6050_RA_I2C_SLV4_CTRL, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
MPU6050_RA_I2C_SLV4_DI, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
MPU6050_RA_INT_PIN_CFG, //MPU6050_RA_I2C_MST_STATUS //Read-only //Setup INT pin and AUX I2C pass through LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
MPU6050_RA_INT_ENABLE, //Enable data ready interrupt LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
MPU6050_RA_I2C_SLV0_DO, //Slave out, dont care LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
MPU6050_RA_I2C_SLV1_DO, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
MPU6050_RA_I2C_SLV2_DO, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
MPU6050_RA_I2C_SLV3_DO, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
MPU6050_RA_I2C_MST_DELAY_CTRL, //More slave config LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
MPU6050_RA_SIGNAL_PATH_RESET, //Reset sensor signal paths LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
MPU6050_RA_MOT_DETECT_CTRL, //Motion detection control LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
MPU6050_RA_USER_CTRL, //Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0 LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
MPU6050_RA_CONFIG, //Disable FSync, 256Hz DLPF LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x00);
MPU6050_RA_FF_THR, //Freefall threshold of |0mg| LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
MPU6050_RA_FF_DUR, //Freefall duration limit of 0 LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
MPU6050_RA_MOT_THR, //Motion threshold of 0mg LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
MPU6050_RA_MOT_DUR, //Motion duration of 0s LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
MPU6050_RA_ZRMOT_THR, //Zero motion threshold LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
MPU6050_RA_ZRMOT_DUR, //Zero motion duration threshold LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
MPU6050_RA_FIFO_EN, //Disable sensor output to FIFO buffer LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
MPU6050_RA_I2C_MST_CTRL, //AUX I2C setup //Sets AUX I2C to single master control, plus other config LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
MPU6050_RA_I2C_SLV0_ADDR, //Setup AUX I2C slaves LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
MPU6050_RA_I2C_SLV0_REG, //LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
MPU6050_RA_I2C_SLV0_CTRL, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
MPU6050_RA_I2C_SLV1_ADDR, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
MPU6050_RA_I2C_SLV1_REG, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
MPU6050_RA_I2C_SLV1_CTRL, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
MPU6050_RA_I2C_SLV2_ADDR, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
MPU6050_RA_I2C_SLV2_REG, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
MPU6050_RA_I2C_SLV2_CTRL, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
MPU6050_RA_I2C_SLV3_ADDR, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
MPU6050_RA_I2C_SLV3_REG, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
MPU6050_RA_I2C_SLV3_CTRL, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
MPU6050_RA_I2C_SLV4_ADDR, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
MPU6050_RA_I2C_SLV4_REG, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
MPU6050_RA_I2C_SLV4_DO, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
MPU6050_RA_I2C_SLV4_CTRL, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
MPU6050_RA_I2C_SLV4_DI, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
MPU6050_RA_I2C_SLV0_DO, // //Slave out, dont care LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
MPU6050_RA_I2C_SLV1_DO, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
MPU6050_RA_I2C_SLV2_DO, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
MPU6050_RA_I2C_SLV3_DO, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
MPU6050_RA_I2C_MST_DELAY_CTRL, //More slave config LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
MPU6050_RA_SIGNAL_PATH_RESET, //Reset sensor signal paths LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
MPU6050_RA_MOT_DETECT_CTRL, //Motion detection control LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
MPU6050_RA_USER_CTRL, //Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0 LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
MPU6050_RA_INT_PIN_CFG, //MPU6050_RA_I2C_MST_STATUS //Read-only //Setup INT pin and AUX I2C pass through LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
MPU6050_RA_INT_ENABLE, //Enable data ready interrupt LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
MPU6050_RA_FIFO_R_W, // LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
0xff
};
// //Sets sample rate to 8000/1+7 = 1000Hz
// LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x07);
//Sets sample rate to 8000/1+15 = 500Hz
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 15);
//Disable gyro self tests, scale of 500 degrees/s
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000);
//Disable accel self tests, scale of +-16g, no DHPF
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0x18);
loop=0;
do
{
TheReg = MPU6050RegTable[loop++];
if(TheReg==0xff) break;
LDByteWriteI2C(MPU6050_ADDRESS,TheReg,0);
}while(1);
//Sets clock source to gyro reference w/ PLL
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
//Controls frequency of wakeups in accel low power mode plus the sensor standby modes
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);
//MPU6050_RA_WHO_AM_I //Read-only, I2C address
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x01);
cputs("\r\nMPU6050 Setup Complete\r\n");
}
void Setup_MPU6050()
{
//Sets sample rate to 8000/1+7 = 1000Hz
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0xFF);
//Disable FSync, 256Hz DLPF
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x00);
//Disable gyro self tests, scale of 500 degrees/s
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000);
//Disable accel self tests, scale of +-2g, no DHPF
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0x00);
//Freefall threshold of |0mg|
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
//Freefall duration limit of 0
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
//Motion threshold of 0mg
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
//Motion duration of 0s
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
//Zero motion threshold
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
//Zero motion duration threshold
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
//Disable sensor output to FIFO buffer
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
//AUX I2C setup
//Sets AUX I2C to single master control, plus other config
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
//Setup AUX I2C slaves
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
//MPU6050_RA_I2C_MST_STATUS //Read-only
//Setup INT pin and AUX I2C pass through
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
//Enable data ready interrupt
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
//Slave out, dont care
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
//More slave config
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
//Reset sensor signal paths
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
//Motion detection control
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
//Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
//Sets clock source to gyro reference w/ PLL
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
//Controls frequency of wakeups in accel low power mode plus the sensor standby modes
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);
//Data transfer to and from the FIFO buffer
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
//MPU6050_RA_WHO_AM_I //Read-only, I2C address
}
int main (void)
{
//CLKDIVbits.RCDIV = 0b000; FRC clock divider
unsigned char accel_xh = 0x00;
unsigned char accel_xl = 0x00;
unsigned char accel_yh = 0x00;
unsigned char accel_yl = 0x00;
unsigned char accel_zh = 0x00;
unsigned char accel_zl = 0x00;
unsigned int _60s = 0;
int g_x=0;
int g_y=0;
int g_z=0;
// Disable Watch Dog Timer
RCONbits.SWDTEN = 0;
// for LED
ODCAbits.ODA6 = 0;
TRISAbits.TRISA6 = 0;
TRISAbits.TRISA0 = 0;
TRISAbits.TRISA1 = 0;
TRISAbits.TRISA5 = 0;
//TRISAbits.TRISA2 = 0; scl2
TRISAbits.TRISA4 = 0;
//TRISAbits.TRISA3 = 0; sda2
// push button
TRISAbits.TRISA7 = 1;
TRISDbits.TRISD6 = 1;
TRISDbits.TRISD7 = 1;
TRISDbits.TRISD13 = 1;
//Enable channel
SPI1Init();
LCDInit();
OpenI2C1( I2C_ON, I2C_BRG );
OpenI2C2( I2C_ON, I2C_BRG );
unsigned char lcd_data1[16];
unsigned char lcd_data2[16];
Setup_MPU6050();
_05ms = false;
LDByteWriteI2C(MPU6050_ADDRESS,MPU6050_RA_PWR_MGMT_1 , 0x00); //turn on IMU
TimerInit();
lcd_data1[12] = ' ';
lcd_data1[13] = ' ';
lcd_data1[14] = ' ';
lcd_data1[15] = ' ';
lcd_data2[6]='y';
lcd_data2[7]=' ';
lcd_data2[8]=' ';
lcd_data2[9]='z';
bool accel_queue = true;
while (1) {
//LATAbits.LATA0 = 0;
//LATAbits.LATA6 = 0;
//LATAbits.LATA5 = 0;
//LATAbits.LATA1 = 0;
if (_60s==20000){
_60s = 0;
LDWordReadI2C(FUELGAUGE_ADDRESS, FUELGAUGE_CONFIG, &bat1, &bat2);
LDWordReadI2C(FUELGAUGE_ADDRESS, FUELGAUGE_SOC, &bat1, &bat2);
}
if(_05ms==true){
//do something
_60s+=1;
if(accel_queue){
LDByteReadI2C(MPU6050_ADDRESS,MPU6050_RA_ACCEL_XOUT_H , &rpiData.imu[accel_p], 6);
if(rpiData.imu[accel_p]==0xff && rpiData.imu[accel_p+1]==0xff)
LDByteWriteI2C(MPU6050_ADDRESS,MPU6050_RA_PWR_MGMT_1 , 0x00); //turn on IMU
accel_xh=rpiData.imu[accel_p];
accel_xl=rpiData.imu[accel_p+1];
accel_yh=rpiData.imu[accel_p+2];
accel_yl=rpiData.imu[accel_p+3];
accel_zh=rpiData.imu[accel_p+4];
accel_zl=rpiData.imu[accel_p+5];
accel_p+=12;
if (accel_p>=1200)
accel_p=0;
lcd_data1[7] = 'A';
lcd_data1[8] = 'C';
lcd_data1[9] = 'C';
lcd_data1[10] = 'E';
lcd_data1[11] = 'L';
accel_queue=false;
}
else{
LDByteReadI2C(MPU6050_ADDRESS,MPU6050_RA_GYRO_XOUT_H , &rpiData.imu[gyro_p], 6);
accel_xh=rpiData.imu[gyro_p];
accel_xl=rpiData.imu[gyro_p+1];
accel_yh=rpiData.imu[gyro_p+2];
accel_yl=rpiData.imu[gyro_p+3];
accel_zh=rpiData.imu[gyro_p+4];
accel_zl=rpiData.imu[gyro_p+5];
gyro_p+=12;
if (gyro_p>=1206)
gyro_p=6;
lcd_data1[7] = 'G';
lcd_data1[8] = 'Y';
lcd_data1[9] = 'R';
lcd_data1[10] = 'O';
lcd_data1[11] = ' ';
accel_queue=true;
}
LCDwriteLine(LCD_LINE1, lcd_data1);
LCDwriteLine(LCD_LINE2, lcd_data2);
_05ms=false;
g_x=accel_xl|accel_xh<<8;
lcd_data1[0]=g_x < 0? '-' : ' ';
g_x=g_x > 0 ? g_x : -g_x;
g_y=accel_yl|accel_yh<<8;
lcd_data2[0]=g_y < 0? '-' : ' ';
g_y=g_y > 0 ? g_y : -g_y;
g_z=accel_zl|accel_zh<<8;
lcd_data2[10]=g_z < 0? '-' : ' ';
g_z=g_z > 0 ? g_z : -g_z;
lcd_data1[5]=(g_x%10)+'0';
lcd_data1[4]=(g_x/10)%10+'0';
lcd_data1[3]=(g_x/100)%10+'0';
lcd_data1[2]=(g_x/1000)%10+'0';
lcd_data1[1]=(g_x/10000)%10+'0';
lcd_data2[5]=(g_y%10)+'0';
lcd_data2[4]=((g_y/10)%10)+'0';
lcd_data2[3]=((g_y/100)%10)+'0';
lcd_data2[2]=(g_y/1000)%10+'0';
lcd_data2[1]=(g_y/10000)%10+'0';
lcd_data2[15]=(g_z%10)+'0';
lcd_data2[14]=(g_z/10)%10+'0';
lcd_data2[13]=(g_z/100)%10+'0';
lcd_data2[12]=(g_z/1000)%10+'0';
lcd_data2[11]=(g_z/10000)%10+'0';
}
}
return 0;
}
void Setup_MPU6050(){
simplePrint("Setup\n");
//Sets sample rate to 1000/1+1 = 500Hz
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);
//Disable FSync, 48Hz DLPF
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03);
//Disable gyro self tests, scale of 500 degrees/s
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000);
//Disable accel self tests, scale of +-4g, no DHPF
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00001000);
//Freefall threshold of <|0mg|
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
//Freefall duration limit of 0
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
//Motion threshold of >0mg
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
//Motion duration of >0s
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
//Zero motion threshold
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
//Zero motion duration threshold
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
//Disable sensor output to FIFO buffer
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
//AUX I2C setup
//Sets AUX I2C to single master control, plus other config
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
//Setup AUX I2C slaves
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
//MPU6050_RA_I2C_MST_STATUS //Read-only
//Setup INT pin and AUX I2C pass through
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
//Enable data ready interrupt
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
//MPU6050_RA_DMP_INT_STATUS //Read-only
//MPU6050_RA_INT_STATUS 3A //Read-only
//MPU6050_RA_ACCEL_XOUT_H //Read-only
//MPU6050_RA_ACCEL_XOUT_L //Read-only
//MPU6050_RA_ACCEL_YOUT_H //Read-only
//MPU6050_RA_ACCEL_YOUT_L //Read-only
//MPU6050_RA_ACCEL_ZOUT_H //Read-only
//MPU6050_RA_ACCEL_ZOUT_L //Read-only
//MPU6050_RA_TEMP_OUT_H //Read-only
//MPU6050_RA_TEMP_OUT_L //Read-only
//MPU6050_RA_GYRO_XOUT_H //Read-only
//MPU6050_RA_GYRO_XOUT_L //Read-only
//MPU6050_RA_GYRO_YOUT_H //Read-only
//MPU6050_RA_GYRO_YOUT_L //Read-only
//MPU6050_RA_GYRO_ZOUT_H //Read-only
//MPU6050_RA_GYRO_ZOUT_L //Read-only
//MPU6050_RA_EXT_SENS_DATA_00 //Read-only
//MPU6050_RA_EXT_SENS_DATA_01 //Read-only
//MPU6050_RA_EXT_SENS_DATA_02 //Read-only
//MPU6050_RA_EXT_SENS_DATA_03 //Read-only
//MPU6050_RA_EXT_SENS_DATA_04 //Read-only
//MPU6050_RA_EXT_SENS_DATA_05 //Read-only
//MPU6050_RA_EXT_SENS_DATA_06 //Read-only
//MPU6050_RA_EXT_SENS_DATA_07 //Read-only
//MPU6050_RA_EXT_SENS_DATA_08 //Read-only
//MPU6050_RA_EXT_SENS_DATA_09 //Read-only
//MPU6050_RA_EXT_SENS_DATA_10 //Read-only
//MPU6050_RA_EXT_SENS_DATA_11 //Read-only
//MPU6050_RA_EXT_SENS_DATA_12 //Read-only
//MPU6050_RA_EXT_SENS_DATA_13 //Read-only
//MPU6050_RA_EXT_SENS_DATA_14 //Read-only
//MPU6050_RA_EXT_SENS_DATA_15 //Read-only
//MPU6050_RA_EXT_SENS_DATA_16 //Read-only
//MPU6050_RA_EXT_SENS_DATA_17 //Read-only
//MPU6050_RA_EXT_SENS_DATA_18 //Read-only
//MPU6050_RA_EXT_SENS_DATA_19 //Read-only
//MPU6050_RA_EXT_SENS_DATA_20 //Read-only
//MPU6050_RA_EXT_SENS_DATA_21 //Read-only
//MPU6050_RA_EXT_SENS_DATA_22 //Read-only
//MPU6050_RA_EXT_SENS_DATA_23 //Read-only
//MPU6050_RA_MOT_DETECT_STATUS //Read-only
//Slave out, dont care
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
//More slave config
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
//Reset sensor signal paths
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
//Motion detection control
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
//Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
//Sets clock source to gyro reference w/ PLL
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
//Controls frequency of wakeups in accel low power mode plus the sensor standby modes
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);
//MPU6050_RA_BANK_SEL //Not in datasheet
//MPU6050_RA_MEM_START_ADDR //Not in datasheet
//MPU6050_RA_MEM_R_W //Not in datasheet
//MPU6050_RA_DMP_CFG_1 //Not in datasheet
//MPU6050_RA_DMP_CFG_2 //Not in datasheet
//MPU6050_RA_FIFO_COUNTH //Read-only
//MPU6050_RA_FIFO_COUNTL //Read-only
//Data transfer to and from the FIFO buffer
LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
//MPU6050_RA_WHO_AM_I //Read-only, I2C address
}
