foglar/CobraV2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update of positioning detection

Browse Source
This commit is contained in:
foglar 2024-06-29 14:27:35 +02:00
parent e214d215d9
commit 82fc3204b0
1 changed files with 203 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

313
sender_module_final/sender_module_final.ino
View File

@ -1,6 +1,6 @@
#include <SPI.h> #include <SPI.h>
#include <SD.h> #include <SD.h>
#include <Servo.h>
#include "Arduino_BMI270_BMM150.h" #include "Arduino_BMI270_BMM150.h"
#include <nRF24L01.h> #include <nRF24L01.h>
#include <RF24.h> #include <RF24.h>
@ -10,16 +10,21 @@ const int NRF_CE_PIN = 9;
const int NRF_CS_PIN = 8; const int NRF_CS_PIN = 8;
const byte address[6] = "00001"; const byte address[6] = "00001";
const int BUZZER_PIN = 7; const int BUZZER_PIN = 2;
const int READY_STAGE_PIN = 5; const int SERVO_A_PIN = 3;
const int LAUNCH_STAGE_PIN = 6; const int SERVO_B_PIN = 5;
int plusThreshold = 40, minusThreshold = -40;
// Create objects for modules // Create objects for modules
RF24 radio(NRF_CE_PIN, NRF_CS_PIN); RF24 radio(NRF_CE_PIN, NRF_CS_PIN);
File dataFile; File dataFile;
Servo A;
Servo B;
// Flight stages // Flight stages
enum FlightStage { enum FlightStage
{
READY, READY,
ARM, ARM,
ASCENT, ASCENT,
@ -29,134 +34,145 @@ enum FlightStage {
FlightStage current_stage = READY; FlightStage current_stage = READY;
void setup() { void setup()
{
// Initialize Serial // Initialize Serial
Serial.begin(115200); Serial.begin(115200);
while (!Serial); while (!Serial)
Serial.println("# Welcome to CobraV2 operating system for rocket"); ;
Serial.println("# Welcome to CobraV2 operating system for rocket");
delay(1000); delay(1000);
// Buzzer pin
pinMode(BUZZER_PIN, OUTPUT); pinMode(BUZZER_PIN, OUTPUT);
pinMode(READY_STAGE_PIN, INPUT); digitalWrite(BUZZER_PIN, LOW);
A.attach(5); // attaches the servo on pin 9 to the servo object
B.attach(6);
// Start with preflight stage // Start with preflight stage
current_stage = READY; current_stage = READY;
delay(1000); delay(1000);
} }
void loop() { void loop()
switch (current_stage) { {
case READY: switch (current_stage)
ready_stage(); {
break; case READY:
case ARM: ready_stage();
arm_stage(); break;
break; case ARM:
case ASCENT: arm_stage();
//ascent_stage(); break;
break; case ASCENT:
case DESCENT: ascent_stage();
//descent_stage(); break;
break; case DESCENT:
case LANDED: descent_stage();
//landed_stage(); break;
break; case LANDED:
landed_stage();
break;
} }
} }
bool isButtonPressed(int buttonPin) { return digitalRead(buttonPin) == HIGH;} bool isButtonPressed(int buttonPin) { return digitalRead(buttonPin) == LOW; }
void beep()
{
digitalWrite(BUZZER_PIN, HIGH);
delay(1000);
digitalWrite(BUZZER_PIN, LOW);
}
void ready_stage() { void ready_stage()
{
Serial.println("# READY stage"); Serial.println("# READY stage");
int counter = 0; beep();
const int buttonPin = 2;
while (true) { if (!IMU.begin())
if (isButtonPressed(buttonPin)) { {
Serial.println("Button is pressed!"); Serial.println("Failed to initialize IMU!");
break; while (1)
} else { ;
Serial.println("Button is not pressed.");
}
delay(100);
} }
// Radio
// radio.begin();
// radio.openWritingPipe(address);
// radio.stopListening(); // Set module as transmitter
// const char msg[] = "# Radio connection activated";
// radio.write(&msg, sizeof(msg));
// Serial.println("# Success nRF24L01 init");
//// SD card
// if (!SD.begin(4))
//{
// 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");
current_stage = ARM; current_stage = ARM;
} }
void arm_stage() { void arm_stage()
{
// System check // System check
// Block parachute ejection // Block parachute ejection
// Wait for launch pin removed // Wait for launch pin removed
// Start sending data // Start sending data
Serial.println("# ARM stage"); Serial.println("# ARM stage");
beep();
beep();
// Radio
if (!radio.begin()) {
Serial.println("# Fail nRF24L01 init");
} else {
radio.openWritingPipe(address);
radio.stopListening(); // Set module as transmitter
const char msg[] = "# Radio connection activated";
radio.write(&msg, sizeof(msg));
Serial.println("# Success nRF24L01 init");
}
// SD card
if (!SD.begin(4)) {
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");
}
// Buzzer
tone(BUZZER_PIN, 1000);
delay(1000);
noTone(BUZZER_PIN);
// Accelerometer Gyroscope
if (!IMU.begin()) {
Serial.println("Failed to initialize IMU!");
while (1);
}
Serial.print("Accelerometer sample rate = ");
Serial.print(IMU.accelerationSampleRate());
Serial.println(" Hz");
// Accelerometer Gyroscope // Accelerometer Gyroscope
int counter = 0; int counter = 0;
float x, y, z; float x, y, z;
while (true) { while (true)
if (IMU.accelerationAvailable()) { {
if (IMU.accelerationAvailable())
{
IMU.readAcceleration(x, y, z); IMU.readAcceleration(x, y, z);
Serial.println(x); Serial.println(x);
if ( x > 1) { if (x > 1)
{
Serial.println("# Launch Detect"); Serial.println("# Launch Detect");
counter++; counter++;
} }
else
{
counter = 0;
}
} }
if (counter > 10) { if (counter > 10)
{
Serial.println("# Launching"); Serial.println("# Launching");
break; break;
} }
@ -165,24 +181,101 @@ void arm_stage() {
current_stage = ASCENT; current_stage = ASCENT;
} }
void ascent_stage() { void ascent_stage()
// Launch {
// Start sending and saving data on SD card Serial.println("ASCENT Stage");
// Check for apogee unsigned long StartTime = millis();
// Eject parachute while (true)
{
unsigned long CurrentTime = millis();
unsigned long ElapsedTime = CurrentTime - StartTime;
Serial.println(ElapsedTime);
float x, y, z;
if (IMU.accelerationAvailable())
{
IMU.readAcceleration(x, y, z);
}
int degreesX = 0;
int degreesY = 0;
if (x > 0.1)
{
x = 100 * x;
degreesX = map(x, 0, 97, 0, 90);
Serial.print(degreesX);
}
if (x < -0.1)
{
x = 100 * x;
degreesX = map(x, 0, -100, 0, 90);
Serial.print(degreesX);
}
if (y > 0.1)
{
y = 100 * y;
degreesY = map(y, 0, 97, 0, 90);
Serial.print(degreesY);
}
if (y < -0.1)
{
y = 100 * y;
degreesY = map(y, 0, -100, 0, 90);
Serial.print(degreesY);
}
if (degreesX < 50)
{
Risk_Counter++;
}
if (ElapsedTime > 9000)
{
break;
}
}
current_stage = DESCENT;
} }
void descent_stage() { void descent_stage()
// Detect apogee with accelerometer {
// Eject parachute Serial.println("# DESCENT stage");
// GPS and height check for (int pos = 0; pos <= 150; pos += 1)
{
// in steps of 1 degree
A.write(pos);
delay(1);
}
for (int pos = 90; pos >= 0; pos -= 1)
{
B.write(pos);
delay(1);
}
unsigned long StartTime = millis();
while (true)
{
unsigned long CurrentTime = millis();
unsigned long ElapsedTime = CurrentTime - StartTime;
if (ElapsedTime > 300000)
{
break;
}
}
current_stage = LANDED;
} }
void landed_stage() { void landed_stage()
// Check for zero velocity {
// Shut down unneeded systems while (true)
// Buzz on {
// Send GPS location Serial.println("# LANDED stage");
// Battery check beep();
// Turn off gyro and accelerometer delay(200);
}
} }