Update of positioning detection
This commit is contained in:
parent
e214d215d9
commit
82fc3204b0
@ -1,6 +1,6 @@
|
||||
#include <SPI.h>
|
||||
#include <SD.h>
|
||||
|
||||
#include <Servo.h>
|
||||
#include "Arduino_BMI270_BMM150.h"
|
||||
#include <nRF24L01.h>
|
||||
#include <RF24.h>
|
||||
@ -10,16 +10,21 @@ const int NRF_CE_PIN = 9;
|
||||
const int NRF_CS_PIN = 8;
|
||||
const byte address[6] = "00001";
|
||||
|
||||
const int BUZZER_PIN = 7;
|
||||
const int READY_STAGE_PIN = 5;
|
||||
const int LAUNCH_STAGE_PIN = 6;
|
||||
const int BUZZER_PIN = 2;
|
||||
const int SERVO_A_PIN = 3;
|
||||
const int SERVO_B_PIN = 5;
|
||||
|
||||
int plusThreshold = 40, minusThreshold = -40;
|
||||
|
||||
// Create objects for modules
|
||||
RF24 radio(NRF_CE_PIN, NRF_CS_PIN);
|
||||
File dataFile;
|
||||
Servo A;
|
||||
Servo B;
|
||||
|
||||
// Flight stages
|
||||
enum FlightStage {
|
||||
enum FlightStage
|
||||
{
|
||||
READY,
|
||||
ARM,
|
||||
ASCENT,
|
||||
@ -29,26 +34,31 @@ enum FlightStage {
|
||||
|
||||
FlightStage current_stage = READY;
|
||||
|
||||
void setup() {
|
||||
void setup()
|
||||
{
|
||||
// Initialize Serial
|
||||
Serial.begin(115200);
|
||||
|
||||
while (!Serial);
|
||||
while (!Serial)
|
||||
;
|
||||
Serial.println("# Welcome to CobraV2 operating system for rocket");
|
||||
delay(1000);
|
||||
|
||||
// Buzzer pin
|
||||
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
|
||||
current_stage = READY;
|
||||
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
switch (current_stage) {
|
||||
void loop()
|
||||
{
|
||||
switch (current_stage)
|
||||
{
|
||||
case READY:
|
||||
ready_stage();
|
||||
break;
|
||||
@ -56,107 +66,113 @@ void loop() {
|
||||
arm_stage();
|
||||
break;
|
||||
case ASCENT:
|
||||
//ascent_stage();
|
||||
ascent_stage();
|
||||
break;
|
||||
case DESCENT:
|
||||
//descent_stage();
|
||||
descent_stage();
|
||||
break;
|
||||
case LANDED:
|
||||
//landed_stage();
|
||||
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");
|
||||
int counter = 0;
|
||||
const int buttonPin = 2;
|
||||
beep();
|
||||
|
||||
while (true) {
|
||||
if (isButtonPressed(buttonPin)) {
|
||||
Serial.println("Button is pressed!");
|
||||
break;
|
||||
} else {
|
||||
Serial.println("Button is not pressed.");
|
||||
if (!IMU.begin())
|
||||
{
|
||||
Serial.println("Failed to initialize IMU!");
|
||||
while (1)
|
||||
;
|
||||
}
|
||||
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
|
||||
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");
|
||||
}
|
||||
// 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");
|
||||
}
|
||||
|
||||
// Buzzer
|
||||
tone(BUZZER_PIN, 1000);
|
||||
delay(1000);
|
||||
noTone(BUZZER_PIN);
|
||||
|
||||
// Accelerometer Gyroscope
|
||||
if (!IMU.begin()) {
|
||||
Serial.println("Failed to initialize IMU!");
|
||||
while (1);
|
||||
}
|
||||
//// 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;
|
||||
}
|
||||
|
||||
void arm_stage()
|
||||
{
|
||||
// System check
|
||||
// Block parachute ejection
|
||||
// Wait for launch pin removed
|
||||
// Start sending data
|
||||
Serial.println("# ARM stage");
|
||||
beep();
|
||||
beep();
|
||||
|
||||
// Accelerometer Gyroscope
|
||||
int counter = 0;
|
||||
float x, y, z;
|
||||
|
||||
while (true) {
|
||||
if (IMU.accelerationAvailable()) {
|
||||
while (true)
|
||||
{
|
||||
if (IMU.accelerationAvailable())
|
||||
{
|
||||
IMU.readAcceleration(x, y, z);
|
||||
|
||||
Serial.println(x);
|
||||
if ( x > 1) {
|
||||
if (x > 1)
|
||||
{
|
||||
Serial.println("# Launch Detect");
|
||||
counter++;
|
||||
}
|
||||
else
|
||||
{
|
||||
counter = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (counter > 10) {
|
||||
if (counter > 10)
|
||||
{
|
||||
Serial.println("# Launching");
|
||||
break;
|
||||
}
|
||||
@ -165,24 +181,101 @@ void arm_stage() {
|
||||
current_stage = ASCENT;
|
||||
}
|
||||
|
||||
void ascent_stage() {
|
||||
// Launch
|
||||
// Start sending and saving data on SD card
|
||||
// Check for apogee
|
||||
// Eject parachute
|
||||
void ascent_stage()
|
||||
{
|
||||
Serial.println("ASCENT Stage");
|
||||
unsigned long StartTime = millis();
|
||||
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() {
|
||||
// Detect apogee with accelerometer
|
||||
// Eject parachute
|
||||
// GPS and height check
|
||||
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() {
|
||||
// Check for zero velocity
|
||||
// Shut down unneeded systems
|
||||
// Buzz on
|
||||
// Send GPS location
|
||||
// Battery check
|
||||
// Turn off gyro and accelerometer
|
||||
void landed_stage()
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
Serial.println("# LANDED stage");
|
||||
beep();
|
||||
delay(200);
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user