int motion_init(void){ int ret = 1; Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, I2C_RATE_400); while(ret){ if(MPU6050_Test_I2C()){simplePrint("MPU is alive\n");} Setup_MPU6050(); delay(10); ret = MPU6050_Check_Registers(); } return(ret); }
int main(void) { Setup_I2C(); //SYSTEMConfigPerformance(SYSCLK); // Peripheral function that configures for best performance given the system's // clock frequency--this is for general practice. // This part is analogous to the void setup(){}; in Arduino. //TRISDCLR = 0x0100; // Set pin RD8 as an output, could be written as TRISD = 0xFEFF; // but takes more clock cycles to perform. //TRISESET = 0x0080; // Set pin RE7 (PROG button) as an input, could be written as TRISE = 0x0080; // but takes more clock cycles to perform. // This part is analogous to the void loop(){}; in Arduino. //SDA = SCL = 0; //SCL_IN = SDA_IN = 0; Setup_MPU6050(); Calibrate_Gyros(); MPU6050_Test_I2C(); while(1) // Loop forever... { // Reads the state of pin RE7, and latches RD8 accordingly. // Note that RE7 is normally set HIGH (3.3V) through an internal pull-up // resistor and will actually be set LOW when the PROG button is pressed. // LATDbits.LATD8 = PORTEbits.RE7; //Get_Accel_Angles(); //i2c_start(); // i2c_tx(0x68); //i2c_start(); //ok //i2c_stop(); //DelayMs(1000); //MPU6050_Test_I2C(); Get_Accel_Values(); Get_Accel_Angles(); GetTemp(); } return 0; // Included because main returns an int which is being expected. } // Make sure the program ends in a new line
/**************************************************************************//** * @brief Main function *****************************************************************************/ int main(void) { /* Setup SysTick Timer for 10 msec interrupts */ if (SysTick_Config(CMU_ClockFreqGet(cmuClock_CORE) / 1000)) { while (1) ; } /* Initialize the display module. */ DISPLAY_Init(); /* Retarget stdio to a text display. */ if (RETARGET_TextDisplayInit() != TEXTDISPLAY_EMSTATUS_OK) { while (1) ; } /* Output text on Memory LCD */ printf("Hello, EFM32 Zero Gecko world!"); Delay(2000); /* Clear screen */ printf("\f"); Setup_MPU6050(); MPU6050_Test_I2C(); MPU6050_Check_Registers(); Calibrate_Gyros(); Calibrate_Acc(); /* Update Memory LCD display forever */ while (1) { Get_Gyro_Rates(); Get_Accel_Values(); Delay(500); printf("\f"); } }
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() { /* Initialize LCD */ lcd_initialize(); /* Clear LCD */ lcd_clear(); /* Display message on LCD */ lcd_buffer_print(LCD_LINE2, " TEST "); /* Initialize motors */ Motors_Init(); /* Turn on motors relay */ Motors_On(); /* Send arm signal to motors */ Motor_Arm(MOTOR_UPPER); Motor_Arm(MOTOR_BOTTOM); /* Initialize servos */ Servos_Init(); /* Initialize sonar */ sonarInitialize(); //must be initialized before IIC, otherwise it will not work /* Setup the 12-bit A/D converter */ S12ADC_init(); /* Initialize I2C with control */ riic_ret_t iic_ret = RIIC_OK; iic_ret |= riic_master_init(); while (RIIC_OK != iic_ret) { nop(); /* Failure to initialize here means demo can not proceed. */ } /* Setup Compare Match Timer */ CMT_init(); /* Initialize PID structure used for PID properties */ PID_Init(&z_axis_PID, 0.7, 0.05, 0.30, dt, 0, 0.5); //0.7 0.05 0.15 PID_Init(&Pitch_PID, 1, 0, 0.01, dt, -30, 30); PID_Init(&Roll_PID, 1, 0, 0.01, dt, -30, 30); Init_AVG(0, &pitchAVG); Init_AVG(0, &rollAVG); /* Make the port connected to SW1 an input */ PORT4.PDR.BIT.B0 = 0; /*MPU6050 Initialization*/ MPU6050_Test_I2C(); Setup_MPU6050(); Calibrate_Gyros(); // Calibrate_Accel(); /*Kalman Initialization*/ init_Kalman(); //MS5611-01BA01 init // MS5611_Init(); desiredState.key.motor_diff_us = 0; desiredState.key.abs.pos.z = 0.20; altitudeValue = 200; mainWDT = WDT_Init(500, Fallback); WDT_Start(&mainWDT); sonarWDT = WDT_Init(60, Sonar_Fallback); WDT_Start(&sonarWDT); }