Niestabilny program z tablicą

woytas · Kwiecień 21, 2020

Witam serdecznie,

Mam problem ze stabilnością programu. Do płytki Arduino podłączyłem barometr do pomiaru wysokości i wyświetlacz LCD. Odczytuję wartość ciśnienia a następnie przeliczam ją na wysokość. Aby wygładzić wykres sygnału postanowiłem stworzyć tablicę w której będzie przechowywane kilkadziesiąt ostatnich wartości pomiarowych a następnie będą uśredniane. Problem objawia się w ten sposób, że gaśnie ekran LCD - tzn. nic się na nim nie wyświetla po kilku sekundach od rozpoczęcia działania programu, ale reszta działa poprawnie. Jeżeli zakomentuję polecenie w pętli:

for (int i = historyLength; i > 0; i--){
  //altitudeHistory[i] = altitudeHistory[i-1];
}

(reszta kodu poniżej) to wyświetlacz działa cały czas normalnie. Wydaje mi się że jest tu problem z przepełnieniem pamięci SRAM. Wiem że istnieje coś takiego jak wskaźniki i chyba mogłyby pomóc, ale nie wiem jak je wykorzystać.

Oto deklaracje zmiennch:

float altitude = 0.;
float sum = 0;
byte historyLength = 0;
byte readingsNumber = 50;
int altitudeHistory[49];

A oto fragment pętli loop:

  //pressure measurement
  for (int i = historyLength; i > 0; i--){
    altitudeHistory[i] = altitudeHistory[i-1];
  }
  if (historyLength < readingsNumber) {
    historyLength ++;
  }
  altitudeHistory[0] = altitude * 100;
  pressure = baro.getPressure();
  altitude = ((float)pressureRef - (float)pressure) / (airDensity * 9.81);
  for (int i = 0; i < historyLength; i++){
    sum += altitudeHistory[i];
  }  
  sum += altitude * 100;
  smoothedAltitude = sum / (historyLength + 1);
  sum = 0;

Jedno wykonanie pętli trwa około 13 ms, więc jest dosyć dużo operacji przepisywania wartości w tablicy i raczej stąd się bierze ten błąd. Wie ktoś jak to naprawić?

ethanak · Kwiecień 21, 2020

Zapomniałeś dodać cały kod, więc mogę tylko gdybać.

Pytanie kontrolne: dlaczego tablica przechowująca 50 wyników ma rozmiar 49?

woytas · Kwiecień 21, 2020

25 minut temu, ethanak napisał:

Pytanie kontrolne: dlaczego tablica przechowująca 50 wyników ma rozmiar 49?

To mój błąd. Sprawdzałem czy wcześniej przestanie działać i nie poprawiłem przed wklejeniem na forum. Nie to jest problemem.

Nie wklejałem całego kodu, bo jest wydawał mi się trochę za długi, ale ok. Oto całość:

#include <LiquidCrystal.h>
#include <Servo.h>
#include <MS5611.h>
#include <EEPROM.h>
MS5611 baro;

#define ledGreen 2
#define lcdLed 12
#define knobPIN A0
#define batteryPIN A1
#define powerPIN A2
#define buzzerPIN 13
#define testButton 10
#define lcdLedButton 11
#define servo1 3

int32_t pressure = 1;
int32_t filtered = 0;
int32_t pressureRef = pressure;
float sum = 0;
float altitude = 0.;
byte historyLength = 0;
byte readingsNumber = 50; //for averaging function
int altitudeHistory[50]; //in centimeters! - for saving SRAM. should equal readingsNumber - 1
int smoothedAltitude = 0; //in centimeters!
float maxAltitude = 0.;
float altitudeTreshold = 1.2; //minimal height to turn on flight mode - gather telemetry and get ready to open valve
byte certaintyTreshold = 3; //number of readings exceeding treshold to avoid turning on flight mode by accident
float descentDistanceTreshold = 2.5;
byte certainty = certaintyTreshold; //temporary certainty
bool flightMode = false; //turns on after altitude treshhold is exceeded
bool descentMode = false;
bool landedMode = false;
float temperature = 20.;
float airDensity = 1.2;
float saturationPressure = 0;
int relativeHumidity = 50; //percentage value for calculating air density
int DataArrayLength = 450;
int altitudeData[450];
int eeAdress = 0; //EEPROM memory starting adress
int j = 0; //data order number;

Servo servo;
int servoStartPosition = 0;
int knobPosition;
int powerMeasure;
int batMeasure = 0; //battery voltage measurement
float battery = 0.; //battery voltage
float power = 0.;
int ignitionDelay = 6000; //in milliseconds (insert only multiples of 1000)
int timeToIgnition = ignitionDelay; //in milliseconds
float countdown = ignitionDelay / 1000; //in seconds - is visible on screen
int lcdRefreshDelay = 500; //in milliseconds
int ledRefreshDelay = 2000; //peroid of blinking LED in "ready to start" mode
int lcdLedDelay = 5000; //LCD LED on time
unsigned long lcdTime = millis();
unsigned long currentTime = millis();
unsigned long currentTimeMicros = micros();
unsigned long ledTime = millis();
unsigned long lcdLedTime = millis();
unsigned long loopTime = 0;
int R1 = 1000; //first resistor in voltage divider, for measuring battery voltage
int R2 = 1000; //second resistor in voltage divider, for measuring battery voltage
int frequency = 3000; //for buzzer
byte buzzLength = 25;
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);


