The latitude and longitude cant be called in serial.monitor

i want to call pvt.lat and pvt.lon using float latitude_ugv and longitude_ugv.

but it result in zero

#include <Arduino.h>
#include <QMC5883LCompass.h>
#include <SoftwareSerial.h>
#include <Servo.h>
#include <SPI.h>      //the communication interface with the modem
#include <RF24.h>     //the library which helps us to control the radio modem (nRF24L)
#include <nRF24L01.h>

Servo servo;

/* Motor DC Declaration */
const int motor1EN = 10;  // Pin EN untuk motor DC 1
const int motor1PWM1 = 7;  // Pin PWM1 untuk motor DC 1
const int motor1PWM2 = 6;  // Pin PWM2 untuk motor DC 1

/* GPS Declaration */
const unsigned char UBX_HEADER[] = { 0xB5, 0x62 };


//type data yang menentukan panjang data
struct NAV_PVT {
  unsigned char cls;
  unsigned char id;
  unsigned short len;
  unsigned long iTOW;          // GPS time of week of the navigation epoch (ms)

  unsigned short year;         // Year (UTC)
  unsigned char month;         // Month, range 1..12 (UTC)
  unsigned char day;           // Day of month, range 1..31 (UTC)
  unsigned char hour;          // Hour of day, range 0..23 (UTC)
  unsigned char minute;        // Minute of hour, range 0..59 (UTC)
  unsigned char second;        // Seconds of minute, range 0..60 (UTC)
  char valid;                  // Validity Flags (see graphic below)
  unsigned long tAcc;          // Time accuracy estimate (UTC) (ns)
  long nano;                   // Fraction of second, range -1e9 .. 1e9 (UTC) (ns)
  unsigned char fixType;       // GNSSfix Type, range 0..5
  char flags;                  // Fix Status Flags
  unsigned char reserved1;     // reserved
  unsigned char numSV;         // Number of satellites used in Nav Solution

  long lon;                    // Longitude (deg)
  long lat;                    // Latitude (deg)
  long height;                 // Height above Ellipsoid (mm)
  long hMSL;                   // Height above mean sea level (mm)
  unsigned long hAcc;          // Horizontal Accuracy Estimate (mm)
  unsigned long vAcc;          // Vertical Accuracy Estimate (mm)

  long velN;                   // NED north velocity (mm/s)
  long velE;                   // NED east velocity (mm/s)
  long velD;                   // NED down velocity (mm/s)
  long gSpeed;                 // Ground Speed (2-D) (mm/s)
  long heading;                // Heading of motion 2-D (deg)
  unsigned long sAcc;          // Speed Accuracy Estimate
  unsigned long headingAcc;    // Heading Accuracy Estimate
  unsigned short pDOP;         // Position dilution of precision
  short reserved2;             // Reserved
  unsigned long reserved3;     // Reserved
  unsigned char dummy[8];
};

NAV_PVT pvt;


void calcChecksum(unsigned char* CK) {
  memset(CK, 0, 2);
  for (int i = 0; i < (int)sizeof(NAV_PVT); i++) {
    CK[0] += ((unsigned char*)(&pvt))[i];
    CK[1] += CK[0];
  }
}

bool processGPS() {
  static int fpos = 0;
  static unsigned char checksum[2];
  const int payloadSize = sizeof(NAV_PVT); //32 byte

  while ( Serial3.available() )
  {
    byte c = Serial3.read();
    if ( fpos < 2 )
    {
      if ( c == UBX_HEADER[fpos] ) // Check Header apakah sesuai
        fpos++;
      else
        fpos = 0;
    }
    else
    {
      if ( (fpos - 2) < payloadSize )
        ((unsigned char*)(&pvt))[fpos - 2] = c;

      fpos++;

      if ( fpos == (payloadSize + 2) ) //CheckSum check
      {
        calcChecksum(checksum);
      }
      else if ( fpos == (payloadSize + 3) )
      {
        if ( c != checksum[0] )
          fpos = 0;
      }
      else if ( fpos == (payloadSize + 4) ) {
        fpos = 0;
        if ( c == checksum[1] ) {
          return true;
        }
      }
      else if ( fpos > (payloadSize + 4) )
      {
        fpos = 0;
      }
    }
  }
  return false;
}

