/*
* The self test data transmission (MAKE_TWI_TRAFFIC_FOR_SELF_TEST).
* Communicate normally on the TWI interface.
* The necessary wires from TWI to the sniffer porst must be connected to self-test.
*/
void noiseTask() {
static uint32_t makeNoise = 0;
static uint8_t noise = 0;
if (makeNoise++ > 500000UL) {
makeNoise = 0;
MPU6050_init(noise++);
if (noise == 1)
noise = 0;
}
}
int main (void){
float acc_x, acc_z, gyro_x;
float acc_angle,kal_angle;
/* Init Systick to 1ms */
SysTick_Config( SystemCoreClock / 1000);
/* Initialize GPIO (sets up clock) */
LPC_SYSCON->SYSAHBCLKCTRL |= (1<<6);
SERVO_init();
if(I2CInit(I2CMASTER) == FALSE){
while(1); /* fatal error */
}
if(MPU6050_init()){
return 0;
}
MPU6050_setZero();
kalman_init();
for(;;){
/* 100Hz loop */
if(gSysTick_10 >= 9){
gSysTick_10 = 0;
/* get sensor values */
gyro_x = MPU6050_getGyroRoll_degree();
acc_x = MPU6050_getAccel_x();
acc_z = -MPU6050_getAccel_z();
/* acc angle */
acc_angle = atan2(acc_x , -acc_z) * 180/3.14159 ; // calculate accel angle
kal_angle = kalman_update(acc_angle,gyro_x, 0.01);
SERVO_set_slew((-kal_angle) - MECH_OFFSET);
}
}
}