void setup()
{
  while (millis() - currentTime < 100); //wait to load libraries
  pinMode(ledGreen, OUTPUT);
  pinMode(lcdLed, OUTPUT);
  Serial.begin(9600);
  servo.attach(servo1);
  pinMode(testButton, INPUT_PULLUP);
  pinMode(lcdLedButton, INPUT_PULLUP);

  //Start barometer
  baro = MS5611();
  baro.begin();
  pressure = baro.getPressure();
  resetTemperature();
  resetAltitude();
  airDensity = (float)pressureRef / (289.6 * ((float)temperature + 273.15)); //289.6 is gas constant for air. Temperature must be converted from Celcius to Kelvin

  lcd.begin(16, 2);
  lcd.display();
  loadingScreen();
}

void loop() {
  loopTime = micros() - currentTimeMicros;
  currentTimeMicros = micros();
  currentTime = millis();
  readParameters();

  if (currentTime - lcdTime > lcdRefreshDelay) {
    printParameters();
    lcdTime = millis();
  }

  //BUTTON - Opening valve test / press loger to print flight data by UART / UP BUTTON
  if (digitalRead(testButton) == LOW) {
    while (digitalRead(testButton) == LOW) {
      if (digitalRead(testButton) == HIGH) testOpenValve();
      if (millis() - currentTime > 2000 && digitalRead(testButton) == LOW) printFlightData();
    }
  }

  //BUTTON - LCD LED on with timer / DOWN BUTTON
  if (digitalRead(lcdLedButton) == LOW) {
    digitalWrite(lcdLed, HIGH);
    lcdLedTime = millis();
    while (digitalRead(lcdLedButton) == LOW){
      if(millis() - currentTime > 2000){
        writeDataToEEPROM();
      }
    }
  }
  else if (currentTime > lcdLedTime && currentTime - lcdLedTime > lcdLedDelay) {
    digitalWrite(lcdLed, LOW);
  }

  //BUTTON - show battery voltage and reset altitude / press longer to change number of readings
  //which are averaged and k factor. Also resets pressureRef, altitude and altitude data storage
  if (analogRead(batteryPIN) > 5) {
    currentTime = millis();
    while (analogRead(batteryPIN) > 5) {
      showBatteryVoltage();
      if (millis() - currentTime > 3000) {
        //setReadingsNumber();
        setAltitudeTreshold();
        setDescentDistanceTreshold();
      }
    }
    resetAltitude();
  }

  //Mode detection
  if (!landedMode){
    if (!flightMode) flightModeDetection();
    if (flightMode) descentModeDetection();
  }
  
  //LED1 blinking and buzzer beep - idle mode
  if (flightMode) ledRefreshDelay = 500;
  else ledRefreshDelay = 2000;
  if (landedMode) buzzLength = 200;
  else buzzLength = 25;
  if (currentTime - ledTime < ledRefreshDelay - 20 && servoStartPosition < 175) {
    digitalWrite(ledGreen, LOW);
  }
  else {
    digitalWrite(ledGreen, HIGH);
    if (currentTime - ledTime > ledRefreshDelay) {
      ledTime = millis();
      tone(buzzerPIN, frequency, buzzLength);
    }
  }
}
////////////////////////////////////////////////////////////////////////////////
//functions:

void readParameters() {
  knobPosition = analogRead(knobPIN);
  servoStartPosition = map(knobPosition, 0, 929, 0, 180);

  servo.write(servoStartPosition);
  if (servoStartPosition > 175) {
    digitalWrite(ledGreen, HIGH);
  }

  powerMeasure = analogRead(powerPIN);
  power = map(powerMeasure, 0, 1023, 0, 500);
  power /= 100;

  //pressure measurement
  for (int i = historyLength; i > 0; i--){
    altitudeHistory[i] = altitudeHistory[i-1];
  }
  if (historyLength < readingsNumber) {
    historyLength ++;
  }
  altitudeHistory[0] = altitude * 100;
  pressure = baro.getPressure();
  altitude = ((float)pressureRef - (float)pressure) / (airDensity * 9.81);

  for (int i = 0; i < historyLength; i++){
    sum += altitudeHistory[i];
  }
  sum += altitude * 100;
  smoothedAltitude = sum; // (historyLength + 1);
  sum = 0;
  if ((float)smoothedAltitude / 100 > maxAltitude) maxAltitude = (float)smoothedAltitude / 100;

  //store telemetry
  if (flightMode) {
    if (j < DataArrayLength) {
      altitudeData[j] = smoothedAltitude; // in centimeters (int uses less mamory)
      j++;
    }
    if (j == DataArrayLength) {
      sadBuzz();
      j++;
    }
  }
}

