акселерометр не работает вместе с ультразвук. датчиками
- Войдите на сайт для отправки комментариев
Вс, 19/01/2020 - 20:23
без них все ок, но с ними при близком расстоянии програма ломается(микроконтроллер ни на что не реагирует) вот код.
Moderator : пожалуйста, вставьте код правильно (возможно, новым сообщением в тему),
P.S в #include newping&Wire.h
http://arduino.ru/forum/obshchii/vstavka-programmnogo-koda-v-temukommentarii
#include //VCC - 5V //GND - GND //SDA - A4 //SCL - A5 //INT - port-2 #include #define TRIGGER_PIN 12 #define ECHO_PIN 11 #define TRIGGER_PIN1 9 #define ECHO_PIN1 10 #define MAX_DISTANCE 60 //Declaring some global variables int gyro_x, gyro_y, gyro_z; long gyro_x_cal, gyro_y_cal, gyro_z_cal; boolean set_gyro_angles; long acc_x, acc_y, acc_z, acc_total_vector; float angle_roll_acc, angle_pitch_acc; float angle_pitch, angle_roll; int angle_pitch_buffer, angle_roll_buffer; float angle_pitch_output, angle_roll_output; long loop_timer; int temp; NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); NewPing sonar1(TRIGGER_PIN1, ECHO_PIN1, MAX_DISTANCE); void setup() { pinMode(3, OUTPUT); pinMode(5, OUTPUT); pinMode(6, INPUT); Wire.begin(); //Start I2C as master setup_mpu_6050_registers(); //Setup the registers of the MPU-6050 for (int cal_int = 0; cal_int < 1000 ; cal_int ++){ //Read the raw acc and gyro data from the MPU-6050 for 1000 times read_mpu_6050_data(); gyro_x_cal += gyro_x; //Add the gyro x offset to the gyro_x_cal variable gyro_y_cal += gyro_y; //Add the gyro y offset to the gyro_y_cal variable gyro_z_cal += gyro_z; //Add the gyro z offset to the gyro_z_cal variable delay(3); //Delay 3us to have 250Hz for-loop } // divide by 1000 to get avarage offset gyro_x_cal /= 1000; gyro_y_cal /= 1000; gyro_z_cal /= 1000; Serial.begin(115200); loop_timer = micros(); //Reset the loop timer } void loop(){ read_mpu_6050_data(); //Subtract the offset values from the raw gyro values gyro_x -= gyro_x_cal; gyro_y -= gyro_y_cal; gyro_z -= gyro_z_cal; //Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5) angle_pitch += gyro_x * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable angle_roll += gyro_y * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians angle_pitch += angle_roll * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel //Accelerometer angle calculations acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Calculate the total accelerometer vector //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Calculate the pitch angle angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch angle_roll_acc -= 0.0; //Accelerometer calibration value for roll if(set_gyro_angles){ //If the IMU is already started angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Correct the drift of the gyro roll angle with the accelerometer roll angle } else{ //At first start angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle set_gyro_angles = true; //Set the IMU started flag } //To dampen the pitch and roll angles a complementary filter is used angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; //Take 90% of the output pitch value and add 10% of the raw pitch value angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; //Take 90% of the output roll value and add 10% of the raw roll value Serial.print(" | Angle = "); Serial.println(angle_pitch_output); int uS=sonar.ping_cm(); int us=sonar1.ping_cm(); Serial.println(us); Serial.println(uS); if(fabs(angle_pitch_output*1.29)>25.0||(us<7&&us!=0)||(uS<7&&uS!=0)){ digitalWrite(3, LOW); }else{ digitalWrite(3, HIGH); } if(digitalRead(6)==1){ digitalWrite(5, HIGH); }else{ digitalWrite(5, LOW); } while(micros() - loop_timer < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop loop_timer = micros();//Reset the loop timer } void setup_mpu_6050_registers(){ //Activate the MPU-6050 Wire.beginTransmission(0x68); //Start communicating with the MPU-6050 Wire.write(0x6B); //Send the requested starting register Wire.write(0x00); //Set the requested starting register Wire.endTransmission(); //Configure the accelerometer (+/-8g) Wire.beginTransmission(0x68); //Start communicating with the MPU-6050 Wire.write(0x1C); //Send the requested starting register Wire.write(0x10); //Set the requested starting register Wire.endTransmission(); //Configure the gyro (500dps full scale) Wire.beginTransmission(0x68); //Start communicating with the MPU-6050 Wire.write(0x1B); //Send the requested starting register Wire.write(0x08); //Set the requested starting register Wire.endTransmission(); } void read_mpu_6050_data(){ //Subroutine for reading the raw gyro and accelerometer data Wire.beginTransmission(0x68); //Start communicating with the MPU-6050 Wire.write(0x3B); //Send the requested starting register Wire.endTransmission(); //End the transmission Wire.requestFrom(0x68,14); //Request 14 bytes from the MPU-6050 while(Wire.available() < 14); //Wait until all the bytes are received acc_x = Wire.read()<<8|Wire.read(); acc_y = Wire.read()<<8|Wire.read(); acc_z = Wire.read()<<8|Wire.read(); temp = Wire.read()<<8|Wire.read(); gyro_x = Wire.read()<<8|Wire.read(); gyro_y = Wire.read()<<8|Wire.read(); gyro_z = Wire.read()<<8|Wire.read(); }"Ни на что не реагирует" - в чём проявляется? В монитор порта ничего не выводит? Как минимум - надо посмотреть, что там внутри делает NewPing.
В том, что зуммер начинает пищать без остановки, в порт ничего не виводится, а фоторезистор не включает светодиод
Строка 136, кстати - совершенно избыточна, метод requestFrom возвращает кол-во принятых байт.
Так что делать?