This repository has been archived by the owner on Aug 28, 2019. It is now read-only.
Permalink
Cannot retrieve contributors at this time
Name already in use
A tag already exists with the provided branch name. Many Git commands accept both tag and branch names, so creating this branch may cause unexpected behavior. Are you sure you want to create this branch?
elcano/Elcano_C4_Planner/Elcano_C4_Planner.ino
Go to fileThis commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
973 lines (825 sloc)
30.8 KB
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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)); | |
} | |