void printParameters() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("H:"));
  lcd.print(maxAltitude);
  lcd.setCursor(7, 0);
  lcd.print(F("a:"));
  lcd.print(smoothedAltitude);
  lcd.setCursor(0, 1);
  lcd.print(F("p:"));
  lcd.print(pressure);
  lcd.print(F(" h:"));
  lcd.print(altitude);
  lcd.print(F("m"));

  Serial.print(F("servo: "));
  Serial.print(servoStartPosition);
  Serial.print(F(", knob: "));
  Serial.print(knobPosition);
  Serial.print(F(", power: "));
  Serial.print(power);
  Serial.print(F("V"));
  Serial.print(F(", A2: "));
  Serial.print(powerMeasure);
  Serial.print(F(", pressure: "));
  Serial.print(pressure);
  Serial.print(F(", Ref: "));
  Serial.print(pressureRef);
  Serial.print(F(", altitude: "));
  Serial.print(altitude);
  Serial.print(F("m"));
  Serial.print(F(" ,smoothed: "));
  Serial.print(smoothedAltitude);
  Serial.print(F(" ,max: "));
  Serial.print(maxAltitude);
  Serial.print(F(", loop time: "));
  Serial.print(loopTime);
  Serial.print(F("us"));
  Serial.print(F(", FM: "));
  Serial.print(flightMode);
  Serial.print(F(", altitude Data length: "));
  Serial.println(j);
}

void showBatteryVoltage() {
  batMeasure = analogRead(batteryPIN);
  battery = map(batMeasure, 0, 916, 0, 4500);
  battery = battery * ((R1 + R2) / R1) / 1000;
  readParameters();

  //temperature measurement
  for (int i = 0; i < readingsNumber; i++) {
    temperature = baro.getTemperature();
    sum += temperature;
  }
  sum /= readingsNumber;
  temperature = sum;
  sum = 0;
  temperature /= 100;

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("P:"));
  lcd.print(power);
  lcd.print(F("V"));
  lcd.print(F(" t:"));
  lcd.print(temperature);
  lcd.setCursor(0, 1);
  lcd.print(F("b:"));
  lcd.print(battery);
  lcd.print(F("V"));
  lcd.print(F(" air:"));
  lcd.print(airDensity);
  //lcd.print(F(" A1:"));
  //lcd.print(batMeasure);

  Serial.print(F("batMeasure: "));
  Serial.print(batMeasure);
  Serial.print(F(", battery: "));
  Serial.print(battery);
  Serial.print(F("V"));
  Serial.print(F(", temperature: "));
  Serial.print(temperature);
  Serial.print(F(", air density: "));
  Serial.print(airDensity);
  Serial.println(F("kg/m^3"));
  delay(lcdRefreshDelay);
}

void setReadingsNumber() {
  currentTime = millis();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("press. readings"));
  lcd.setCursor(0, 1);
  lcd.print(F("per loop: "));

  while (millis() - currentTime < 2000) {
    if (digitalRead(testButton) == LOW) {
      readingsNumber ++;
      delay(250);
      currentTime = millis();
    }
    if (digitalRead(lcdLedButton) == LOW) {
      readingsNumber --;
      delay(250);
      currentTime = millis();
    }
    lcd.setCursor(10, 1);
    lcd.print(readingsNumber);
    lcd.print(F("      "));
  }
  buzz();
}

void setAltitudeTreshold() {
  currentTime = millis();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("set altitude"));
  lcd.setCursor(0, 1);
  lcd.print(F("treshold: "));

  while (millis() - currentTime < 2000) {
    if (digitalRead(testButton) == LOW) {
      altitudeTreshold += 0.2;
      delay(250);
      currentTime = millis();
    }
    if (digitalRead(lcdLedButton) == LOW) {
      altitudeTreshold -= 0.2;
      delay(250);
      currentTime = millis();
    }
    lcd.setCursor(11, 1);
    lcd.print(altitudeTreshold);
  }
  buzz();
}

void setDescentDistanceTreshold() {
  currentTime = millis();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("set descent dist"));
  lcd.setCursor(0, 1);
  lcd.print(F("treshold: "));

  while (millis() - currentTime < 2000) {
    if (digitalRead(testButton) == LOW) {
      descentDistanceTreshold += .2;
      delay(250);
      currentTime = millis();
    }
    if (digitalRead(lcdLedButton) == LOW) {
      descentDistanceTreshold -= .2;
      delay(250);
      currentTime = millis();
    }
    lcd.setCursor(11, 1);
    lcd.print(descentDistanceTreshold);
  }
  buzz();
}

void resetAltitude() {
  sum = 0;
  for (int i = 0; i < 50; i++) {
    sum += baro.getPressure();
  }
  pressureRef = sum / 50 + 6;
  filtered = pressure;
  sum = 0;
  maxAltitude = 0;
  j = 0;
  
  //calculation of humid air density based on:
  //https://en.wikipedia.org/wiki/Density_of_air#Humid_air
  airDensity = (float)temperature * -0.00411 + 1.2867;
  pressure = airDensity * 287.058 * ((float)temperature + 273.15);
  saturationPressure = 6.1078 * pow(10, 7.5 * (float)temperature / ((float)temperature + 237.3));
  airDensity = (pressure * 0.0289654 + (float)relativeHumidity * saturationPressure * 0.018016) / (8.314 * ((float)temperature + 273.15)); //8.314 is universal gas constant
  pressure = pressureRef;
  
  for (int i = 0; i < DataArrayLength; i++) {
    altitudeData[i] = 0;
  }
  for (int i = 0; i < readingsNumber; i++) {
    altitudeHistory[i] = 0;
  }
  historyLength = 0;
  flightMode = false;
  descentMode = false;
  landedMode = false;
}

void resetTemperature() {
  sum = 0;
  for (int i = 0; i < 20; i++) {
    sum += baro.getTemperature();
  }
  sum /= 20;
  temperature = sum;
  temperature /= 100;
  sum = 0;
}

