Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
974 lines (825 sloc) 30.8 KB
#include <Common.h>
#include <IO.h>
#include <SPI.h>
#include <SD.h>
#include <ElcanoSerial.h>
using namespace elcano;
SerialData data;
ParseState ps;
#define PI ((float) 3.1415925)
#ifndef NULL
#define NULL 0
#endif
#define currentlocation -1 //currentLocation
#define EMPTY -2
//void DataReady();
extern bool DataAvailable;
/*---------------------------------------------------------------------------------------*/
// EDIT for route
// CONES includes start and stop
#define CONES 1
long goal_lat[CONES] = {47760934};
long goal_lon[CONES] = {-122189963};
//long goal_lat[CONES] = { 47621881, 47621825, 47623144, 47620616, 47621881};
//long goal_lon[CONES] = {-122349894, -122352120, -122351987, -122351087, -122349894};
/* mph mm/s
3 1341
5 2235
8 3576
10 4470
15 6706
20 8941
25 11176
30 13411
45 20117
60 26822
*/
#define DESIRED_SPEED_mmPs 2235
/*---------------------------------------------------------------------------------------*/
#define MAX_MAPS 10 // The maximum number of map files stored to SD card.
#define MAX_WAYPOINTS 40 // The maximum number of waypoints in each map file.
extern int map_points = 5;//16;
struct junction Nodes[MAX_WAYPOINTS];
struct AStar
{
int ParentID;
long CostFromStart;
long CostToGoal;
long TotalCost;
}Open[MAX_WAYPOINTS];
class waypoint Origin, Start;
waypoint Path[MAX_WAYPOINTS]; // course route to goal
waypoint mission[CONES]; // aka MDF //The target Nodes to hit
int waypoints = CONES;
/*---------------------------------------------------------------------------------------*/
// Fill the distances of the junctions in the MAP
void ConstructNetwork(junction *Map, int MapPoints)
{
double deltaX, deltaY;
int destination;
for (int i = 0; i < MapPoints; i++)
{
if( Map[i].east_mm == INVALID) continue;
for (int j = 0; j< 4; j++)
{
destination = Map[i].destination[j];
if (destination == END) continue;
deltaX = Map[i].east_mm;
deltaX -= Map[destination].east_mm;
deltaY = Map[i].north_mm;
deltaY -= Map[destination].north_mm;
Map[i].Distance[j] *= sqrt(deltaX * deltaX + deltaY * deltaY); //in rough scale
}
}
}
/*---------------------------------------------------------------------------------------*/
// Set up mission structure from cone latitude and longitude list.
void GetGoals(junction *nodes , int Goals)
{
double deltaX, deltaY, Distance;
for (int i = 0; i < CONES; i++)
{
// mission[i].latitude = Nodes[i].east_mm;//goal_lat[i];
// mission[i].longitude = Nodes[i].north_mm;//goal_lon[i];
mission[i].east_mm = goal_lat[i];
mission[i].north_mm = goal_lon[i];
// mission[i].Compute_mm();
mission[i].speed_mmPs = DESIRED_SPEED_mmPs;
mission[i].index = 1 | GOAL;
mission[i].sigma_mm = 1000;
mission[i].time_ms = 0;
if (i == 0) //If CONE == 1
{
mission[i].Evector_x1000 = 1000;
mission[i].Nvector_x1000 = 0;
}
else
{
deltaX = mission[i].east_mm - mission[i-1].east_mm;
deltaY = mission[i].north_mm - mission[i-1].north_mm;
Distance = sqrt(deltaX*deltaX + deltaY*deltaY);
mission[i-1].Evector_x1000 = (deltaX * 1000.) / Distance;
mission[i-1].Nvector_x1000 = (deltaY * 1000.) / Distance;
}
// if (i == CONES - 1)
// {
// mission[i].Evector_x1000 = mission[i-1].Evector_x1000;
// mission[i].Nvector_x1000 = mission[i-1].Nvector_x1000;
// mission[i].index |= END;
// }
}
}
/*---------------------------------------------------------------------------------------*/
// Find the distance from (east_mm, north_mm) to a road segment Nodes[i].distance[j]
// return distance in mm, and per cent of completion from i to j.
// distance is negative if (east_mm, north_mm) lies to the left of the road
// when road direction is from i to j
// Compare this routine to distance() in C3 Pilot
//k = index into Nodes[]
//east_mm : current
long distance(int cur_node, int *k, long cur_east_mm, long cur_north_mm, int* perCent)
{
float deltaX, deltaY, dist_mm;
int cur , destination;
long Eunit_x1000, Nunit_x1000;
long closest_mm = MAX_DISTANCE;
long Road_distance, RoadDX_mm, RoadDY_mm;
long pc; //per cent of completion from i to j.
*perCent = 0;
*k = 0;
closest_mm = MAX_DISTANCE;
for (cur = 0; cur < 4; cur++) // Don't make computations twice.
{
destination = Nodes[cur_node].destination[cur];
if (destination == 0 || destination < cur_node) continue; //replace Destination with END
// compute road unit vectors from i to cur
RoadDX_mm = Nodes[destination].east_mm - Nodes[cur_node].east_mm;
// Serial.println("RoadX_mm " + String(RoadDX_mm));
// Serial.println("Destination " + String(destination));
// Serial.println("Nodes[destination].east_mm " + String(Nodes[destination].east_mm));
// Serial.println("-Nodes[cur_loc].east_mm " + String(-Nodes[cur_node].east_mm));
//
int Eunit_x1000 = RoadDX_mm * 1000 / Nodes[cur_node].Distance[cur];
RoadDY_mm = Nodes[destination].north_mm - Nodes[cur_node].north_mm;
//
// Serial.println("RoadY_mm " + String(RoadDY_mm));
// Serial.println("Nodes[destination].north_mm " + String(Nodes[destination].north_mm));
// Serial.println("-Nodes[cur_loc].north_mm " + String(-Nodes[cur_node].north_mm));
//
int Nunit_x1000 = RoadDY_mm * 1000 / Nodes[cur_node].Distance[cur];
//
// // normal vector is (Nunit, -Eunit)
// //Answers: What would be the change in X/Y from my current Node.
// deltaX = cur_east_mm - Nodes[cur_node].east_mm;
// deltaY = cur_north_mm - Nodes[cur_node].north_mm;
//
//
// // sign of return value gives which side of road it is on.
// Road_distance = (-deltaY * Eunit_x1000 + deltaX * Nunit_x1000) / 1000;
Road_distance = sqrt( (RoadDX_mm * RoadDX_mm) + (RoadDY_mm * RoadDY_mm));
//Why do percentage computation like this?
pc = (deltaX * Eunit_x1000 + deltaY * Nunit_x1000) / (Nodes[cur_node].Distance[cur] * 10);
// Serial.println("Closest_mm " + String(closest_mm) + "\t Road_distance " + String(Road_distance));
// Serial.println("Road Distance " + String(Road_distance));
// Serial.println("closest Distance " + String(closest_mm));
if (abs(Road_distance) < abs(closest_mm) && pc >= 0 && pc <= 100)
{
closest_mm = Road_distance;
*k = destination;
*perCent = pc;
}
}
return closest_mm;
}
/*---------------------------------------------------------------------------------------*/
void FindClosestRoad(waypoint *start, waypoint *road) //populate road with best road from start
{
// long closest_mm = MAX_DISTANCE;
// long dist;
// int close_index;
// int perCent;
// long done = 1;//2000;
// int i, node_successor;
//
// for (i = 0; i < 5/*map_points*/; i++) // find closest road.
// {
// dist = distance(i, &node_successor, start->east_mm, start->north_mm, &perCent); //next node to visit
//// Serial.println("Start : Latitude " + String(start->latitude) + "\t Longitude " + String(start->longitude) + "\t Dist "
//// + String(dist));
//
// if (abs(dist) < abs(closest_mm))
// {
// close_index = node_successor;
// closest_mm = dist;
// done = 1;// perCent; //Not really true amount of nodes done?
// road->index = i;
// road->sigma_mm = node_successor;
// }
// }
// if (closest_mm < MAX_DISTANCE)
// {
// i = road->index; //0
// node_successor = close_index; //0
// road->east_mm = Nodes[i].east_mm + done *(Nodes[node_successor].east_mm - Nodes[i].east_mm) / 100;
// road->north_mm = Nodes[i].north_mm + done *(Nodes[node_successor].north_mm - Nodes[i].north_mm) / 100;
// }
// else
// {
// for (i = 0; i < 5/*map_points*/; i++) // find closest node
// {
//// Serial.println("I got here");
// dist = start->distance_mm(Nodes[i].east_mm, Nodes[i].north_mm);
//
// if (dist < closest_mm)
// {
// close_index = i;
// closest_mm = dist;
// }
// }
// road->index = road->sigma_mm = close_index;
// road->east_mm = Nodes[close_index].east_mm;
// road->north_mm = Nodes[close_index].north_mm;
// }
//
// road->Evector_x1000 = 1000;
// road->Nvector_x1000 = 0;
// road->time_ms = 0;
// road->speed_mmPs = DESIRED_SPEED_mmPs;
// Test FindClosest Road:
// Serial.println("Distance " + String(dist));
// Serial.println("Road : East_mm " + String(road->east_mm) + "\t North_mm " + String(road->north_mm));
}
//Test ClosestRoad:
void test_closestRoad(){
waypoint roadOrigin;
waypoint roadDestination;
Serial.println("First " );
FindClosestRoad(&mission[0], &roadOrigin);
Serial.println();
Serial.println("Second ");
FindClosestRoad(&mission[3], &roadDestination);
for(int last = 0; last < 4; last++){
Serial.println(" mission " + String(mission[last].east_mm ) + "\t roadOrigin " + String(roadOrigin.east_mm));
Serial.println(" mission " + String(mission[last].longitude ) + "\t roadOrigin " + String(roadOrigin.east_mm));
}
for(int last = 0; last < 4; last++){
Serial.println(" mission " + String(mission[last].east_mm ) + "\t roadOrigin " + String(roadDestination.east_mm));
Serial.println(" mission " + String(mission[last].longitude ) + "\t roadOrigin " + String(roadDestination.north_mm));
}
}
/*---------------------------------------------------------------------------------------*/
// start and destination are on the road network given in Nodes.
// start is in Path[1].
// Place other junction waypoints into Path.
// Returned value is next index into Path.
// start->index identifies the closest node.
// sigma_mm holds the index to the other node.
// A* is traditionally done with pushing and popping node from an Open and Closed list.
// Since we have a small number of nodes, we instead reserve a slot on Open and Closed
// for each node.
int BuildPath (int j, waypoint* start, waypoint* destination)// Construct path backward to start.
{
Serial.println("To break");
int last = 1;
int route[map_points];
int i, k, node;
long dist_mm;
k = map_points-1;
route[k] = j;
while(Open[j].ParentID != currentlocation)
{
j = route[--k] = Open[j].ParentID;
}
Path[last] = start;
for ( ; k < map_points; k++)
{
node = route[k];
Path[++last].east_mm = Nodes[node].east_mm;
Path[last].north_mm = Nodes[node].north_mm;
}
Path[++last] = destination;
for (k = 0; k <= last; k++)
{
if(k > 0) Path[k].sigma_mm = 10; // map should be good to a cm.
Path[k].index = k;
Path[k].speed_mmPs = DESIRED_SPEED_mmPs;
Path[k].Compute_LatLon(); // this is never used
}
last++;
for (j = 0; j < last-1; j++)
{
Path[j].vectors(&Path[j+1]);
}
return last;
}
/*---------------------------------------------------------------------------------------*/
void test_buildPath(){
// BuildPath(0, Path, destination);
}
int FindPath(waypoint *start, waypoint *destination)//While OpenSet is not empty
{
Serial.println("Start East_mm " + String(start->east_mm) + "\t North " + String(start->north_mm));
Serial.println("Start East_mm " + String(destination->east_mm) + "\t North " + String(destination->north_mm));
long ClosedCost[map_points];
int i, neighbor, k;
long NewCost, NewStartCost, NewCostToGoal;
long NewIndex;
long BestCost, BestID;
bool Processed = false;
for (i = 0; i < map_points; i++)// mark all nodes as empty
{
Open[i].TotalCost = MAX_DISTANCE;
ClosedCost[i] = MAX_DISTANCE;
}
i = start->index; // get successor nodes of start
Open[i].CostFromStart = start->distance_mm(Nodes[i].east_mm, Nodes[i].north_mm);
Open[i].CostToGoal = destination->distance_mm(Nodes[i].east_mm, Nodes[i].north_mm);
Open[i].TotalCost = Open[i].CostFromStart + Open[i].CostToGoal;
Open[i].ParentID = currentlocation;
i = start->sigma_mm;
Open[i].CostFromStart = start->distance_mm(Nodes[i].east_mm, Nodes[i].north_mm);
Open[i].CostToGoal = destination->distance_mm(Nodes[i].east_mm, Nodes[i].north_mm);
Open[i].TotalCost = Open[i].CostFromStart + Open[i].CostToGoal;
Open[i].ParentID = currentlocation;
while (BestCost < MAX_DISTANCE) //While OpenSet is not empty
{
BestCost = MAX_DISTANCE;
BestID = -1;
// pop lowest cost node from Open; i.e. find index of lowest cost item
for (i = 0; i < 6; i++)
{
if (Open[i].TotalCost < BestCost)
{
BestID = i;
// Serial.println("BESTID " + String(BestID));
BestCost = Open[i].TotalCost;
// Serial.println("BestCost " + String(BestCost));
}
}
if (BestID < 0)
{
return INVALID;
}
Serial.println("BESTID " + String(BestID));
Serial.println("DestinationINdex " + String(destination->index));
Open[BestID].TotalCost = MAX_DISTANCE; // Remove node from "stack".
if (BestID == destination->index || BestID == destination->sigma_mm)// Done:: reached the goal!!
{
// return BuildPath(BestID, start, destination); // Construct path backward to start.
return 5;
}
i = BestID; // get successor nodes from map
for (neighbor = 0; neighbor < 5; neighbor++)
{
NewIndex = Nodes[i].destination[neighbor];
if (NewIndex == END)continue; // No success in this slot
NewStartCost = Open[i].CostFromStart + Nodes[i].Distance[neighbor];
NewCostToGoal = destination->distance_mm(Nodes[NewIndex].east_mm, Nodes[NewIndex].north_mm);
NewCost = NewStartCost + NewCostToGoal;
if (NewCost >= ClosedCost[NewIndex]) // check if this node is already on Open or Closed.
continue; // Have already looked at this node
else if (ClosedCost[NewIndex] != MAX_DISTANCE) // looked at this node before, but at a higher cost
{
ClosedCost[NewIndex] = MAX_DISTANCE; // remove node from Closed
}
if (NewCost >= Open[NewIndex].TotalCost)
continue; // This node is a less efficient way of getting to a node on the list
// Push successor node onto stack.
Open[NewIndex].CostFromStart = NewStartCost;
Open[NewIndex].CostToGoal = NewCostToGoal;
Open[NewIndex].TotalCost = NewCost;
Open[NewIndex].ParentID = i;
} // end of successor nodes
ClosedCost[BestID] = BestCost; // Push node onto Closed
}
Serial.println("Destination East_mm " + String(destination->east_mm) + "\t North " + String(destination->north_mm));
return 0; // failure
}
/*---------------------------------------------------------------------------------------*/
// Low level path is a straight line from start to detination.
// PathPlan makes an intermediate level path that uses as many roads as possible.
//start = currentlocation: destination = heading to;
int PlanPath (waypoint *start, waypoint *destination)
{
// Serial.println("Start : East_mm = " + String(start->east_mm) + "\t North_mm = " + String(start->north_mm));
waypoint roadOrigin, roadDestination;
int last = 0;
Path[0] = start;
Path[0].index = 0;
// FindClosestRoad( start, &roadOrigin );
// FindClosestRoad( destination, &roadDestination );
// int w = abs(start->east_mm - roadOrigin.east_mm) + abs(start->north_mm - roadOrigin.north_mm);
// int x = abs(destination->east_mm - roadDestination.east_mm) + abs(destination->north_mm - roadDestination.north_mm);
//
// int straight_dist = 190 * abs(start->east_mm - destination->east_mm)+ abs(start->north_mm - destination->north_mm);
//// if (w + x >= straight_dist) // don't use roads; go direct
// {
// last = 1;
//// Serial.println("In Straight");
// }
//
// else // use A* with the road network
// {
// Serial.println("In Else");
Path[1] = roadOrigin;
Path[1].index = 1;
destination -> index = 7;
// last = FindPath(roadOrigin, roadDestination);
last = FindPath(start, destination);
// }
Path[last] = destination;
Path[last-1].vectors(&Path[last]);
Path[last].Evector_x1000 = Path[last-1].Evector_x1000;
Path[last].Nvector_x1000 = Path[last-1].Nvector_x1000;
Path[last].index = last | END;
// Serial.println("Destination : East_mm = " + String(destination->east_mm) + "\t North_mm = " + String(destination->north_mm));
Serial.println();
return last;
}
/*---------------------------------------------------------------------------------------*/
// Transmit the path to C3 Pilot over a serial line.
void SendPath(waypoint *course, int count)
{
SerialData results;
for( int i = 0; i < count; i++)
{
results.clear();
results.number = i;
results.kind = MsgType::seg;
results.posE_cm = course->east_mm / 10;//data.posE_cm + 4;
results.posN_cm = course->north_mm / 10;//data.posN_cm + 5;
float angle = atan2(course->Nvector_x1000, course->Evector_x1000) * 180 / PI + 90.;//data.posE_cm;
results.bearing_deg = (long) (-angle); //data.bearing_deg + 8; // (long) (-angle);
results.speed_cmPs = course->speed_mmPs / 10;//data.speed_cmPs + 9;
results.write(&Serial2);
delay(100);
}
}
//Converts provided Longitude and Latitude to MM
boolean convertLatLonToMM(long latitude, long longitude){
long scaledOriginLat = LATITUDE_ORIGIN*1000000;
long scaledOriginLon = LONGITUDE_ORIGIN*1000000;
double dLat = (latitude) * (PI/180.0) - (scaledOriginLat)* (PI / 180.0);
double dLon = latitude * (PI/180.0) - scaledOriginLon * (PI / 180.0);
Serial.println("dLat = " + String(dLat) + " dLon = " + String(dLon));
double soln = sin(dLat/2) * cos(dLat/2) + cos(scaledOriginLat * PI / 180) * cos(latitude * PI / 180) * sin(dLon/2) * sin(dLon/2);
double ans = 2 * atan2(sqrt(soln), sqrt(1-soln));
double finalAns = EARTH_RADIUS_MM * ans;
return true;
}
/*---------------------------------------------------------------------------------------*/
// LoadMap
// Loads the map nodes from a file.
// Takes in the name of the file to load and loads the appropriate map.
// Returns true if the map was loaded.
// Returns false if the load failed.
boolean LoadMap(char* fileName)
{
// open the file. note that only one file can be open at a time,
// so you have to close this one before opening another.
File myFile = SD.open(fileName, FILE_READ);
// if the file opened okay, read from it:
if (myFile) {
// Initialize a string buffer to read lines from the file into
// Allocate an extra char at the end to add null terminator
char* buffer = (char*)malloc(myFile.size()+1);
// index for the next character to read into buffer
char* ptr = buffer;
// read from the file until there's nothing else in it:
while (myFile.available()) {
*ptr = myFile.read();
++ptr;
}
// Null terminate the buffer string
*ptr = '\0';
// Set up tokenizer for the file buffer string
char delimiter[2] = ",\n";
char* token;
int col = 0;
int row = 0;
// get the first token
token = strtok(buffer, delimiter);
// walk through other tokens
while( token != NULL )
{
switch (col % 10)
{
case 0: // latitude
Nodes[row].east_mm = atol(token);
col++;
break;
case 1: // longitude
Nodes[row].north_mm = atol(token);
col++;
break;
case 2: // filename
if (token == "END")
{
Nodes[row].destination[0] = END;
}
else
{
Nodes[row].destination[0] = atoi(token);
}
col++;
break;
case 3: // filename
if (token == "END")
{
Nodes[row].destination[1] = END;
}
else
{
Nodes[row].destination[1] = atoi(token);
}
col++;
break;
case 4: // filename
if (token == "END")
{
Nodes[row].destination[2] = END;
}
else
{
Nodes[row].destination[2] = atoi(token);
}
col++;
break;
case 5: // filename
if (token == "END")
{
Nodes[row].destination[3] = END;
}
else
{
Nodes[row].destination[3] = atoi(token);
}
col++;
break;
case 6: // filename
Nodes[row].Distance[0] = atol(token);
col++;
break;
case 7: // filename
Nodes[row].Distance[1] = atol(token);
col++;
break;
case 8: // filename
Nodes[row].Distance[2] = atol(token);
col++;
break;
case 9: // filename
Nodes[row].Distance[3] = atol(token);
convertLatLonToMM(Nodes[row].east_mm, Nodes[row].north_mm);
col++;
row++;
break;
default: // unexpected condition; print error
Serial.print("Unexpected error happened while reading map description file.");
Serial.print("Please verify the file is in the correct format.");
Serial.println("Planner may not work correctly if this message appears.");
break;
}
token = strtok(NULL, delimiter);
}
map_points = row;
//To test LoadMap:
for (int i = 0; i < map_points; i++)
{
Serial.print(Nodes[i].east_mm);
Serial.print(",");
Serial.print(Nodes[i].north_mm);
Serial.print(",");
Serial.print(Nodes[i].destination[0]);
Serial.print(",");
Serial.print(Nodes[i].destination[1]);
Serial.print(",");
Serial.print(Nodes[i].destination[2]);
Serial.print(",");
Serial.print(Nodes[i].destination[3]);
Serial.print(",");
Serial.print(Nodes[i].Distance[0]);
Serial.print(",");
Serial.print(Nodes[i].Distance[1]);
Serial.print(",");
Serial.print(Nodes[i].Distance[2]);
Serial.print(",");
Serial.println(Nodes[i].Distance[3]);
}
// If file loaded, read data into Nodes[]
free(buffer);
myFile.close();
} else {
// if the file didn't open, print an error:
myFile.close();
Serial.print("error opening ");
Serial.print(fileName);
return false;
}
return true;
}
/*---------------------------------------------------------------------------------------*/
// SelectMap: Return the file name of the closest origin
// Determines which map to load.
// Takes in the current location as a waypoint and a string with the name of the file that
// contains the origins and file names of the maps.
// Determines which origin is closest to the waypoint and returns it as a junction.
// Assumes the file is in the correct format according to the description above.
void SelectMap(waypoint currentLocation, char* fileName, char* nearestMap)
{
// open the file. note that only one file can be open at a time,
// so you have to close this one before opening another.
File myFile = SD.open(fileName, FILE_READ);
// if the file opened okay, read from it:
if (myFile) {
// Initialize a string buffer to read lines from the file into
// Allocate an extra char at the end to add null terminator
char* buffer = (char*)malloc(myFile.size()+1);
//Serial.println("Printing file info");
// index for the next character to read into buffer
char* ptr = buffer;
// read from the file until there's nothing else in it:
while (myFile.available()) {
*ptr = myFile.read();
++ptr;
}
// Null terminate the buffer string
*ptr = '\0';
// Set up storage for coordinates and file names
// Note: we malloc here because the stack is too small
float* map_latitudes = (float*)malloc(MAX_MAPS * 4);
float* map_longitudes = (float*)malloc(MAX_MAPS * 4);
char** map_file_names = (char**)malloc(MAX_MAPS * sizeof(char*));
for(int i = 0; i < MAX_MAPS; i++)
{
// initialize using invalid values so that we can ensure valid data in allocated memory
map_latitudes[i] = 91;
map_longitudes[i] = 181;
map_file_names[i] = "";
}
// Set up tokenizer for the file buffer string
char delimiter[] = ", \n";
char* token;
int col = 0;
int row = 1;
// get the first token
token = strtok(buffer, delimiter);
// fill the map_latitude, map_longitude, & map_file with tokens
while( token != NULL )
{
switch (col % 3)
{
case 0: // latitude
map_latitudes[row] = atof(token);
col++;
break;
case 1: // longitude
map_longitudes[row] = atof(token);
col++;
break;
case 2: // filename
map_file_names[row] = token;
col++;
break;
default: // unexpected condition; print error
Serial.println("Unexpected error happened while reading map description file. Please verify the file is in the correct format. Planner may not work correctly if this message appears.");
break;
}
token = strtok(NULL, delimiter);
}
int closestIndex = -1;
long closestDistance = MAX_DISTANCE;
for (int i = 0; i < MAX_MAPS; i++)
{
int dist = sqrt((map_latitudes[i] - currentLocation.latitude)*(map_latitudes[i] - currentLocation.latitude) +
(map_longitudes[i] - currentLocation.longitude)*(map_longitudes[i] - currentLocation.longitude));
if (dist < closestDistance)
{
closestIndex = i;
closestDistance = dist;
}
}
if (closestIndex >= 0)
{
// Determine closest map to current location
// Update Origin global variable
Origin.latitude = map_latitudes[closestIndex];
Origin.longitude = map_longitudes[closestIndex];
for (int i = 0; i < 13; i++)
{
nearestMap[i] = map_file_names[closestIndex][i];
}
}
else
{
Serial.println("error determining closest map.");
}
// Free the memory allocated for the buffer
free(buffer);
free(map_latitudes);
free(map_longitudes);
free(map_file_names);
// close the file:
myFile.close();
Serial.println("Map definitions loaded.");
} else {
// if the file didn't open, print an error:
myFile.close();
Serial.println("error opening map_defs.txt");
}
}
/*---------------------------------------------------------------------------------------*/
void initialize()
{
int last;
Origin.Evector_x1000 = INVALID;
Origin.Nvector_x1000 = INVALID;
Origin.east_mm = 0;
Origin.north_mm = 0;
Origin.latitude = INVALID;
Origin.longitude = INVALID;
Start.east_mm = 0;
Start.north_mm = 0;
Serial.println("Initializing SD card...");
pinMode(SS, OUTPUT);
if (!SD.begin(chipSelect)) {
Serial.println("initialization failed!");
}
Serial.println("initialization done.");
char nearestMap[13] = "";
// SelectMap(Start,"MAPDEFS.TXT",nearestMap); //pollutes info from map_def to nearestMap
LoadMap("MOCKMAP.TXT");// use nearestMap:: pollutes the Node, juction with the nearestMap
ConstructNetwork(Nodes, map_points); //To fill out the rest of the nodes info
/* Convert latitude and longitude positions to flat earth coordinates.
Fill in waypoint structure */
GetGoals(Nodes, CONES);
// SendPath(mission, CONES); //send mission to C3
// SendPath(mission, CONES); //send mission to C3
// last = PlanPath (&mission[0], &mission[3]);
// Path[last].index |= GOAL; //number of waypoint sequences
//SendPath(Path, MAX_WAYPOINTS); //Send a complete Path to C3
// SendPath(Path, MAX_WAYPOINTS); //Send a complete Path to C3
Serial.println();
}
/*---------------------------------------------------------------------------------------*/
void setup()
{
Serial.begin(9600);
Serial.flush();
// pinMode(DATA_READY, INPUT);
DataAvailable = true; //false
//attachInterrupt(0, DataReady, FALLING);
initialize();
Serial1.begin(baudrate);
Serial2.begin(baudrate);
ps.dt = &data;
ps.input = &Serial2;
ps.output = &Serial2;
ps.capture = MsgType::sensor;
}
/*---------------------------------------------------------------------------------------*/
//Test mission::a list of waypoints to cover
void test_mission(){
for(int i = 0; i <map_points; i++)
{
Serial.println("mission " + String(i) + " = " + String(mission[i].latitude) +"\t north_mm " + String(mission[i].north_mm));
}
}
void loop()
{
static int Goal = 2;
int last;//what is last?
/*
Maintain a list of waypoints from current position to destination.
Eliminate any waypoints that have already been visited.
About once a second, write the current position and remaining mission, so that if we
get a reset, we can start from where we left off.
*/
// if (DataAvailable)
// {
// ParseStateError r = ps.update();// read vehicle estimated position from C6
// if(r == ParseStateError::success)
// {
// digitalWrite(C4_DATA_SENT, HIGH); // transition interrupts the processor
digitalWrite(C4_DATA_SENT, LOW);
//Start.readPointString(1000, 0);
// last = 0;
Start.east_mm = Nodes[0].east_mm;//Path[last].east_mm;
Start.north_mm = Nodes[0].north_mm;
last = PlanPath (&Start, &mission[0]);
// Serial.println(last);
// Path[last].index |= GOAL;
// if(Start.east_mm == mission[3].east_mm && Start.north_mm == mission[3].north_mm)break;
// test_path();
// TO DO: Make a fine path, providing proper curve path
// if (last < MAX_WAYPOINTS / 2 && Goal < CONES)
// {
// last = PlanPath (&Start, &mission[++Goal]);
// Path[last].index |= GOAL;
// }
// Serial.println( "Last = " + String(last));
//
// SendPath(Path, 2); // send data to C3
//}
// test_mission();
// test_distance();
// test_closestRoad();
long end_time = millis() + 1000;// delay for 1 sec, waiting for data
while (millis() < end_time)
{
if (DataAvailable) break;
}
}
///MOCK_MAP.TXTT
void test_path(){
for(int i = 0; i < 4; i++){
Serial.println("Path " + String(Path[i].east_mm));
}
}
void test_distance()
{
int percent ;
int destination;
int dist = distance(0, &destination ,0 , 0, &percent);
Serial.println("dist " + String(dist));
}