float latitude_UGV = pvt.lat/10000000.0;
float longitude_UGV = pvt.lon/10000000.0;
// float latitude_UGV;
// float longitude_UGV;
float acc;
float radlat_UGV, radlon_UGV, radlat_wp, radlon_wp;
float a, c, distance, jarak;
float o, t, bearing, direction, theta;
const float toRad = 0.0174533;
const float toDeg = 57.2957795;
float lat_wp[] = {-6.975818}; // Change Here
float lon_wp[] = {107.630307}; // Change Here
int wp;

/* Compass Declaration */
QMC5883LCompass compass;
int heading;

/* Autonomous Fuzzy Logic Declaration */
#define FIS_TYPE float
#define FIS_RESOLUSION 101
#define FIS_MIN -3.4028235E+38
#define FIS_MAX 3.4028235E+38
typedef FIS_TYPE(*_FIS_MF)(FIS_TYPE, FIS_TYPE*);
typedef FIS_TYPE(*_FIS_ARR_OP)(FIS_TYPE, FIS_TYPE);
typedef FIS_TYPE(*_FIS_ARR)(FIS_TYPE*, int, _FIS_ARR_OP);

// Number of inputs to the fuzzy inference system
const int fis_gcI = 2;
// Number of outputs to the fuzzy inference system
const int fis_gcO = 2;
// Number of rules to the fuzzy inference system
const int fis_gcR = 25;

FIS_TYPE g_fisInput[fis_gcI];
FIS_TYPE g_fisOutput[fis_gcO];


// Motor Speed Values - Start at zero
int UGVSpeed = 0;

// UGV Direction Values - Start at ninety
int UGVDirection = 90;

void UGVControl(int speed, int degree) {
  digitalWrite(motor1PWM1, LOW);
  digitalWrite(motor1PWM2, HIGH);
  analogWrite(motor1EN, speed);

  servo.write(degree);
}

//***********************************************************************
// Support functions for Fuzzy Inference System                          
//***********************************************************************
// Triangular Member Function
FIS_TYPE fis_trimf(FIS_TYPE x, FIS_TYPE* p)
{
    FIS_TYPE a = p[0], b = p[1], c = p[2];
    FIS_TYPE t1 = (x - a) / (b - a);
    FIS_TYPE t2 = (c - x) / (c - b);
    if ((a == b) && (b == c)) return (FIS_TYPE) (x == a);
    if (a == b) return (FIS_TYPE) (t2*(b <= x)*(x <= c));
    if (b == c) return (FIS_TYPE) (t1*(a <= x)*(x <= b));
    t1 = min(t1, t2);
    return (FIS_TYPE) max(t1, 0);
}

FIS_TYPE fis_min(FIS_TYPE a, FIS_TYPE b)
{
    return min(a, b);
}

FIS_TYPE fis_probor(FIS_TYPE a, FIS_TYPE b)
{
    return (a + b - (a * b));
}

FIS_TYPE fis_prod(FIS_TYPE a, FIS_TYPE b)
{
    return (a * b);
}

FIS_TYPE fis_sum(FIS_TYPE a, FIS_TYPE b)
{
    return (a + b);
}

FIS_TYPE fis_array_operation(FIS_TYPE *array, int size, _FIS_ARR_OP pfnOp)
{
    int i;
    FIS_TYPE ret = 0;

    if (size == 0) return ret;
    if (size == 1) return array[0];

    ret = array[0];
    for (i = 1; i < size; i++)
    {
        ret = (*pfnOp)(ret, array[i]);
    }

    return ret;
}


//***********************************************************************
// Data for Fuzzy Inference System                                       
//***********************************************************************
// Pointers to the implementations of member functions
_FIS_MF fis_gMF[] =
{
    fis_trimf
};

// Count of member function for each Input
int fis_gIMFCount[] = { 5, 5 };

// Count of member function for each Output 
int fis_gOMFCount[] = { 5, 2 };