void testOpenValve() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("landing burn"));
  lcd.setCursor(0, 1);
  lcd.print(F("start in: "));
  while (timeToIgnition > 0) {
    currentTime = millis();
    timeToIgnition -= 100;
    countdown -= 0.1;
    lcd.print(countdown);
    lcd.setCursor(13, 1);
    lcd.print(F("sec"));
    lcd.setCursor(10, 1);
    if (timeToIgnition > 1000 && timeToIgnition % 1000 == 0) {
      digitalWrite(ledGreen, HIGH);
      tone(buzzerPIN, frequency, buzzLength);
    }
    if (timeToIgnition > 1000 && timeToIgnition % 1000 == 900) digitalWrite(ledGreen, LOW);
    if (timeToIgnition <= 1000 && timeToIgnition % 200 == 0) {
      digitalWrite(ledGreen, HIGH);
      tone(buzzerPIN, frequency, buzzLength);
    }
    if (timeToIgnition < 1000 && timeToIgnition % 200 == 100) digitalWrite(ledGreen, LOW);
    while (millis() - currentTime < 100); //wait for 100 miliseconds
  }
  servo.write(180);
  timeToIgnition = ignitionDelay;
  countdown = ignitionDelay / 1000;
  delay(500);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("HYDRA has"));
  lcd.setCursor(0, 1);
  lcd.print(F("landed! =D"));
  buzz();
  delay(3000);
}

void openValve() {
  servo.write(180);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("landing burn"));
  lcd.setCursor(0, 1);
  lcd.print(F("at:"));
  lcd.print(altitude);
  lcd.print(F("m"));
  buzz();
  delay(3000);
}

void flightModeDetection() {
  if ((float)smoothedAltitude / 100 > altitudeTreshold) {
    certainty --;
    if (certainty == 0){
      flightMode = true;
      certainty = certaintyTreshold;
    }
  }
  else certainty = certaintyTreshold;
}

void descentModeDetection() {
  if (maxAltitude - (float)smoothedAltitude / 100 > descentDistanceTreshold) {
    certainty --;
    if (certainty == 0){
      certainty = certaintyTreshold;
      flightMode = false;
      descentMode = false;
      landedMode = true;
      openValve();
    }
  }
}

void printFlightData() {
  int eeData;
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("printing RAMdata"));
  
  for (int i = 1; i < DataArrayLength; i++) {
    EEPROM.get(eeAdress, eeData);
    Serial.print(i);
    Serial.print(F(", altitude,"));
    Serial.println(eeData);
    Serial.println(altitudeData[i]);
    lcd.setCursor(0, 1);
    lcd.print(i);
    eeAdress += 2;
  }
  eeAdress = 0;
  delay(250);
  buzz();
  lcd.print(F(" complete!"));
  delay(1000);
}

void writeDataToEEPROM(){
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("writing data to"));
  lcd.setCursor(0, 1);
  lcd.print(F("EEPROM:"));
  lcd.setCursor(9, 1);

  for (int i = 0; i < DataArrayLength; i++){
    EEPROM.put(eeAdress, altitudeData[i]);
    lcd.setCursor(8, 1);
    lcd.print(i);
    eeAdress += 2;
  }
  delay(250);
  lcd.print(F(" end!"));
  buzz();
  eeAdress = 0;
  delay(1000);
}

void buzz() {
  tone(buzzerPIN, 2500, 100);
  delay(150);
  tone(buzzerPIN, 3000, 100);
  delay(150);
  tone(buzzerPIN, 3500, 100);
}

void sadBuzz() {
  tone(buzzerPIN, 3500, 100);
  delay(150);
  tone(buzzerPIN, 3000, 100);
  delay(150);
  tone(buzzerPIN, 2500, 100);
}

void logoScreen() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("Rakieta HYDRA"));
  delay(1000);
  lcd.clear();
  lcd.print(F("laduje miekko"));
  lcd.setCursor(0, 1);
  delay(500);
  lcd.print(F("bez spadochronu!"));
  delay(1000);
}

void loadingScreen() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("loading..."));
  delay(250);
  for (int i = 0; i < 16; i++) {
    lcd.setCursor(i, 1);
    lcd.print(F("#"));
    delay(50);
  }
  buzz();
}

i do tego schemat całego układu:

ethanak · Kwiecień 21, 2020

