ESP32 Forum

Code: Select all

#include <DFRobot_BME680_I2C.h>
#include <Wire.h>
#include <TinyGPS++.h>
#include <RTClib.h>
#include <math.h>
#include <SimpleKalmanFilter.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <uEEPROMLib.h>
#include <Adafruit_VL53L0X.h>

#define S1_RXD 12
#define S1_TXD 14
#define S2_RXD 27
#define S2_TXD 26

const byte M2 = 18, M1 = 19, MQ1 = 2, MQ2 = 4, VS = 25; //, servo1Pin = 5, servo2Pin = 17;
//Servo servo1,servo2;

byte a, sensor_status[4] = { 1, 1, 1, 1 }, leg_status[6] = { 0, 0, 0, 0, 0, 0 };
bool count = 0, motor = 0, arm_up = 0, arm_down = 0;
long unsigned int time1000 = 0, packetcount = 0;
unsigned int hour, minute, tof;
float voltage_cs, ReferencePressure, b, altitude, volt = 0.0, pressure, kalman_pres, percent;

typedef struct struct_message {
  int a;
  float roll;
  float pitch;
  float yaw;
} struct_message;

struct_message myData;
//esp_now_peer_info_t peerInfo;
sensors_event_t orientationData;
SimpleKalmanFilter pressureKalmanFilter(1, 1, 1);

DFRobot_BME680_I2C bme(0x77);  //0x77 I2C address
Adafruit_VL53L0X lox = Adafruit_VL53L0X();
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
RTC_DS3231 rtc;
TinyGPSPlus gps;
uEEPROMLib eeprom(0x50);

void setup() {
  
  Serial.begin(115200);
  Serial1.begin(230400);
  Serial2.begin(9600);
  delay(100);
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);

  digitalWrite(M1, LOW);
  digitalWrite(M2, LOW);

 /* Wire.begin();
   WiFi.mode(WIFI_STA);*/

  if (!rtc.begin())
    sensor_status[0] = 0;
  //Serial.println("RTC Error");

  if (!bno.begin())  //Initialize IMU sensor
    sensor_status[1] = 0;
  //Serial.println("IMU Error");

  if (bme.begin())  //Initialize Pressure sensor
    sensor_status[2] = 0;
  //Serial.println("Pressure Sensor Error");

  if (!lox.begin())
    sensor_status[3] = 0;
  //Serial.println("Distance Sensor Error");

  lox.startRangeContinuous();

  if (rtc.lostPower())
    rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));

  for (int i = 0; i < 50; i++) {
    bme.startConvert();
    bme.update();
    b += bme.readPressure();
    Serial.println(b);
    delay(100);
  }
  ReferencePressure = b / 50;
  b = 0;
}

void loop() {

  bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);
  myData.roll = orientationData.orientation.y;
  myData.pitch = orientationData.orientation.z;
  myData.yaw = orientationData.orientation.x;

  a = Serial1.read();  // Serial.read();  Send 1-12 nos.

  if (a > 0) {
    myData.a = a;
    motor = 0;
    arm_up = 0;
    arm_down = 0;
  }

   motion();

    if (lox.isRangeComplete())
      tof = lox.readRange() - 8;

    smartDelay(0);
    bme.startConvert();
    bme.update();
    DateTime now = rtc.now();
    kalman_pres = pressureKalmanFilter.updateEstimate(bme.readPressure());

    altitude = 44330 * (1 - pow(kalman_pres / ReferencePressure, 0.1903));
    volt = (analogRead(VS) * 3.30) / (0.2 * 4095.0) + 0.66;
    percent = (voltage_cs/12.6)*100;
    hour = gps.time.hour() + 5;
    minute = gps.time.minute() + 30;

    if (minute > 59) {
      minute = minute - 60;
      hour++;
    }
    if (hour > 23)
      hour = hour - 24;

    String tele= String("Team_Nirma") + ',' + String(now.hour()) + ':' + String(now.minute()) + ':' + String(now.second())  + ',' + 
                 String(packetcount) + ',' + String(altitude,2) + ',' + String(kalman_pres) + ',' + String(bme.readTemperature()/100, 2)
                  + ',' + String(bme.readHumidity()/1000, 2) + ',' + String(bme.readGasResistance())+ ',' + String(analogRead(MQ1)) + ',' 
                  + String(analogRead(MQ2)) + ',' + String(volt,2) + ',' + String("0") +',' + String(tof) + ',' + String(myData.roll) + ','
                  + String(myData.pitch) + ',' + String(myData.yaw) + ',' + String(hour) + ':' + String(minute) + ':' + String(gps.time.second()) + ',' + 
                  String(gps.location.lat(), 4) + ',' + String(gps.location.lng(), 4) + ',' + String(gps.altitude.meters(), 1) + ',' 
                  + String(gps.satellites.value()) + ',' + String(percent,0);

    Serial2.println(tele);
    Serial1.println(tele);
    Serial.println(tele);
    Serial.println(myData.a);
    packetcount++;
  
}

void motion() {
  switch (myData.a) {

    case 57:  //Wheel_down
      if (motor == 0) {
        delay(2000);
        digitalWrite(M1, HIGH);
        digitalWrite(M2, HIGH);
        motor = 1;
      }
      break;

    case 58:  //Wheel_up
      digitalWrite(M1, LOW);
      digitalWrite(M2, LOW);
      break;

    case 59:
      if (arm_down == 0) {
        ARM_DOWN();
        arm_down = 1;
      }
      break;

    case 60:
      if (arm_up == 0) {
        ARM_UP();
        arm_up = 1;
      }
      break;

    default:

      break;
  }
}

void ARM_UP() {                                                                                                                                   
}

void ARM_DOWN() {
}

static void smartDelay(unsigned long ms) {
  unsigned long start = millis();
  do {
    while (Serial2.available())
      gps.encode(Serial2.read());
  } while (millis() - start < ms);
}

Code: Select all

smartDelay(0);
...
static void smartDelay(unsigned long ms) {
  unsigned long start = millis();
  do {
    while (Serial2.available())
      gps.encode(Serial2.read());
  } while (millis() - start < ms); <--- 
}