// Coefficients for the Input Member Functions
FIS_TYPE fis_gMFI0Coeff1[] = { 0, 0, 10 };
FIS_TYPE fis_gMFI0Coeff2[] = { 0, 10, 20 };
FIS_TYPE fis_gMFI0Coeff3[] = { 10, 20, 30 };
FIS_TYPE fis_gMFI0Coeff4[] = { 30, 40, 10000 };
FIS_TYPE fis_gMFI0Coeff5[] = { 20, 30, 40 };
FIS_TYPE* fis_gMFI0Coeff[] = { fis_gMFI0Coeff1, fis_gMFI0Coeff2, fis_gMFI0Coeff3, fis_gMFI0Coeff4, fis_gMFI0Coeff5 };
FIS_TYPE fis_gMFI1Coeff1[] = { 0, 5, 15 };
FIS_TYPE fis_gMFI1Coeff2[] = { -1000, -15, -5 };
FIS_TYPE fis_gMFI1Coeff3[] = { 5, 15, 1000 };
FIS_TYPE fis_gMFI1Coeff4[] = { -15, -5, 0 };
FIS_TYPE fis_gMFI1Coeff5[] = { -5, 0, 5 };
FIS_TYPE* fis_gMFI1Coeff[] = { fis_gMFI1Coeff1, fis_gMFI1Coeff2, fis_gMFI1Coeff3, fis_gMFI1Coeff4, fis_gMFI1Coeff5 };
FIS_TYPE** fis_gMFICoeff[] = { fis_gMFI0Coeff, fis_gMFI1Coeff };

// Coefficients for the Output Member Functions
FIS_TYPE fis_gMFO0Coeff1[] = { 0, 0, 0 };
FIS_TYPE fis_gMFO0Coeff2[] = { 0, 0, 45 };
FIS_TYPE fis_gMFO0Coeff3[] = { 0, 0, 90 };
FIS_TYPE fis_gMFO0Coeff4[] = { 0, 0, 135 };
FIS_TYPE fis_gMFO0Coeff5[] = { 0, 0, 160 };
FIS_TYPE* fis_gMFO0Coeff[] = { fis_gMFO0Coeff1, fis_gMFO0Coeff2, fis_gMFO0Coeff3, fis_gMFO0Coeff4, fis_gMFO0Coeff5 };
FIS_TYPE fis_gMFO1Coeff1[] = { 0, 0, 80 };
FIS_TYPE fis_gMFO1Coeff2[] = { 0, 0, 100 };
FIS_TYPE* fis_gMFO1Coeff[] = { fis_gMFO1Coeff1, fis_gMFO1Coeff2 };
FIS_TYPE** fis_gMFOCoeff[] = { fis_gMFO0Coeff, fis_gMFO1Coeff };

// Input membership function set
int fis_gMFI0[] = { 0, 0, 0, 0, 0 };
int fis_gMFI1[] = { 0, 0, 0, 0, 0 };
int* fis_gMFI[] = { fis_gMFI0, fis_gMFI1};

// Output membership function set

int* fis_gMFO[] = {};

// Rule Weights
FIS_TYPE fis_gRWeight[] = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 };

// Rule Type
int fis_gRType[] = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 };

// Rule Inputs
int fis_gRI0[] = { 1, 1 };
int fis_gRI1[] = { 1, 2 };
int fis_gRI2[] = { 1, 3 };
int fis_gRI3[] = { 1, 4 };
int fis_gRI4[] = { 1, 5 };
int fis_gRI5[] = { 2, 1 };
int fis_gRI6[] = { 2, 2 };
int fis_gRI7[] = { 2, 3 };
int fis_gRI8[] = { 2, 4 };
int fis_gRI9[] = { 2, 5 };
int fis_gRI10[] = { 3, 1 };
int fis_gRI11[] = { 3, 2 };
int fis_gRI12[] = { 3, 3 };
int fis_gRI13[] = { 3, 4 };
int fis_gRI14[] = { 3, 5 };
int fis_gRI15[] = { 4, 1 };
int fis_gRI16[] = { 4, 2 };
int fis_gRI17[] = { 4, 3 };
int fis_gRI18[] = { 4, 4 };
int fis_gRI19[] = { 4, 5 };
int fis_gRI20[] = { 5, 1 };
int fis_gRI21[] = { 5, 2 };
int fis_gRI22[] = { 5, 3 };
int fis_gRI23[] = { 5, 4 };
int fis_gRI24[] = { 5, 5 };
int* fis_gRI[] = { fis_gRI0, fis_gRI1, fis_gRI2, fis_gRI3, fis_gRI4, fis_gRI5, fis_gRI6, fis_gRI7, fis_gRI8, fis_gRI9, fis_gRI10, fis_gRI11, fis_gRI12, fis_gRI13, fis_gRI14, fis_gRI15, fis_gRI16, fis_gRI17, fis_gRI18, fis_gRI19, fis_gRI20, fis_gRI21, fis_gRI22, fis_gRI23, fis_gRI24 };