Dobra, nie będę analizować całości, zacznijmy od czegoś takiego:

 while (digitalRead(testButton) == LOW) {
      if (digitalRead(testButton) == HIGH) testOpenValve();

W jakiej sytuacji ma zostać wywołana funkcja testOpenValve? Jeśli klawisz jest wciśnięty i puszczony jednocześnie? Czy wtedy kiedy z puszczeniem klawisza trafisz akurat w chwilę po sprawdzeniu warunku w while a przed sprawdzeniem w if? Raczej marne szanse...

Taki kwiatek:

  for (int i = 0; i < historyLength; i++){
    sum += altitudeHistory[i];
  }
  sum += altitude * 100;
  smoothedAltitude = sum; // (historyLength + 1);
  sum = 0;

To nawet ma szanse działać... ale z czytelnością ma niewiele wspólnego. Naturalne jest najpierw zerowanie jakiejś zmiennej, a potem dodawanie do niej poszczególnych wartości. Wtedy taka zmienna może być równie dobrze lokalna.

Co do wielkości tablicy... masz coś takiego:

  for (int i = historyLength; i > 0; i--){
    altitudeHistory[i] = altitudeHistory[i-1];
  }
  if (historyLength < readingsNumber) {
    historyLength ++;
  }

Czyli zwiększasz historyLength aż osiągnie wartość 50. Zastanów się, czy na pewno pętla jest prawidłowa i do którego elementu tablicy odwołasz się przy pierwszym obrocie pętlil, kiedy historyLength będzie wynosić 50?

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay

woytas · Kwiecień 22, 2020

Dnia 21.04.2020 o 11:38, ethanak napisał:
Dobra, nie będę analizować całości, zacznijmy od czegoś takiego:
 while (digitalRead(testButton) == LOW) {
      if (digitalRead(testButton) == HIGH) testOpenValve();
      
W jakiej sytuacji ma zostać wywołana funkcja testOpenValve? Jeśli klawisz jest wciśnięty i puszczony jednocześnie? Czy wtedy kiedy z puszczeniem klawisza trafisz akurat w chwilę po sprawdzeniu warunku w while a przed sprawdzeniem w if? Raczej marne szanse...

Co prawda funkcja się uruchamiała, ale nie za każdym razem gdy wciskałem przycisk.

Dnia 21.04.2020 o 11:38, ethanak napisał:

Czyli zwiększasz historyLength aż osiągnie wartość 50. Zastanów się, czy na pewno pętla jest prawidłowa i do którego elementu tablicy odwołasz się przy pierwszym obrocie pętlil, kiedy historyLength będzie wynosić 50?

Musiałem chwilę pomyśleć zanim doszedłem o co chodzi. Prosty błąd a ja się douszukiwałem jakichś skomplikowanych rozwiązań... wyszedłem poza zakres tablicy. Oczywiście po zmianie wartości readingsNumber na 49 wszystko działa tak jak powinno:) dzięki za pomoc! Poniżej wklejam poprawiony kod:

#include <LiquidCrystal.h>
#include <Servo.h>
#include <MS5611.h>
#include <EEPROM.h>
MS5611 baro;

#define ledGreen 2
#define lcdLed 12
#define knobPIN A0
#define batteryPIN A1
#define powerPIN A2
#define buzzerPIN 13
#define testButton 10
#define lcdLedButton 11
#define servo1 3

int32_t pressure = 1;
int32_t filtered = 0;
int32_t pressureRef = pressure;
float sum = 0;
float altitude = 0.;
byte historyLength = 0;
byte readingsNumber = 49; //for averaging function
int altitudeHistory[50]; //in centimeters! - for saving SRAM. should equal readingsNumber - 1
int smoothedAltitude = 0; //in centimeters!
float maxAltitude = 0.;
float altitudeTreshold = 1.2; //minimal height to turn on flight mode - gather telemetry and get ready to open valve
byte certaintyTreshold = 3; //number of readings exceeding treshold to avoid turning on flight mode by accident
float descentDistanceTreshold = 2.5;
byte certainty = certaintyTreshold; //temporary certainty
bool flightMode = false; //turns on after altitude treshhold is exceeded
bool descentMode = false;
bool landedMode = false;
float temperature = 20.;
float airDensity = 1.2;
float saturationPressure = 0;
int relativeHumidity = 50; //percentage value for calculating air density
int DataArrayLength = 400;
int altitudeData[400];
int eeAdress = 0; //EEPROM memory starting adress
int j = 0; //data order number;

Servo servo;
int servoStartPosition = 0;
int knobPosition;
int powerMeasure;
int batMeasure = 0; //battery voltage measurement
float battery = 0.; //battery voltage
float power = 0.;
int ignitionDelay = 6000; //in milliseconds (insert only multiples of 1000)
int timeToIgnition = ignitionDelay; //in milliseconds
float countdown = ignitionDelay / 1000; //in seconds - is visible on screen
int lcdRefreshDelay = 500; //in milliseconds
int ledRefreshDelay = 2000; //peroid of blinking LED in "ready to start" mode
int lcdLedDelay = 5000; //LCD LED on time
unsigned long lcdTime = millis();
unsigned long currentTime = millis();
unsigned long currentTimeMicros = micros();
unsigned long ledTime = millis();
unsigned long lcdLedTime = millis();
unsigned long loopTime = 0;
int R1 = 1000; //first resistor in voltage divider, for measuring battery voltage
int R2 = 1000; //second resistor in voltage divider, for measuring battery voltage
int frequency = 3000; //for buzzer
byte buzzLength = 25;
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);


void setup()
{
  while (millis() - currentTime < 100); //wait to load libraries
  pinMode(ledGreen, OUTPUT);
  pinMode(lcdLed, OUTPUT);
  Serial.begin(9600);
  servo.attach(servo1);
  pinMode(testButton, INPUT_PULLUP);
  pinMode(lcdLedButton, INPUT_PULLUP);

  //Start barometer
  baro = MS5611();
  baro.begin();
  pressure = baro.getPressure();
  resetTemperature();
  resetAltitude();
  airDensity = (float)pressureRef / (289.6 * ((float)temperature + 273.15)); //289.6 is gas constant for air. Temperature must be converted from Celcius to Kelvin

  lcd.begin(16, 2);
  lcd.display();
  loadingScreen();
}

