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