final without sd card
This commit is contained in:
shinya
parent
9e25b917f6
commit
f7da4b9885
@ -1,7 +1,9 @@
|
||||
#include <SPI.h>
|
||||
#include <SD.h>
|
||||
#include <Servo.h>
|
||||
#include "Arduino_BMI270_BMM150.h"
|
||||
#include <Adafruit_MPU6050.h>
|
||||
#include <Adafruit_Sensor.h>
|
||||
#include <Wire.h>
|
||||
|
||||
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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user