void loop() {
  loopTime = micros() - currentTimeMicros;
  currentTimeMicros = micros();
  currentTime = millis();
  readParameters();

  if (currentTime - lcdTime > lcdRefreshDelay) {
    printParameters();
    lcdTime = millis();
  }

  //BUTTON - Opening valve test / press loger to print flight data by UART / UP BUTTON
  if (digitalRead(testButton) == LOW) {
    while (digitalRead(testButton) == LOW) {
      if (millis() - currentTime > 2000) printFlightData();
    }
    if (millis() - currentTime < 2000) testOpenValve();
  }

  //BUTTON - LCD LED on with timer / DOWN BUTTON
  if (digitalRead(lcdLedButton) == LOW) {
    digitalWrite(lcdLed, HIGH);
    lcdLedTime = millis();
    while (digitalRead(lcdLedButton) == LOW){
      if(millis() - currentTime > 2000){
        writeDataToEEPROM();
      }
    }
  }
  else if (currentTime > lcdLedTime && currentTime - lcdLedTime > lcdLedDelay) {
    digitalWrite(lcdLed, LOW);
  }

  //BUTTON - show battery voltage and reset altitude / press longer to change number of readings
  //which are averaged and k factor. Also resets pressureRef, altitude and altitude data storage
  if (analogRead(batteryPIN) > 5) {
    currentTime = millis();
    while (analogRead(batteryPIN) > 5) {
      showBatteryVoltage();
      if (millis() - currentTime > 3000) {
        //setReadingsNumber();
        setAltitudeTreshold();
        setDescentDistanceTreshold();
      }
    }
    resetAltitude();
  }

  //Mode detection
  if (!landedMode){
    if (!flightMode) flightModeDetection();
    if (flightMode) descentModeDetection();
  }
  
  //LED1 blinking and buzzer beep - idle mode
  if (flightMode) ledRefreshDelay = 500;
  else ledRefreshDelay = 2000;
  if (landedMode) buzzLength = 200;
  else buzzLength = 25;
  if (currentTime - ledTime < ledRefreshDelay - 20 && servoStartPosition < 175) {
    digitalWrite(ledGreen, LOW);
  }
  else {
    digitalWrite(ledGreen, HIGH);
    if (currentTime - ledTime > ledRefreshDelay) {
      ledTime = millis();
      tone(buzzerPIN, frequency, buzzLength);
    }
  }
}
////////////////////////////////////////////////////////////////////////////////
//functions:

void readParameters() {
  knobPosition = analogRead(knobPIN);
  servoStartPosition = map(knobPosition, 0, 929, 0, 180);

  servo.write(servoStartPosition);
  if (servoStartPosition > 175) {
    digitalWrite(ledGreen, HIGH);
  }

  powerMeasure = analogRead(powerPIN);
  power = map(powerMeasure, 0, 1023, 0, 500);
  power /= 100;

  //pressure measurement
  for (int i = historyLength; i > 0; i--){
    altitudeHistory[i] = altitudeHistory[i-1];
    //Serial.println(i);
  }
  if (historyLength < readingsNumber) {
    historyLength ++;
  }
  altitudeHistory[0] = altitude * 100;
  pressure = baro.getPressure();
  altitude = ((float)pressureRef - (float)pressure) / (airDensity * 9.81);

  sum = 0;
  for (int i = 0; i < historyLength; i++){
    sum += altitudeHistory[i];
    //Serial.println(i);
  }
  sum += altitude * 100;
  smoothedAltitude = sum / (historyLength + 1);
  if ((float)smoothedAltitude / 100 > maxAltitude) maxAltitude = (float)smoothedAltitude / 100;

  //store telemetry
  if (flightMode) {
    if (j < DataArrayLength) {
      altitudeData[j] = smoothedAltitude; // in centimeters (int uses less mamory)
      j++;
    }
    if (j == DataArrayLength) {
      sadBuzz();
      j++;
    }
  }
}

void printParameters() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("H:"));
  lcd.print(maxAltitude);
  lcd.setCursor(7, 0);
  lcd.print(F("a:"));
  lcd.print(smoothedAltitude);
  lcd.setCursor(0, 1);
  lcd.print(F("p:"));
  lcd.print(pressure);
  lcd.print(F(" h:"));
  lcd.print(altitude);
  lcd.print(F("m"));

  Serial.print(F("servo: "));
  Serial.print(servoStartPosition);
  Serial.print(F(", knob: "));
  Serial.print(knobPosition);
  Serial.print(F(", power: "));
  Serial.print(power);
  Serial.print(F("V"));
  Serial.print(F(", A2: "));
  Serial.print(powerMeasure);
  Serial.print(F(", pressure: "));
  Serial.print(pressure);
  Serial.print(F(", Ref: "));
  Serial.print(pressureRef);
  Serial.print(F(", altitude: "));
  Serial.print(altitude);
  Serial.print(F("m"));
  Serial.print(F(" ,smoothed: "));
  Serial.print(smoothedAltitude);
  Serial.print(F(" ,max: "));
  Serial.print(maxAltitude);
  Serial.print(F(", loop time: "));
  Serial.print(loopTime);
  Serial.print(F("us"));
  Serial.print(F(", FM: "));
  Serial.print(flightMode);
  Serial.print(F(", altitude Data length: "));
  Serial.println(j);
}

