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"); } }
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); }