// Rule Outputs
int fis_gRO0[] = { 2, 1 };
int fis_gRO1[] = { 5, 1 };
int fis_gRO2[] = { 1, 1 };
int fis_gRO3[] = { 4, 1 };
int fis_gRO4[] = { 3, 1 };
int fis_gRO5[] = { 2, 1 };
int fis_gRO6[] = { 5, 1 };
int fis_gRO7[] = { 1, 1 };
int fis_gRO8[] = { 4, 1 };
int fis_gRO9[] = { 3, 1 };
int fis_gRO10[] = { 2, 1 };
int fis_gRO11[] = { 5, 1 };
int fis_gRO12[] = { 1, 1 };
int fis_gRO13[] = { 4, 1 };
int fis_gRO14[] = { 3, 1 };
int fis_gRO15[] = { 2, 2 };
int fis_gRO16[] = { 5, 2 };
int fis_gRO17[] = { 1, 2 };
int fis_gRO18[] = { 4, 2 };
int fis_gRO19[] = { 3, 2 };
int fis_gRO20[] = { 2, 2 };
int fis_gRO21[] = { 5, 2 };
int fis_gRO22[] = { 1, 2 };
int fis_gRO23[] = { 4, 2 };
int fis_gRO24[] = { 3, 2 };
int* fis_gRO[] = { fis_gRO0, fis_gRO1, fis_gRO2, fis_gRO3, fis_gRO4, fis_gRO5, fis_gRO6, fis_gRO7, fis_gRO8, fis_gRO9, fis_gRO10, fis_gRO11, fis_gRO12, fis_gRO13, fis_gRO14, fis_gRO15, fis_gRO16, fis_gRO17, fis_gRO18, fis_gRO19, fis_gRO20, fis_gRO21, fis_gRO22, fis_gRO23, fis_gRO24 };

// Input range Min
FIS_TYPE fis_gIMin[] = { 0, -20 };

// Input range Max
FIS_TYPE fis_gIMax[] = { 100, 20 };

// Output range Min
FIS_TYPE fis_gOMin[] = { 0, 0 };

// Output range Max
FIS_TYPE fis_gOMax[] = { 1, 1 };