void showBatteryVoltage() {
  batMeasure = analogRead(batteryPIN);
  battery = map(batMeasure, 0, 916, 0, 4500);
  battery = battery * ((R1 + R2) / R1) / 1000;
  readParameters();

  //temperature measurement
  for (int i = 0; i < readingsNumber; i++) {
    temperature = baro.getTemperature();
    sum += temperature;
  }
  sum /= readingsNumber;
  temperature = sum;
  sum = 0;
  temperature /= 100;

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("P:"));
  lcd.print(power);
  lcd.print(F("V"));
  lcd.print(F(" t:"));
  lcd.print(temperature);
  lcd.setCursor(0, 1);
  lcd.print(F("b:"));
  lcd.print(battery);
  lcd.print(F("V"));
  lcd.print(F(" air:"));
  lcd.print(airDensity);
  //lcd.print(F(" A1:"));
  //lcd.print(batMeasure);

  Serial.print(F("batMeasure: "));
  Serial.print(batMeasure);
  Serial.print(F(", battery: "));
  Serial.print(battery);
  Serial.print(F("V"));
  Serial.print(F(", temperature: "));
  Serial.print(temperature);
  Serial.print(F(", air density: "));
  Serial.print(airDensity);
  Serial.println(F("kg/m^3"));
  delay(lcdRefreshDelay);
}

void setReadingsNumber() {
  currentTime = millis();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("press. readings"));
  lcd.setCursor(0, 1);
  lcd.print(F("per loop: "));

  while (millis() - currentTime < 2000) {
    if (digitalRead(testButton) == LOW) {
      readingsNumber ++;
      delay(250);
      currentTime = millis();
    }
    if (digitalRead(lcdLedButton) == LOW) {
      readingsNumber --;
      delay(250);
      currentTime = millis();
    }
    lcd.setCursor(10, 1);
    lcd.print(readingsNumber);
    lcd.print(F("      "));
  }
  buzz();
}

void setAltitudeTreshold() {
  currentTime = millis();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("set altitude"));
  lcd.setCursor(0, 1);
  lcd.print(F("treshold: "));

  while (millis() - currentTime < 2000) {
    if (digitalRead(testButton) == LOW) {
      altitudeTreshold += 0.2;
      delay(250);
      currentTime = millis();
    }
    if (digitalRead(lcdLedButton) == LOW) {
      altitudeTreshold -= 0.2;
      delay(250);
      currentTime = millis();
    }
    lcd.setCursor(11, 1);
    lcd.print(altitudeTreshold);
  }
  buzz();
}

void setDescentDistanceTreshold() {
  currentTime = millis();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("set descent dist"));
  lcd.setCursor(0, 1);
  lcd.print(F("treshold: "));

  while (millis() - currentTime < 2000) {
    if (digitalRead(testButton) == LOW) {
      descentDistanceTreshold += .2;
      delay(250);
      currentTime = millis();
    }
    if (digitalRead(lcdLedButton) == LOW) {
      descentDistanceTreshold -= .2;
      delay(250);
      currentTime = millis();
    }
    lcd.setCursor(11, 1);
    lcd.print(descentDistanceTreshold);
  }
  buzz();
}

void resetAltitude() {
  sum = 0;
  for (int i = 0; i < 50; i++) {
    sum += baro.getPressure();
  }
  pressureRef = sum / 50 + 6;
  filtered = pressure;
  sum = 0;
  maxAltitude = 0;
  j = 0;
  
  //calculation of humid air density based on:
  //https://en.wikipedia.org/wiki/Density_of_air#Humid_air
  airDensity = (float)temperature * -0.00411 + 1.2867;
  pressure = airDensity * 287.058 * ((float)temperature + 273.15);
  saturationPressure = 6.1078 * pow(10, 7.5 * (float)temperature / ((float)temperature + 237.3));
  airDensity = (pressure * 0.0289654 + (float)relativeHumidity * saturationPressure * 0.018016) / (8.314 * ((float)temperature + 273.15)); //8.314 is universal gas constant
  pressure = pressureRef;
  
  for (int i = 0; i < DataArrayLength; i++) {
    altitudeData[i] = 0;
  }
  for (int i = 0; i < readingsNumber; i++) {
    altitudeHistory[i] = 0;
  }
  historyLength = 0;
  flightMode = false;
  descentMode = false;
  landedMode = false;
}

void resetTemperature() {
  sum = 0;
  for (int i = 0; i < 20; i++) {
    sum += baro.getTemperature();
  }
  sum /= 20;
  temperature = sum;
  temperature /= 100;
  sum = 0;
}

void testOpenValve() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("landing burn"));
  lcd.setCursor(0, 1);
  lcd.print(F("start in: "));
  while (timeToIgnition > 0) {
    currentTime = millis();
    timeToIgnition -= 100;
    countdown -= 0.1;
    lcd.print(countdown);
    lcd.setCursor(13, 1);
    lcd.print(F("sec"));
    lcd.setCursor(10, 1);
    if (timeToIgnition > 1000 && timeToIgnition % 1000 == 0) {
      digitalWrite(ledGreen, HIGH);
      tone(buzzerPIN, frequency, buzzLength);
    }
    if (timeToIgnition > 1000 && timeToIgnition % 1000 == 900) digitalWrite(ledGreen, LOW);
    if (timeToIgnition <= 1000 && timeToIgnition % 200 == 0) {
      digitalWrite(ledGreen, HIGH);
      tone(buzzerPIN, frequency, buzzLength);
    }
    if (timeToIgnition < 1000 && timeToIgnition % 200 == 100) digitalWrite(ledGreen, LOW);
    while (millis() - currentTime < 100); //wait for 100 miliseconds
  }
  servo.write(180);
  timeToIgnition = ignitionDelay;
  countdown = ignitionDelay / 1000;
  delay(500);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("HYDRA has"));
  lcd.setCursor(0, 1);
  lcd.print(F("landed! =D"));
  buzz();
  delay(3000);
}

