static void init_mpu(MPU_handle mpu) { MPU_init_str mpu_init; mpu_init.acc_on = TRUE; mpu_init.gyro_on = TRUE; mpu_init.sample_rate_divider = 0x20; MPU_init(mpu, &mpu_init); }
bool initSensors() { #if defined(TRACE) Serial.println(">Start initSensors"); #endif i2c_init(); delay(100); if (!MPU_init()) return false; if (!Gyro_init()) return false ; if (!ACC_init()) return false; f.I2C_INIT_DONE = 1; #if defined(TRACE) Serial.println("<End OK initSensors"); #endif return true; }
bool initSensors() { #if defined(TRACE) Serial.println(">>Start initSensors"); #endif i2c_init(); delay(100); if (!MPU_init()) { #if defined(TRACE) Serial.println("<<End KO MPU_init"); #endif return false; } if (!Gyro_init()) { #if defined(TRACE) Serial.println("<<End KO Gyro_init"); #endif return false; } if (!ACC_init()) { #if defined(TRACE) Serial.println("<<End KO ACC_init"); #endif return false; } i2c_getTemperature(MPU6050_ADDRESS, 0x41); imu.temperature = (rawTemp[0] << 8) | rawTemp[1]; #if defined(TRACE) Serial.print("Temperature: "); Serial.println(((double)imu.temperature + 12412.0) / 340.0); Serial.println("<<End OK initSensors"); #endif return true; }
int main(void) { LED_INIT; //servoTx; sei(); USART_init(); MPU_init(); SERVO_init(); init_counters(); initvar(); SERVO_update_EEPROM(BROADCASTING_ID); wait(10); move_to_std(); wait_until_gyro_stable(); LED0_ON; USART_send_message("Gyro Stable"); // ------ TESTCODE FOR READING SERVO ------- //servoGoto(1, 3.14/3, 0x200); SERVO_update_EEPROM(BROADCASTING_ID); // NOTE: needs to run once for SERVO_get position to work //---------------------------- reset_counter_1(); set_counter_1(3000); uint8_t readyCounter = 0; while(1) { MPU_update(); if(USART_get_turn_flag()) { turn_degrees(USART_get_turn_angle(), USART_get_turn_dir()); } if(USART_get_climb_flag()) { climb(); } uint8_t r = USART_getRotation(); uint8_t s = USART_getSpeed(); uint8_t d = USART_getDirection(); if(s != 0 || r != 50) { std_pos_flag = 0; reset_counter_1(); readyCounter = 3; } move_robot(d, r, s); if(r == 50 && s == 0 && d == 0 && readyCounter) { cli(); USART_send_ready(); sei(); readyCounter--; } if(move_to_std_flag == 1) { move_to_std_flag = 0; move_to_std(); } /*climb(); for(int i = 0; i < 10; ++i) { move_robot(0,50,100); //wait(2000); } */ /* change_z(-120); move_to_std(); turn_degrees(180,1); // Takes a predecided number of steps forward // This is good when testing different things. wait(100); for(int i = 0; i < 10; ++i) { move_robot(0,50,100); //wait(2000); } */ USART_decode_rx_fifo(); } }