From f7da4b9885d8858b22d899381fbc305ac3b13886 Mon Sep 17 00:00:00 2001 From: shinya Date: Sun, 22 Jun 2025 17:32:17 +0200 Subject: [PATCH] final without sd card --- sender/sender_module_final.ino | 233 +++++++++++---------------------- 1 file changed, 76 insertions(+), 157 deletions(-) diff --git a/sender/sender_module_final.ino b/sender/sender_module_final.ino index 8de22fe..0b56a91 100644 --- a/sender/sender_module_final.ino +++ b/sender/sender_module_final.ino @@ -1,7 +1,9 @@ #include #include #include -#include "Arduino_BMI270_BMM150.h" +#include +#include +#include const int BUZZER_PIN = 2; const int SERVO_A_PIN = 7; @@ -10,10 +12,10 @@ const int CHIP_SELECT = 4; // Create objects for modules File dataFile; Servo A; +Adafruit_MPU6050 mpu; // Flight stages -enum FlightStage -{ +enum FlightStage { READY, ARM, ASCENT, @@ -23,14 +25,8 @@ enum FlightStage FlightStage current_stage = READY; -void setup() -{ - // Initialize Serial +void setup() { Serial.begin(115200); - - //while (!Serial) - // ; - //Serial.println("# Welcome to CobraV2"); delay(1000); pinMode(BUZZER_PIN, OUTPUT); @@ -38,198 +34,123 @@ void setup() A.attach(SERVO_A_PIN); A.write(80); - // Start with preflight stage current_stage = READY; + // Initialize MPU6050 + if (!mpu.begin()) { + Serial.println("Failed to find MPU6050 chip"); + while (1) delay(10); + } + + mpu.setAccelerometerRange(MPU6050_RANGE_16_G); + mpu.setGyroRange(MPU6050_RANGE_250_DEG); + mpu.setFilterBandwidth(MPU6050_BAND_21_HZ); + delay(1000); } -void loop() -{ - switch (current_stage) - { - case READY: - ready_stage(); - break; - case ARM: - arm_stage(); - break; - case ASCENT: - ascent_stage(); - break; - case DESCENT: - descent_stage(); - break; - case LANDED: - landed_stage(); - break; +void loop() { + switch (current_stage) { + case READY: + ready_stage(); + break; + case ARM: + arm_stage(); + break; + case ASCENT: + ascent_stage(); + break; + case DESCENT: + descent_stage(); + break; + case LANDED: + landed_stage(); + break; } } -void beep() -{ +void beep() { digitalWrite(BUZZER_PIN, HIGH); delay(1000); digitalWrite(BUZZER_PIN, LOW); } -void ready_stage() -{ +void ready_stage() { Serial.println("# READY stage"); analogWrite(LED_BUILTIN, HIGH); beep(); - if (!IMU.begin()) - { - Serial.println("Failed to initialize IMU!"); - while (1) - ; - } - - // SD card - //if (!SD.begin(CHIP_SELECT)) - //{ - // Serial.println("# Fail SD module init"); - //} - //else - //{ - // Serial.println("# Success SD module init"); - //} -// - //if (SD.exists("data.txt")) - //{ - // Serial.println("# File exists"); - //} - //else - //{ - // Serial.println("# File does not exist"); - // dataFile = SD.open("data.txt", FILE_WRITE); - //} -// - //if (dataFile) - //{ - // Serial.println("# File opened"); - // dataFile.println("# CobraV2 flight data"); - //} - //else - //{ - // Serial.println("# Error opening file"); - //} - - Serial.print("# Accelerometer sample rate = "); - Serial.print(IMU.accelerationSampleRate()); - Serial.println(" Hz"); - - //dataFile.print("# Accelerometer sample rate = "); - //dataFile.println(IMU.accelerationSampleRate()); - //dataFile.close(); - + Serial.println("# MPU6050 initialized"); current_stage = ARM; } -void arm_stage() -{ - // System check - // Block parachute ejection - // Wait for launch pin removed - // Start sending data +void arm_stage() { Serial.println("# ARM stage"); beep(); beep(); - // Accelerometer Gyroscope int counter = 0; - float x, y, z; - bool toggle = false; - //dataFile = SD.open("data.txt", FILE_WRITE); - //dataFile.println("# ARM stage"); - while (true) - { - if (IMU.accelerationAvailable()) - { - IMU.readAcceleration(x, y, z); - Serial.print(x); - Serial.print(", "); - Serial.print(y); - Serial.print(", "); - Serial.println(z); + sensors_event_t a, g, temp; + while (true) { + mpu.getEvent(&a, &g, &temp); - //dataFile.print(x); - //dataFile.print(" "); - //dataFile.print(y); - //dataFile.print(" "); - //dataFile.println(z); + float x = a.acceleration.x; + float y = a.acceleration.y; + float z = a.acceleration.z; - if (x > 1) - { - Serial.println("# Launch Detect"); - counter++; - } - else - { - counter = 0; - } + Serial.print(x); Serial.print(", "); + Serial.print(y); Serial.print(", "); + Serial.println(z); + + if (x > 1.0) { + Serial.println("# Launch Detect"); + counter++; + } else { + counter = 0; } - if (counter < 15) - { + if (counter >= 15) { Serial.println("# Launching"); - //dataFile.println("# Launching"); break; } - if (toggle) - { - digitalWrite(LED_BUILTIN, HIGH); - } - else - { - digitalWrite(LED_BUILTIN, LOW); - } + digitalWrite(LED_BUILTIN, toggle ? HIGH : LOW); toggle = !toggle; + delay(10); } current_stage = ASCENT; - //dataFile.close(); } -void ascent_stage() -{ - Serial.println("# ASCENT Stage"); +void ascent_stage() { + Serial.println("# ASCENT stage"); unsigned long StartTime = millis(); - int FailOrientationCounter = 0; - //File dataFile = SD.open("data.txt", FILE_WRITE); - //dataFile.println("# ASCENT stage"); - while (true) - { + + while (true) { unsigned long CurrentTime = millis(); unsigned long ElapsedTime = CurrentTime - StartTime; Serial.println(ElapsedTime); - if (IMU.gyroscopeAvailable()) - { - float x, y, z; - IMU.readGyroscope(x, y, z); - } + sensors_event_t a, g, temp; + mpu.getEvent(&a, &g, &temp); - if ((ElapsedTime > 9000)) - { - dataFile.println("# Apogee detected by time"); + // Add orientation failure logic here if needed + + if (ElapsedTime > 9000) { + Serial.println("# Apogee detected by time"); break; } + + delay(10); } current_stage = DESCENT; - //dataFile.close(); } -void descent_stage() -{ +void descent_stage() { Serial.println("# DESCENT stage"); - //File dataFile = SD.open("data.txt", FILE_WRITE); - //dataFile.println("# DESCENT stage"); A.write(80); delay(1000); A.write(180); @@ -237,25 +158,23 @@ void descent_stage() Serial.println("# Parachute deployed"); unsigned long StartTime = millis(); - while (true) - { + while (true) { unsigned long CurrentTime = millis(); unsigned long ElapsedTime = CurrentTime - StartTime; - if (ElapsedTime > 300000) - { - //dataFile.println("# Landing detected"); + if (ElapsedTime > 300000) { + Serial.println("# Landing detected"); break; } + + delay(1000); } + current_stage = LANDED; - //dataFile.close(); } -void landed_stage() -{ - while (true) - { +void landed_stage() { + while (true) { Serial.println("# LANDED stage"); beep(); delay(200);