void openValve() {
  servo.write(180);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("landing burn"));
  lcd.setCursor(0, 1);
  lcd.print(F("at:"));
  lcd.print(altitude);
  lcd.print(F("m"));
  buzz();
  delay(3000);
}

void flightModeDetection() {
  if ((float)smoothedAltitude / 100 > altitudeTreshold) {
    certainty --;
    if (certainty == 0){
      flightMode = true;
      certainty = certaintyTreshold;
    }
  }
  else certainty = certaintyTreshold;
}

void descentModeDetection() {
  if (maxAltitude - (float)smoothedAltitude / 100 > descentDistanceTreshold) {
    certainty --;
    if (certainty == 0){
      certainty = certaintyTreshold;
      flightMode = false;
      descentMode = false;
      landedMode = true;
      openValve();
    }
  }
}

void printFlightData() {
  int eeData;
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("printing from"));
  lcd.setCursor(0, 1);
  lcd.print(F("EEPROM: "));
  
  for (int i = 1; i < DataArrayLength; i++) {
    EEPROM.get(eeAdress, eeData);
    Serial.print(i);
    Serial.print(F(", altitude,"));
    Serial.println(eeData);
    lcd.setCursor(9, 1);
    lcd.print(i);
    eeAdress += 2;
  }
  eeAdress = 0;
  delay(250);
  buzz();
  lcd.print(F(" complete!"));
  delay(1000);
}

void writeDataToEEPROM(){
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("writing data to"));
  lcd.setCursor(0, 1);
  lcd.print(F("EEPROM:"));
  lcd.setCursor(9, 1);

  for (int i = 0; i < DataArrayLength; i++){
    EEPROM.put(eeAdress, altitudeData[i]);
    lcd.setCursor(8, 1);
    lcd.print(i);
    eeAdress += 2;
  }
  delay(250);
  lcd.print(F(" end!"));
  buzz();
  eeAdress = 0;
  delay(1000);
}

void buzz() {
  tone(buzzerPIN, 2500, 100);
  delay(150);
  tone(buzzerPIN, 3000, 100);
  delay(150);
  tone(buzzerPIN, 3500, 100);
}

void sadBuzz() {
  tone(buzzerPIN, 3500, 100);
  delay(150);
  tone(buzzerPIN, 3000, 100);
  delay(150);
  tone(buzzerPIN, 2500, 100);
}

void logoScreen() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("Rakieta HYDRA"));
  delay(1000);
  lcd.clear();
  lcd.print(F("laduje miekko"));
  lcd.setCursor(0, 1);
  delay(500);
  lcd.print(F("bez spadochronu!"));
  delay(1000);
}

void loadingScreen() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(F("loading..."));
  delay(250);
  for (int i = 0; i < 16; i++) {
    lcd.setCursor(i, 1);
    lcd.print(F("#"));
    delay(50);
  }
  buzz();
}

ethanak · Kwiecień 22, 2020

7 minut temu, woytas napisał:

Prosty błąd a ja się douszukiwałem jakichś skomplikowanych rozwiązań... wyszedłem poza zakres tablicy.

To jest akurat tak powszechny błąd że ma nawet swoją nazwę: błąd +/- jeden 🙂 Nie ma chyba kogoś, kto pisuje czasami programy i takiego błędu nie popełnił więc nie ma się czym przejmować tylko następnym razem uważać 🙂

11 minut temu, woytas napisał:

Co prawda funkcja się uruchamiała, ale nie za każdym razem gdy wciskałem przycisk.

No właśnie...

Ja tam jestem zwolennikiem prostszych rozwiązań: na początku loop() sprawdzam sobie wszystko co dotyczy przycisków, w efekcie dostaję np. informację "przycisk numer dwa puszczony po dwóch sekundach", a potem jeśli było coś interesującego to wykonuję. A już nigdy nie wrzucam tam blokujących while... ogólnie można przyjąć że instrukcji "while" w ogóle nie ma, a z pętli to tylko "for" i to tylko do takich zastosowań, jak operacje na tablicach. Takie wtykanie digitalRead() w różne miejsca programu to proszenie się o kłopoty...

Ale to Twój program i być może tak ma działać.

Niestabilny program z tablicą

Pomocna odpowiedź

woytas

Link do komentarza

Share on other sites

ethanak

Link do komentarza

Share on other sites

woytas

Link do komentarza

Share on other sites

ethanak

Link do komentarza

Share on other sites

Polecacz 101

woytas

Link do komentarza

Share on other sites

ethanak

Link do komentarza

Share on other sites

Bądź aktywny - zaloguj się lub utwórz konto!

Utwórz konto w ~20 sekund!

Zaloguj się

Quizy

Najnowsze posty w innych tematach

Kurs Arduino - #9 - Czujnik odległości HC-SR04, funkcje 1 2 3 4 23

Kurs FPGA - #1 - podstawy VHDL w praktyce, spis treści 1 2

Zawodne połączenia przewodów do MAX7219 w Arduino UNO

Błąd odczytu napięcia ORNO OR-WE-504 po Modbus RTU podłączonego do ARDUINO Mega

Inteligentna skrzynka na listy – wygoda i nowoczesność w jednym

Inne