Code to run the microcontrollers on the R5 competition bot

Dependencies:   LineSensors mbed

Revision:
17:5046b27f5441
Child:
19:9f4510646c9e
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/r5map.cpp	Sat Apr 18 03:02:28 2015 +0000
@@ -0,0 +1,218 @@
+#include "r5map.h"
+#include <iostream>
+#include <cstdlib>
+
+R5Map::R5Map(int conSize) : size(conSize)
+{
+    int cellCount = 1;
+    
+    for(int i = 6; i >= 0; i--)
+    {
+        for(int j = 0; j < 7; j++)
+        {
+            map[j][i].number = cellCount++;
+        }
+    }
+
+    switch(size)
+    {
+        case 5 :
+            start.x = 5;
+            start.y = 0;
+            fin.x = 0;
+            fin.y = 5;
+            pos = start;
+            break;
+        case 6 :
+            start.x = 5;
+            start.y = 0;
+            fin.x = 0;
+            fin.y = 6;
+            pos = start;
+            break;
+        case 7 :
+            start.x = 6;
+            start.y = 0;
+            fin.x = 0;
+            fin.y = 5;
+            pos = start;
+            break;
+        default :
+            cerr<<"ERROR: Invalid R5Map size. Exiting."<<endl;
+            exit(EXIT_FAILURE); 
+    }
+}
+
+void R5Map::setPosition(position spot) 
+{
+    if(spot.x >= 0 && spot.y >= 0 && spot.x <= 6 && spot.y <= 6)
+    {
+        pos = spot;
+        map[pos.x][pos.y].visited = true;
+    }
+    else
+    {
+        cerr<<"ERROR: Position assignment with R5Map::setPosition(position) outside map bounds!"<<endl;
+        exit(EXIT_FAILURE);
+    }
+}
+
+void R5Map::setStart(position newStart)
+{
+    if(newStart.x >= 0 && newStart.y >= 0 && newStart.x <= 6 && newStart.y <= 6)
+        start = newStart;
+    else
+    {
+        cerr<<"ERROR: Start assignment with R5Map::setStart(position) outside map Bounds!"<<endl;
+        exit(EXIT_FAILURE);
+    }
+}
+
+void R5Map::setFinish(position newFin)
+{
+    if(newFin.x>= 0 && newFin.y >= 0 && newFin.x <= 6 && newFin.y <= 6)
+        fin = newFin;
+    else
+    {
+        cerr<<"ERROR: Finsih assignment with R5Map::setFinish(position) outside map Bounds!"<<endl;
+        exit(EXIT_FAILURE);
+    }
+}
+
+void R5Map::setWall(position cell, direction dir, wallState state)
+{
+    if(cell.x >= 0 && cell.y >= 0 && cell.x <= 6 && cell.y <= 6)
+    {
+        switch(dir)
+        {
+            case NORTH : map[cell.x][cell.y].north = state;
+                break;
+            case SOUTH : map[cell.x][cell.y].south = state;
+                break;
+            case EAST : map[cell.x][cell.y].east = state;
+                break;
+            case WEST : map[cell.x][cell.y].west = state; 
+        }
+    }
+    else
+    {
+        cerr<<"ERROR: Position selection in R5Map::setWall(position, cell, direction) outside map bounds!"<<endl;
+        exit(EXIT_FAILURE);
+    }
+}
+
+void R5Map::setCurWall(direction dir, wallState state)
+{
+    switch(dir)
+    {
+        case NORTH : map[pos.x][pos.y].north = state;
+            break;
+        case SOUTH : map[pos.x][pos.y].south = state;
+            break;
+        case EAST : map[pos.x][pos.y].east = state;
+            break;
+        case WEST : map[pos.x][pos.y].west = state;
+    }
+}
+
+void R5Map::setVisited(position cell) 
+{
+    if(cell.x >= 0 && cell.y >= 0 && cell.x <= 6 && cell.y <= 6)
+        map[cell.x][cell.y].visited = true;
+    else
+    {
+        cerr<<"ERROR: Position selection in R5Map::setVisited(position) outside of map bounds!"<<endl;
+        exit(EXIT_FAILURE);
+    }
+}
+
+void R5Map::setCurVisited() { map[pos.x][pos.y].visited = true; }
+
+position R5Map::getPosition() const { return pos; }
+
+position R5Map::getStart() const { return start; }
+
+position R5Map::getFinish() const { return fin; }
+
+wallState R5Map::getWall(position cell, direction dir) const
+{
+    if(cell.x >= 0 && cell.y >= 0 && cell.x <= 6 && cell.y <= 6)
+    {
+        switch(dir)
+        {
+            case NORTH : return map[cell.x][cell.y].north;
+            case SOUTH : return map[cell.x][cell.y].south;
+            case EAST : return map[cell.x][cell.y].east;
+            case WEST : return map[cell.x][cell.y].west;
+        }
+    }
+    else
+    {
+        cerr<<"ERROR: Position selection in R5Map::getWall(position, direction) outside of map bounds!"<<endl;
+        exit(EXIT_FAILURE);
+    }
+}
+
+wallState R5Map::getCurWall(direction dir) const
+{
+    switch(dir)
+    {
+        case NORTH : return map[pos.x][pos.y].north;
+        case SOUTH : return map[pos.x][pos.y].south;
+        case EAST : return map[pos.x][pos.y].east;
+        case WEST : return map[pos.x][pos.y].west;
+    }
+}
+
+bool R5Map::getVisited(position cell) const 
+{
+    if(cell.x >= 0 && cell.y >= 0 && cell.x <= 6 && cell.y <= 6)
+        return map[cell.x][cell.y].visited;
+    else
+    {
+        cerr<<"ERROR: Position selection in R5Map::getVisited(position) outside of map bounds!"<<endl;
+        return false;
+    }
+}
+
+bool R5Map::getCurVisited() const { return map[pos.x][pos.y].visited; }
+
+vector<char> R5Map::getSymbols(position cell) const 
+{
+    if(cell.x >= 0 && cell.y >= 0 && cell.x <= 6 && cell.y <= 6)
+        return map[cell.x][cell.y].symbols;
+    else
+    {
+        cerr<<"ERROR: Position selection in R5Map::getSymbols(position) outside of map bounds!"<<endl;
+        vector<char> empty;
+        return empty;
+    }
+}
+
+vector<char> R5Map::getCurSymbols() const { return map[pos.x][pos.y].symbols; }
+
+int R5Map::getSize() const { return size; }
+
+void R5Map::printMap() const
+{
+    for(int i = 0; i < size; i++)
+    {
+        for(int k = 0; k < size; k++)
+        {
+            cout<<"cell : "<<i+1<<','<<k+1<<endl;
+            cout<<"   North   : "<<map[i][k].north<<endl;
+            cout<<"   South   : "<<map[i][k].south<<endl;
+            cout<<"   East    : "<<map[i][k].east<<endl;
+            cout<<"   West    : "<<map[i][k].west<<endl;
+            cout<<"   Symbols : ";
+
+            for(int j = 0; j < map[i][k].symbols.size(); j++)
+            {
+                cout<<map[i][k].symbols.at(j)<<" ";
+            }
+            cout<<endl<<"----------------------------"<<endl<<endl;
+        }
+    }
+}
+
+R5Map::~R5Map() { }