1 fyp2016/Logger.cpp
View file
@@ -8,6 +8,7 @@

Logger const& Logger::operator<<(std::ostream& (*F)(std::ostream&)) const {

Log::process();
Log::repostTime = true;

if (!suppressed) {
15 fyp2016/Logger.h
View file
@@ -23,15 +23,17 @@ class Logger {
short color;
//HRTimer* timer;
std::chrono::time_point<std::chrono::system_clock> start;
std::stringstream* stream;

public:

Logger() { start = std::chrono::system_clock::now(); suppressed = false; color = 0x0a; }
Logger(std::chrono::time_point<std::chrono::system_clock> s) : start(s) { suppressed = false; color = 0x0a; }
Logger(std::chrono::time_point<std::chrono::system_clock> s, short col) : start(s), color(col) { suppressed = false; }

void suppress(bool b) { suppressed = b; }
bool isSuppressed() { return suppressed; }

void setStringStream(std::stringstream* ss) { stream = ss; }

template<typename T>
const Logger& operator<<(const T& v) const;
@@ -55,12 +57,23 @@ inline const Logger& Logger::operator<<(const T& v) const {

char buf[20];
sprintf(buf, "%12.6f", seconds.count());


if (Log::repostTime)
std::cout << "[" << buf << "]\t" << v;
else
std::cout << v;


if (stream != NULL) {
if (Log::repostTime) {
(*stream) << "[" << buf << "]\t" << v;
}
else
(*stream) << v;
}
//Log::process();

Log::repostTime = false;
}
return *this;
12 fyp2016/NavigationSystem.h
View file
@@ -2,6 +2,7 @@

#include <vector>
#include "Point.h"
#include "LatLng.h"

class NavigationSystem
{
@@ -17,20 +18,27 @@ class NavigationSystem
*/
virtual void clearPath() = 0;

/*
sets the position (in LatLng) of the relative calculations
so that everything else can be transformed to metres
*/
virtual void setBaseLocation(LatLng) = 0;

/*
sets the navigation path to be this list of points, in the order given
*/
virtual void setPath(std::vector<Point*>) = 0;
virtual void setPath(std::vector<Point>) = 0;

/*
adds the current point to the navigation path, appending to the end of the list
*/
virtual void addPoint(Point) = 0;
virtual void addPoint(LatLng) = 0;

/*
returns the navigation path
*/
virtual std::vector<Point*> getPath() = 0;
virtual std::vector<Point> getPath() = 0;

/*
Prepares the list of points into a path that can be followed by the actual vehicle.
42 fyp2016/Overlord.cpp
View file
@@ -30,6 +30,7 @@ bool Overlord::initialise() {
Log::i << "-> Initialising navigation system..." << endl;
ns = new SimpleNavigator();
ns->initialise();
ns->subdivide();
Log::i << "-> NAVIGATION SYSTEM DONE" << endl << endl;

Log::i << "-> Initialising dummy hardware interface..." << endl;
@@ -77,8 +78,8 @@ void Overlord::run() {
window->showWindow(true);

// Feature detector stuff
//fd->loadScan();
//fd->createImage(DISPLAY_KERNEL);
fd->loadScan();
fd->createImage(DISPLAY_KERNEL);

//populating the path
/*ns->addPoint(Point(-2.5, -3));
@@ -87,9 +88,9 @@ void Overlord::run() {
ns->addPoint(Point(10, -3));
ns->subdivide();*/

fd->runScan();
fd->createImage(DISPLAY_RAW);
window->update(fd->retrieveImage(), PANE_BOTTOMLEFT);
//fd->runScan();
//fd->createImage(DISPLAY_RAW);
//window->update(fd->retrieveImage(), PANE_BOTTOMLEFT);
window->update(NULL, PANE_BOTTOMRIGHT);

showvp = true;
@@ -248,6 +249,8 @@ void Overlord::handleEvents() {
break;
case ID_BRAKE:
Log::d << "Action: brake" << endl;
//TODO(Jono) this needs to not be here
dc->setEnabled(!dc->isEnabled());
handled = true;
break;
case ID_HANDBRAKE_OFF:
@@ -260,7 +263,7 @@ void Overlord::handleEvents() {
break;
case ID_REQ_QUAD_SPEED:
{
float speed = hwi->getVelocity();
float speed = dhwi->getVelocity();
//Log::d << "Request: quad speed " << speed << endl;
Packet* op = new Packet();
op->packetID = ID_QUAD_SPEED;
@@ -273,7 +276,7 @@ void Overlord::handleEvents() {
}
case ID_REQ_QUAD_HEADING:
{
float head = hwi->getAbsoluteHeading();
float head = dhwi->getAbsoluteHeading();
//Log::d << "Request: quad speed " << speed << endl;
Packet* op = new Packet();
op->packetID = ID_QUAD_HEADING;
@@ -286,7 +289,7 @@ void Overlord::handleEvents() {
}
case ID_REQ_QUAD_POSITION:
{
Point pos = hwi->getPosition();
Point pos = dhwi->getPosition();
//Log::d << "Request: quad speed " << speed << endl;
Packet* op = new Packet();
op->packetID = ID_QUAD_POSITION;
@@ -301,6 +304,23 @@ void Overlord::handleEvents() {
case ID_NAV_PATH:
{
Log::i << "Received navigation path" << endl;
dc->setEnabled(false);
ns->clearPath();
Point dummyPoints = Point(-2.5, -3);
ns->addPoint(dummyPoints);
/*
dummyPoints = Point(-5, 2);
ns->addPoint(dummyPoints);
dummyPoints = Point(0, 2.5);
ns->addPoint(dummyPoints);
dummyPoints = Point(0, -3);
ns->addPoint(dummyPoints);
dummyPoints = Point(0, -2);
ns->addPoint(dummyPoints);
dummyPoints = Point(5, 4);
ns->addPoint(dummyPoints);
*/

union u_tag {
float f[2];
double dval;
@@ -315,9 +335,15 @@ void Overlord::handleEvents() {
u.f[1] = p->data[i + 3];

double lon = u.dval;

ns->addPoint(LatLng(lat, lon));


Log::i << "\t Lat/Lng: " << std::setprecision(16) << lat << "E " << lon << "N " << endl;
//Log::i << "\t Lat/Lng: " << std::setprecision(10) << p->data[i] << "E " << p->data[i + 1] << "N" << endl;
}
ns->subdivide();
vp->drawPathToTexture();
handled = true;
break;
}
1 fyp2016/Packet.h
View file
@@ -30,6 +30,7 @@ enum ID : uint8_t {
ID_NAV_POINTS = 0x61,
ID_NAV_PATH = 0x62,
ID_NAV_ZONE = 0x63,
ID_NAV_BASELOC = 0x64,

ID_SET_QUAD_THROTTLE = 0x70,
ID_SET_QUAD_STEERING = 0x71,
13 fyp2016/SimpleController.cpp
View file
@@ -4,6 +4,7 @@
SimpleController::SimpleController()
{
alive = true;
enabled = false;
navState = NAV_CRUISE;
pathTravDir = 1;
landmineDetected = false;
@@ -71,7 +72,9 @@ bool SimpleController::updateLoop() {

// TODO(harry) : this needs to be time independent.
// TODO(jono) : this is time independent...
updateDynamics();
if (enabled) {
updateDynamics();
}

//hrt.reset();
start = end;
@@ -96,19 +99,19 @@ void SimpleController::updateDynamics() {
}

// find angle between heading and to the next path point
double angleToPathPoint = -1 * atan2(ns->getPath().at(currentPathPoint)->y - hwi->getPosition().y, ns->getPath().at(currentPathPoint)->x - hwi->getPosition().x) + PI / 2;
double angleToPathPoint = -1 * atan2(ns->getPath().at(currentPathPoint).y - hwi->getPosition().y, ns->getPath().at(currentPathPoint).x - hwi->getPosition().x) + PI / 2;
if (angleToPathPoint > PI) angleToPathPoint -= 2 * PI;
if (angleToPathPoint < -PI) angleToPathPoint += 2 * PI;
double alpha = angleToPathPoint - hwi->getAbsoluteHeading();
double distance = hwi->getPosition().getDistanceTo(*ns->getPath().at(currentPathPoint));
double distance = hwi->getPosition().getDistanceTo(ns->getPath().at(currentPathPoint));
double steerAngleReq = -atan(2 * hwi->wheelBase * sin(alpha) / distance);

if (steerAngleReq > hwi->maxSteerAngle) steerAngleReq = hwi->maxSteerAngle;
if (steerAngleReq < -hwi->maxSteerAngle) steerAngleReq = -hwi->maxSteerAngle;
hwi->setDesiredSteeringAngle(steerAngleReq);


if (distance > hwi->getPosition().getDistanceTo(*ns->getPath().at(currentPathPoint + pathTravDir))) {
if (distance > hwi->getPosition().getDistanceTo(ns->getPath().at(currentPathPoint + pathTravDir))) {
if (navState != NAV_LANDMINE_DETECTED) {
// this means that we need to change direction when the quadbike reaches currentPathPoint (turn inbound?).
navState = NAV_TURNINBOUND;
@@ -152,7 +155,7 @@ void SimpleController::updateDynamics() {
while (loop) {
currentPathPoint--;
Point curPos = hwi->getPosition();
distance = curPos.getDistanceTo(*ns->getPath().at(currentPathPoint));
distance = curPos.getDistanceTo(ns->getPath().at(currentPathPoint));
if (initialDist - distance < diff) {
loop = false;
}
67 fyp2016/SimpleNavigator.cpp
View file
@@ -6,6 +6,7 @@ SimpleNavigator::SimpleNavigator()
{
simpleTurnMaxAngleRad = 50 * PI / 180;
//simpleTurnMaxAngleRad = 0;
baseLoc = LatLng(-34.919, 138.603);
}


@@ -16,19 +17,37 @@ SimpleNavigator::~SimpleNavigator()

bool SimpleNavigator::initialise() {

// TODO(): subdivide() here for testing purposes at the moment
subdivide();
//filling path with dummy points for testing purposes:
Point dummyPoints = Point(-2.5, -3);
addPoint(dummyPoints);
dummyPoints = Point(-4, 1.5);
addPoint(dummyPoints);
dummyPoints = Point(0, 1.5);
addPoint(dummyPoints);
dummyPoints = Point(0, -3);
addPoint(dummyPoints);
dummyPoints = Point(0, -2);
addPoint(dummyPoints);
dummyPoints = Point(5, 4);
addPoint(dummyPoints);


Log::d << "Navigator initialised." << std::endl;
return true;
}

void SimpleNavigator::clearPath() {
//TODO(Harry)

path.clear();
}

void SimpleNavigator::setPath(std::vector<Point>) {

}

void SimpleNavigator::setPath(std::vector<Point*>) {

void SimpleNavigator::setBaseLocation(LatLng ll) {
baseLoc = ll;
}


@@ -38,11 +57,13 @@ else when we go to subdivide it is going to try and subdivide points which have
already been subdivided.
*/
void SimpleNavigator::addPoint(Point p) {
Point* np = new Point(p.x, p.y);

path.push_back(np);
path.push_back(p);
}
void SimpleNavigator::addPoint(LatLng ll) {
Point rel = ll.relativeFrom(baseLoc);

path.push_back(rel);
}



@@ -64,25 +85,13 @@ bool SimpleNavigator::subdivide() {
std::vector<double> turnAngleListDegrees = {
0, 26, 62.6, 81.5, 101.9, 125.1, 148.3, 180 };

std::vector<Point> subdividedPath;

//filling path with dummy points for testing purposes:
Point dummyPoints = Point(-2.5, -3);
addPoint(dummyPoints);
dummyPoints = Point(-4, 1.5);
addPoint(dummyPoints);
dummyPoints = Point(0, 1.5);
addPoint(dummyPoints);
dummyPoints = Point(0, -3);
addPoint(dummyPoints);
dummyPoints = Point(0, -2);
addPoint(dummyPoints);
dummyPoints = Point(5, 4);
addPoint(dummyPoints);
//std::vector<Point> subdividedPath;
subdividedPath.clear();


// for each line segment (each line between two 'ultimate' waypoints)
Point curPoint = *path.at(0);
Point nexPoint = *path.at(1);
Point curPoint = path.at(0);
Point nexPoint = path.at(1);
for (unsigned int i = 0; i < path.size() - 1; i++) {

/*
@@ -94,7 +103,7 @@ bool SimpleNavigator::subdivide() {
//Point curPoint = *path.at(i);
//Point nexPoint = *path.at(i + 1);

nexPoint = *path.at(i + 1);
nexPoint = path.at(i + 1);

Point directionVector = Point(nexPoint.x - curPoint.x,
nexPoint.y - curPoint.y);
@@ -137,7 +146,7 @@ bool SimpleNavigator::subdivide() {

// figure out the turn angle
double angle1 = atan2(curPoint.y - nexPoint.y, curPoint.x - nexPoint.x);
double angle2 = atan2(nexPoint.y - path.at(i + 2)->y, nexPoint.x - path.at(i + 2)->x);
double angle2 = atan2(nexPoint.y - path.at(i + 2).y, nexPoint.x - path.at(i + 2).x);

// positive is clockwise. turn angle from -pi to pi
double turnAngle = (angle1 - angle2);
@@ -307,6 +316,7 @@ bool SimpleNavigator::subdivide() {
curPoint = subdividedPath.at(subdividedPath.size() - 1);
}

/*
for (unsigned int i = 0; i < path.size(); i++) {
delete path.at(i);
}
@@ -316,6 +326,7 @@ bool SimpleNavigator::subdivide() {
//Log::i << subdividedPath.at(i).x << ", " << subdividedPath.at(i).y << endl;
}
subdividedPath.clear();
*/
Log::d << "Path subdivision completed" << endl;
return false;
}
@@ -381,8 +392,8 @@ bool SimpleNavigator::startPath() {
return true;
}

std::vector<Point*> SimpleNavigator::getPath() {
return path;
std::vector<Point> SimpleNavigator::getPath() {
return subdividedPath;
}


32 fyp2016/SimpleNavigator.h
View file
@@ -13,32 +13,38 @@
class SimpleNavigator :
public NavigationSystem
{
private:
std::vector<Point*> path;

double simpleTurnMaxAngleRad;

std::chrono::time_point<std::chrono::high_resolution_clock> startTime;
std::chrono::time_point<std::chrono::high_resolution_clock> endTime;

std::thread* updater;
double getDeltaY(double radians);

public:
SimpleNavigator();
~SimpleNavigator();

bool initialise();
void clearPath();

void setPath(std::vector<Point*>);
std::vector<Point*> getPath();
void setBaseLocation(LatLng);
void setPath(std::vector<Point>);
std::vector<Point> getPath();
void addPoint(Point);
void addPoint(LatLng);

bool subdivide();
bool startPath();

void loop();


private:
std::vector<Point> path;
std::vector<Point> subdividedPath;
LatLng baseLoc;

double simpleTurnMaxAngleRad;

std::chrono::time_point<std::chrono::high_resolution_clock> startTime;
std::chrono::time_point<std::chrono::high_resolution_clock> endTime;

std::thread* updater;
double getDeltaY(double radians);


};

38 fyp2016/VirtualPlatform.cpp
View file
@@ -46,9 +46,9 @@ bool VirtualPlatform::initialise(HardwareInterface* hwi, NavigationSystem* nav,

setupFont();

drawScale = 60;
focusX = -4;
focusY = 1.4;
drawScale = 15;
focusX = 0;
focusY = 0;

return true;
}
@@ -78,32 +78,32 @@ void VirtualPlatform::redrawGraphTexture() {
SDL_SetRenderDrawColor(simulationCanvas->getRenderer(), 0xEE, 0xEE, 0xF2, 0xFF);
SDL_RenderClear(simulationCanvas->getRenderer());


SimpleTexture graphVeloc = SimpleTexture(graphCanvas->getRenderer());
SimpleTexture graphSteer = SimpleTexture(graphCanvas->getRenderer());
SimpleTexture graphGear = SimpleTexture(graphCanvas->getRenderer());
SimpleTexture graphThrot = SimpleTexture(graphCanvas->getRenderer());

graphVeloc.loadFromSurface(velocityGraph->retrieveImage());
graphSteer.loadFromSurface(steerGraph->retrieveImage());
graphGear.loadFromSurface(gearGraph->retrieveImage());
graphThrot.loadFromSurface(throttleGraph->retrieveImage());

SDL_Rect destRectVeloc = { 0, 0, graphWidth, graphHeight };
SDL_Rect destRectSteer = { 0, (graphHeight), graphWidth, graphHeight };
SDL_Rect destRectGear = { 0, 2* (graphHeight), graphWidth, graphHeight };
SDL_Rect destRectThrot = { 0, 3* (graphHeight), graphWidth, graphHeight };
SDL_Rect destRectGear = { 0, 2 * (graphHeight), graphWidth, graphHeight };
SDL_Rect destRectThrot = { 0, 3 * (graphHeight), graphWidth, graphHeight };

SDL_RenderCopy(graphCanvas->getRenderer(), graphVeloc.getTexture(), NULL, &destRectVeloc);
SDL_RenderCopy(graphCanvas->getRenderer(), graphSteer.getTexture(), NULL, &destRectSteer);
SDL_RenderCopy(graphCanvas->getRenderer(), graphGear.getTexture(), NULL, &destRectGear);
SDL_RenderCopy(graphCanvas->getRenderer(), graphThrot.getTexture(), NULL, &destRectThrot);


drawText("Velocity", graphWidth - 5, 0 * (graphHeight), true);
drawText("Steering Angle", graphWidth - 5, 1 * (graphHeight), true);
drawText("Gear Selection", graphWidth-5, 2 * (graphHeight), true);
drawText("Throttle Percentage", graphWidth-5, 3 * (graphHeight), true);
drawText("Gear Selection", graphWidth - 5, 2 * (graphHeight), true);
drawText("Throttle Percentage", graphWidth - 5, 3 * (graphHeight), true);


// rendering text
@@ -120,20 +120,20 @@ void VirtualPlatform::redrawGraphTexture() {
titleText += vel;
titleText += " m/s";
//drawText(titleText, 840, 76);
drawText(titleText, graphWidth - 5, 1 * (graphHeight) - 18, true);
drawText(titleText, graphWidth - 5, 1 * (graphHeight)-18, true);

titleText = "Throttle: ";
titleText += std::to_string((int)round(hw->getRealThrottlePercentage()));
titleText += " %";
//drawText(titleText, 840, 376);
drawText(titleText, graphWidth - 5, 4 * (graphHeight) - 18, true);
drawText(titleText, graphWidth - 5, 4 * (graphHeight)-18, true);

titleText = "Gear: ";
if (hw->getRealGear() == 1) titleText += "Drive";
if (hw->getRealGear() == 0) titleText += "Neutral";
if (hw->getRealGear() == -1) titleText += "Reverse";
//drawText(titleText, 840, 276);
drawText(titleText, graphWidth - 5, 3 * (graphHeight) - 18, true);
drawText(titleText, graphWidth - 5, 3 * (graphHeight)-18, true);

titleText = "Brake Percentage: ";
std::string brakePerc = std::to_string((int)abs(hw->getRealBrakePercentage()));
@@ -145,10 +145,12 @@ void VirtualPlatform::redrawGraphTexture() {
titleText += stang;
titleText += " degrees";
//drawText(titleText, 840, 176);
drawText(titleText, graphWidth - 5, 2 * (graphHeight) - 18, true);


drawText(titleText, graphWidth - 5, 2 * (graphHeight)-18, true);

for (int i = 0; i < 10; i++) {
titleText = std::string(Log::getLineFromBack(i));
drawText(titleText, 10, 200 - 20*i);
}
SDL_SetRenderTarget(graphCanvas->getRenderer(), NULL);

}
@@ -164,8 +166,8 @@ void VirtualPlatform::drawPathToTexture() {
// drawing the path in this for loop
for (int i = 0; i < (int)ns->getPath().size() - 1; i++) {

Point loc1 = Point(ns->getPath().at(i)->x, ns->getPath().at(i)->y);
Point loc2 = Point(ns->getPath().at(i + 1)->x, ns->getPath().at(i + 1)->y);
Point loc1 = Point(ns->getPath().at(i).x, ns->getPath().at(i).y);
Point loc2 = Point(ns->getPath().at(i + 1).x, ns->getPath().at(i + 1).y);

// transformed (x,y) locations for drawing to screen (scale, computers inverted y coordinate, and focus point)
Point loc1transf = transform(loc1);
2 fyp2016/VirtualPlatform.h
View file
@@ -50,7 +50,7 @@ class VirtualPlatform
int textureWidth;
int textureHeight;

int drawScale = 1;
double drawScale = 0.2;
double focusX = 0;
double focusY = 0;

2 fyp2016/fyp2016.vcxproj
View file
@@ -130,6 +130,7 @@
<ClCompile Include="GPR.cpp" />
<ClCompile Include="Graph.cpp" />
<ClCompile Include="HardwareInterface.cpp" />
<ClCompile Include="LatLng.cpp" />
<ClCompile Include="Log.cpp" />
<ClCompile Include="Logger.cpp" />
<ClCompile Include="main.cpp" />
@@ -164,6 +165,7 @@
<ClInclude Include="GPR.h" />
<ClInclude Include="Graph.h" />
<ClInclude Include="HardwareInterface.h" />
<ClInclude Include="LatLng.h" />
<ClInclude Include="Packet.h" />
<ClInclude Include="QuadInterface.h" />
<ClInclude Include="Serial.h" />
6 fyp2016/fyp2016.vcxproj.filters
View file
@@ -78,6 +78,9 @@
<ClCompile Include="VirtualPlatform.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="LatLng.cpp">
<Filter>Source Files</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<Library Include="SDL2.lib">
@@ -175,6 +178,9 @@
<ClInclude Include="VirtualPlatform.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="LatLng.h">
<Filter>Header Files</Filter>
</ClInclude>
</ItemGroup>
<ItemGroup>
<None Include="..\makefile">