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 }