//***********************************************************************
// Fuzzy Inference System                                                
//***********************************************************************
void fis_evaluate()
{
    FIS_TYPE fuzzyInput0[] = { 0, 0, 0, 0, 0 };
    FIS_TYPE fuzzyInput1[] = { 0, 0, 0, 0, 0 };
    FIS_TYPE* fuzzyInput[fis_gcI] = { fuzzyInput0, fuzzyInput1, };
    FIS_TYPE fuzzyOutput0[] = { 0, 0, 0, 0, 0 };
    FIS_TYPE fuzzyOutput1[] = { 0, 0 };
    FIS_TYPE* fuzzyOutput[fis_gcO] = { fuzzyOutput0, fuzzyOutput1, };
    FIS_TYPE fuzzyRules[fis_gcR] = { 0 };
    FIS_TYPE fuzzyFires[fis_gcR] = { 0 };
    FIS_TYPE* fuzzyRuleSet[] = { fuzzyRules, fuzzyFires };
    FIS_TYPE sW = 0;

    // Transforming input to fuzzy Input
    int i, j, r, o;
    for (i = 0; i < fis_gcI; ++i)
    {
        for (j = 0; j < fis_gIMFCount[i]; ++j)
        {
            fuzzyInput[i][j] =
                (fis_gMF[fis_gMFI[i][j]])(g_fisInput[i], fis_gMFICoeff[i][j]);
        }
    }

    int index = 0;
    for (r = 0; r < fis_gcR; ++r)
    {
        if (fis_gRType[r] == 1)
        {
            fuzzyFires[r] = FIS_MAX;
            for (i = 0; i < fis_gcI; ++i)
            {
                index = fis_gRI[r][i];
                if (index > 0)
                    fuzzyFires[r] = fis_min(fuzzyFires[r], fuzzyInput[i][index - 1]);
                else if (index < 0)
                    fuzzyFires[r] = fis_min(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
                else
                    fuzzyFires[r] = fis_min(fuzzyFires[r], 1);
            }
        }
        else
        {
            fuzzyFires[r] = 0;
            for (i = 0; i < fis_gcI; ++i)
            {
                index = fis_gRI[r][i];
                if (index > 0)
                    fuzzyFires[r] = fis_probor(fuzzyFires[r], fuzzyInput[i][index - 1]);
                else if (index < 0)
                    fuzzyFires[r] = fis_probor(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
                else
                    fuzzyFires[r] = fis_probor(fuzzyFires[r], 0);
            }
        }

        fuzzyFires[r] = fis_gRWeight[r] * fuzzyFires[r];
        sW += fuzzyFires[r];
    }

    if (sW == 0)
    {
        for (o = 0; o < fis_gcO; ++o)
        {
            g_fisOutput[o] = ((fis_gOMax[o] + fis_gOMin[o]) / 2);
        }
    }
    else
    {
        for (o = 0; o < fis_gcO; ++o)
        {
            FIS_TYPE sWI = 0.0;
            for (j = 0; j < fis_gOMFCount[o]; ++j)
            {
                fuzzyOutput[o][j] = fis_gMFOCoeff[o][j][fis_gcI];
                for (i = 0; i < fis_gcI; ++i)
                {
                    fuzzyOutput[o][j] += g_fisInput[i] * fis_gMFOCoeff[o][j][i];
                }
            }

            for (r = 0; r < fis_gcR; ++r)
            {
                index = fis_gRO[r][o] - 1;
                sWI += fuzzyFires[r] * fuzzyOutput[o][index];
            }

            g_fisOutput[o] = sWI / sW;
        }
    }
}

void distance_read() {
  // if (pvt.fixType == 3) {
    latitude_UGV = pvt.lat/10000000.0;
    longitude_UGV = pvt.lon/10000000.0;
    // latitude_UGV = -6.9758071;
    // longitude_UGV = 107.6302517;
    radlat_UGV = latitude_UGV*toRad;
    radlon_UGV = longitude_UGV*toRad;
    radlat_wp = lat_wp[wp]*toRad;
    radlon_wp = lon_wp[wp]*toRad;
    // haversine
    a = (sq(sin(radlat_wp - radlat_UGV)/2)+cos(radlat_UGV)*cos(radlat_wp)*(sq(sin(radlon_wp-radlon_UGV)/2))); 
    c = 2*atan2(sqrt(a), sqrt(1-a));
    // kali ke radius bumi dalam meter
    distance = c*6371000;

    // DEBUG:
    Serial.print("LAT: ");
    Serial.print(latitude_UGV,7);
    Serial.print(" | ");
    Serial.print("LON: ");
    Serial.print(longitude_UGV,7);
    Serial.print(" | ");
    Serial.print("DIST: ");
    Serial.print(distance);
    Serial.print(" | ");
  // } else {
  //   Serial.print(F("INVALID DIST"));
  //   Serial.print(" | "); 
  // }
}


void direction_read() {
  // if (pvt.fixType == 3) {
    compass.read();
    heading = compass.getAzimuth();
    latitude_UGV = pvt.lat/10000000.0;
    longitude_UGV = pvt.lon/10000000.0;
    // latitude_UGV = -6.9758071;
    // longitude_UGV = 107.6302517;
  
    radlat_UGV = latitude_UGV*toRad;
    radlon_UGV = longitude_UGV*toRad;
    radlat_wp = lat_wp[wp]*toRad;
    radlon_wp = lon_wp[wp]*toRad;
    o = sin(radlon_wp - radlon_UGV)*cos(radlon_wp);
    t = (cos(radlat_UGV)*sin(radlat_wp))-(sin(radlat_UGV)*cos(radlat_wp)*cos(radlon_wp - radlon_UGV));              
    theta = atan2(o,t);
    //ubah ke derajat dari radian
    bearing = theta*toDeg;
    
    //kalo nilai derajat minus, ditambah 360 supaya bisa jadi positif
    if (bearing < 0){
      bearing = bearing + 360;
    }

    direction = heading - bearing;

    // DEBUG:
    Serial.print("HEAD: ");
    Serial.print(heading);
    Serial.print(" | ");
    Serial.print("BEAR: ");
    Serial.print(bearing);
    Serial.print(" | ");
    Serial.print("DIR: ");
    Serial.print(direction);
    Serial.print(" | ");
  // } else {
  //   Serial.print(F("INVALID DIR"));
  //   Serial.print(" | "); Serial.print("ft = "); Serial.print(pvt.fixType);
  // }
}


void otomasi() {
  /* Autonomous Switching */
  while (Serial3.available() > 0) {
    // if (processGPS()) {
      latitude_UGV = pvt.lat/10000000.0;
      longitude_UGV = pvt.lon/10000000.0;
      // latitude_UGV = -6.9758071;
      // longitude_UGV = 107.6302517;
      direction_read();
      distance_read();

      if (latitude_UGV != 0 && longitude_UGV != 0) {
        // Read Input: Jarak
        g_fisInput[0] = distance;
        // Read Input: Sudut
        g_fisInput[1] = direction;

        g_fisOutput[0] = 0;
        g_fisOutput[1] = 0;

        if (distance < 2) {
          UGVControl(0, 165);
          delay(500);
          UGVControl(0, 35);
        } else {
          fis_evaluate();

          UGVSpeed = g_fisOutput[1];
          UGVDirection = g_fisOutput[0];
          int ReverseDirection = map(UGVDirection, 0, 180, 180, 0);

          UGVControl(UGVSpeed, ReverseDirection);

          // DEBUG:
          Serial.print("Speed: ");   
          Serial.print(UGVSpeed);   
          Serial.print(" | "); 
          Serial.print("Direction: ");   
          Serial.println(UGVDirection);
        }

        // DEBUG:
        Serial.print("Speed: ");   
        Serial.print(UGVSpeed);   
        Serial.print(" | "); 
        Serial.print("Direction: ");   
        Serial.println(UGVDirection);
      } else {
        UGVSpeed = 0;
        UGVDirection = 90;

        UGVControl(UGVSpeed, UGVDirection);

        // DEBUG:
        Serial.print("Speed: ");   
        Serial.print(UGVSpeed);   
        Serial.print(" | "); 
        Serial.print("Direction: ");   
        Serial.println(UGVDirection);
      }
    // }
  }
}

void setup()
{

  Serial.begin(9600);
  // Setup Compass
  compass.init();
  compass.setCalibration(-1345, 1112, -1855, 691, -62, 353);
  compass.setSmoothing(10,true);  

  // Setup GPS
  Serial3.begin(115200);

  //Setup Motor DC
  pinMode(motor1EN, OUTPUT);
  pinMode(motor1PWM1, OUTPUT);
  pinMode(motor1PWM2, OUTPUT);

  // Setup Motor Servo
  servo.attach(3);
 
}

void loop() {
  latitude_UGV = pvt.lat/10000000.0;
  longitude_UGV = pvt.lon/10000000.0;
  otomasi();
  latitude_UGV = pvt.lat/10000000.0;
  longitude_UGV = pvt.lon/10000000.0;
}


pls help guys

im using paltformio BTW

it result the same, but I have finish it. thankyou for the answer :smiley:

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.