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);
}
void motion_calibrate(void){
Calibrate_Gyros();
simplePrint("Gyros calibrated\n");
}
