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

287
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,26 +34,31 @@ 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) { {
switch (current_stage)
{
case READY: case READY:
ready_stage(); ready_stage();
break; break;
@ -56,107 +66,113 @@ void loop() {
arm_stage(); arm_stage();
break; break;
case ASCENT: case ASCENT:
//ascent_stage(); ascent_stage();
break; break;
case DESCENT: case DESCENT:
//descent_stage(); descent_stage();
break; break;
case LANDED: case LANDED:
//landed_stage(); landed_stage();
break; 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);
}
current_stage = ARM;
}
void arm_stage() {
// System check
// Block parachute ejection
// Wait for launch pin removed
// Start sending data
Serial.println("# ARM stage");
// Radio // Radio
if (!radio.begin()) { // radio.begin();
Serial.println("# Fail nRF24L01 init"); // radio.openWritingPipe(address);
} else { // radio.stopListening(); // Set module as transmitter
radio.openWritingPipe(address); // const char msg[] = "# Radio connection activated";
radio.stopListening(); // Set module as transmitter // radio.write(&msg, sizeof(msg));
const char msg[] = "# Radio connection activated"; // Serial.println("# Success nRF24L01 init");
radio.write(&msg, sizeof(msg));
Serial.println("# Success nRF24L01 init");
}
// SD card //// SD card
if (!SD.begin(4)) { // if (!SD.begin(4))
Serial.println("# Fail SD module init"); //{
} else { // Serial.println("# Fail SD module init");
Serial.println("# Success SD module init"); // }
} // else
//{
if (SD.exists("data.txt")) { // Serial.println("# Success SD module init");
Serial.println("# File exists"); // }
} else { // if (SD.exists("data.txt"))
Serial.println("# File does not exist"); //{
} // Serial.println("# File exists");
// }
dataFile = SD.open("data.txt", FILE_WRITE); // else
if (dataFile) { //{
Serial.println("# File opened"); // Serial.println("# File does not exist");
dataFile.println("# CobraV2 flight data"); // dataFile = SD.open("data.txt", FILE_WRITE);
} else { // }
Serial.println("# Error opening file"); // if (dataFile)
} //{
// Serial.println("# File opened");
// Buzzer // dataFile.println("# CobraV2 flight data");
tone(BUZZER_PIN, 1000); // }
delay(1000); // else
noTone(BUZZER_PIN); //{
// Serial.println("# Error opening file");
// Accelerometer Gyroscope // }
if (!IMU.begin()) {
Serial.println("Failed to initialize IMU!");
while (1);
}
Serial.print("Accelerometer sample rate = "); Serial.print("Accelerometer sample rate = ");
Serial.print(IMU.accelerationSampleRate()); Serial.print(IMU.accelerationSampleRate());
Serial.println(" Hz"); Serial.println(" Hz");
current_stage = ARM;
}
void arm_stage()
{
// System check
// Block parachute ejection
// Wait for launch pin removed
// Start sending data
Serial.println("# ARM stage");
beep();
beep();
// 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);
} }
void descent_stage() { int degreesX = 0;
// Detect apogee with accelerometer int degreesY = 0;
// Eject parachute
// GPS and height check 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);
} }
void landed_stage() { if (degreesX < 50)
// Check for zero velocity {
// Shut down unneeded systems Risk_Counter++;
// Buzz on }
// Send GPS location
// Battery check if (ElapsedTime > 9000)
// Turn off gyro and accelerometer {
break;
}
}
current_stage = DESCENT;
}
void descent_stage()
{
Serial.println("# DESCENT stage");
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()
{
while (true)
{
Serial.println("# LANDED stage");
beep();
delay(200);
}
} }