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