void TimerFunction(int value)
{
updateCoordinate(&x5,&y5,&x5step,&y5step);
updateCoordinate(&x2,&y2,&x2step,&y2step);
updateCoordinate(&x3,&y3,&x3step,&y3step);
updateCoordinate(&x4,&y4,&x4step,&y4step);
glutPostRedisplay();
glutTimerFunc(33,TimerFunction,1);
}
// Function is called by Owen to return next rover direction
char approachIntersection(char left, char right, char forward, int Distance) {
static int Total_Points = 1;
static int intersection = 0;
static int point = 1;
static int xcoor = 10;
xcoor = updateXCOOR(0, false);
static int ycoor = 10;
ycoor = updateYCOOR(0, false);
int Coordinate = updateCoordinate(xcoor, ycoor, point, false);
static int getXCoor[50];
static int getYCoor[50];
static int pointArray[50];
static int typeArray[50];
static int exploredArray[50];
static bool initialize = true;
while (initialize == true) {
exploredArray[0] = 1;
getXCoor[0] = xcoor;
getYCoor[0] = ycoor;
initialize = false;
}
intersection++;
typeArray[intersection] = getType(left, right, forward);
if (Coordinate > 0) {
// Previous node identified
pointArray[intersection] = Coordinate;
exploredArray[Coordinate]++;
exploredArray[intersection] = exploredArray[Coordinate] + 1;
return old_DetermineDirection(left, right, forward, xcoor, ycoor, intersection, Distance, exploredArray, typeArray, pointArray, point, getXCoor, getYCoor);
}
else {
// New node identified
pointArray[intersection] = Total_Points;
exploredArray[intersection]++;
Total_Points++;
// Save Coordinate
updateCoordinate(xcoor, ycoor, point, true);
//Coordinate[xcoor][ycoor] = point;
getXCoor[point] = xcoor;
getYCoor[point] = ycoor;
point++;
//Determine next direction
return new_DetermineDirection(left, right, forward, xcoor, ycoor, Distance);
}
}
Color OtfTransmittanceMapOccluder::sample(const OcclusionSampleState &state) const
{
// Error checking
if (!m_camera) {
throw MissingCameraException("");
}
// Transform to camera space for depth and raster space for pixel coordinate
Vector csP = m_camera->worldToCamera(state.wsP, state.rayState.time);
Vector rsP = m_camera->worldToRaster(state.wsP, state.rayState.time);
// Bounds checks
if (m_clipBehindCamera && csP.z < 0.0) {
return Colors::one();
}
if (rsP.x < 0.0 || rsP.x >= m_floatRasterBounds.x ||
rsP.y < 0.0 || rsP.y >= m_floatRasterBounds.y) {
return Colors::one();
}
// Compute depth to sample at
Vector wsCamPosition = m_camera->position(state.rayState.time);
float depth = (state.wsP - wsCamPosition).length();
// Ensure all samples are available
updateCoordinate(rsP);
// Finally interpolate
return m_transmittanceMap.lerp(rsP.x, rsP.y, depth);
}
// This is the function that makes the algorith work!!!
// Determines the direction to take after coming to an intersection
// North=2, South=4, East=1, West=3
char old_DetermineDirection(char left, char right, char forward, int xcoor, int ycoor, int intersection, int Distance, int exploredArray[], int typeArray[], int pointArray[], int point, int getXCoor[], int getYCoor[]) {
int Direction = updateDirection(0, false);
bool directionNew = true;
int compareType = 0;
int Coordinate = updateCoordinate(xcoor, ycoor, point, false);
if (Direction == 1) {
if (forward) {
directionNew = true;
int i = 0;
while (i < intersection) {
if (xcoor < getXCoor[i] && ycoor == getYCoor[i]) {
if (exploredArray[i] >= typeArray[i] && Coordinate != 0) {
directionNew = false;
}
}
i++;
}
if (directionNew == true) {
Direction = updateDirection(1, true);
updateXCOOR(Distance, true);
return directionToRover(Direction);
}
else if (directionNew == false) {
compareType++;
}
}
if (left) {
directionNew = true;
int i = 0;
while (i < intersection) {
if (ycoor > getYCoor[i] && xcoor == getXCoor[i]) {
if (exploredArray[i] >= typeArray[i] && Coordinate != 0) {
directionNew = false;
}
}
i++;
}
if (directionNew == true) {
Direction = updateDirection(2, true);
updateYCOOR((-1 * Distance), true);
return directionToRover(Direction);
}
else if (directionNew == false) {
compareType++;
}
}
if (right) {
directionNew = true;
int i = 0;
while (i < intersection) {
if (ycoor < getYCoor[i] && xcoor == getXCoor[i]) {
if (exploredArray[i] >= typeArray[i] && Coordinate != 0) {
directionNew = false;
}
}
i++;
}
if (directionNew == true) {
Direction = updateDirection(4, true);
updateYCOOR(Distance, true);
return directionToRover(Direction);
}
else if (directionNew == false) {
compareType++;
}
}
if (compareType == getType(left, right, forward)) {
return traversedAdjacentPath(left, right, forward, xcoor, ycoor, intersection, Distance, exploredArray, typeArray, pointArray, point, getXCoor, getYCoor);
}
}
else if (Direction == 2) {
if (right) {
directionNew = true;
int i = 0;
while (i < intersection) {
if (xcoor < getXCoor[i] && ycoor == getYCoor[i]) {
if (exploredArray[i] >= typeArray[i] && Coordinate != 0) {
directionNew = false;
}
}
i++;
}
if (directionNew == true) {
Direction = updateDirection(1, true);
updateXCOOR(Distance, true);
return directionToRover(Direction);
}
else if (directionNew == false) {
compareType++;
}
}
if (forward) {
directionNew = true;
int i = 0;
while (i < intersection) {
if (ycoor > getYCoor[i] && xcoor == getXCoor[i]) {
if (exploredArray[i] >= typeArray[i] && Coordinate != 0) {
directionNew = false;
}
}
i++;
}
if (directionNew == true) {
Direction = updateDirection(2, true);
updateYCOOR((-1 * Distance), true);
return directionToRover(Direction);
}
else if (directionNew == false) {
compareType++;
}
}
if (left) {
directionNew = true;
int i = 0;
while (i < intersection) {
if (xcoor > getXCoor[i] && ycoor == getYCoor[i]) {
if (exploredArray[i] >= typeArray[i] && Coordinate != 0) {
directionNew = false;
}
}
i++;
}
if (directionNew == true) {
Direction = updateDirection(3, true);
updateXCOOR((-1 * Distance), true);
return directionToRover(Direction);
}
else if (directionNew == false) {
compareType++;
}
}
if (compareType == getType(left, right, forward)) {
return traversedAdjacentPath(left, right, forward, xcoor, ycoor, intersection, Distance, exploredArray, typeArray, pointArray, point, getXCoor, getYCoor);
}
}
else if (Direction == 3) {
if (right) {
directionNew = true;
int i = 0;
while (i < intersection) {
if (ycoor > getYCoor[i] && xcoor == getXCoor[i]) {
if (exploredArray[i] > 0) {
directionNew = false;
}
}
i++;
}
if (directionNew == true) {
Direction = updateDirection(2, true);
updateYCOOR((-1 * Distance), true);
return directionToRover(Direction);
}
else if (directionNew == false) {
compareType++;
}
}
if (forward) {
directionNew = true;
int i = 0;
while (i < intersection) {
if (xcoor > getXCoor[i] && ycoor == getYCoor[i]) {
if (exploredArray[i] > 0 && typeArray[i] != 0) {
directionNew = false;
}
}
i++;
}
if (directionNew == true) {
Direction = updateDirection(3, true);
updateXCOOR((-1 * Distance), true);
return directionToRover(Direction);
}
else if (directionNew == false) {
compareType++;
}
}
if (left) {
directionNew = true;
int i = 0;
while (i < intersection) {
if (ycoor < getYCoor[i] && xcoor == getXCoor[i]) {
if (exploredArray[i] > 0) {
directionNew = false;
}
}
i++;
}
if (directionNew == true) {
Direction = updateDirection(4, true);
updateYCOOR(Distance, true);
return directionToRover(Direction);
}
else if (directionNew == false) {
compareType++;
}
}
if (compareType == getType(left, right, forward)) {
return traversedAdjacentPath(left, right, forward, xcoor, ycoor, intersection, Distance, exploredArray, typeArray, pointArray, point, getXCoor, getYCoor);
}
}
else if (Direction == 4) {
if (left) {
directionNew = true;
int i = 0;
while (i < intersection) {
if (xcoor < getXCoor[i] && ycoor == getYCoor[i]) {
if (exploredArray[i] > 0) {
directionNew = false;
}
}
i++;
}
if (directionNew == true) {
Direction = updateDirection(1, true);
updateXCOOR(Distance, true);
return directionToRover(Direction);
}
else if (directionNew == false) {
compareType++;
}
}
if (right) {
directionNew = true;
int i = 0;
while (i < intersection) {
if (xcoor > getXCoor[i] && ycoor == getYCoor[i]) {
if (exploredArray[i] > 0) {
directionNew = false;
}
}
i++;
}
if (directionNew == true) {
Direction = updateDirection(3, true);
updateXCOOR((-1 * Distance), true);
return directionToRover(Direction);
}
else if (directionNew == false) {
compareType++;
}
}
if (forward) {
directionNew = true;
int i = 0;
while (i < intersection) {
if (ycoor < getYCoor[i] && xcoor == getXCoor[i]) {
if (exploredArray[i] > 0) {
directionNew = false;
}
}
i++;
}
if (directionNew == true) {
Direction = updateDirection(4, true);
updateYCOOR(Distance, true);
return directionToRover(Direction);
}
else if (directionNew == false) {
compareType++;
}
}
if (compareType == getType(left, right, forward)) {
return traversedAdjacentPath(left, right, forward, xcoor, ycoor, intersection, Distance, exploredArray, typeArray, pointArray, point, getXCoor, getYCoor);
}
}
}
// Determines what to do if all adjacent paths have already been traversed
char traversedAdjacentPath(char left, char right, char forward, int xcoor, int ycoor, int intersection, int Distance, int exploredArray[], int typeArray[], int pointArray[], int point, int getXCoor[], int getYCoor[]) {
int stop = 1;
int Direction = updateDirection(0, false);
int Coordinate = updateCoordinate(xcoor, ycoor, point, false);
static bool endMap = false;
int isDeadEnd = getType(left, right, forward);
if (isDeadEnd == 1) {
return new_DetermineDirection(left, right, forward, xcoor, ycoor, Distance);
}
else {
int i = 0;
while (i < intersection) {
stop = 1;
if (pointArray[i] == Coordinate) {
if (exploredArray[i - 1] < typeArray[i - 1]) {
if (ycoor == getYCoor[pointArray[i - 1]]) {
if (xcoor > getXCoor[pointArray[i - 1]]) {
Direction = updateDirection(3, true);
updateXCOOR((-1 * Distance), true);
}
else if (xcoor < getXCoor[pointArray[i - 1]]) {
Direction = updateDirection(1, true);
updateXCOOR(Distance, true);
}
}
else if (xcoor == getXCoor[pointArray[i - 1]]) {
if (ycoor > getYCoor[pointArray[i - 1]]) {
Direction = updateDirection(2, true);
updateYCOOR((-1 * Distance), true);
}
else if (ycoor < getYCoor[pointArray[i - 1]]) {
Direction = updateDirection(4, true);
updateYCOOR(Distance, true);
}
}
stop--;
}
else if (exploredArray[i + 1] < typeArray[i + 1]) {
if (ycoor == getYCoor[pointArray[i + 1]]) {
if (xcoor > getXCoor[pointArray[i + 1]]) {
Direction = updateDirection(3, true);
updateXCOOR((-1 * Distance), true);
}
else if (xcoor < getXCoor[pointArray[i + 1]]) {
Direction = updateDirection(1, true);
updateXCOOR(Distance, true);
}
}
else if (xcoor == getXCoor[pointArray[i + 1]]) {
if (ycoor > getYCoor[pointArray[i + 1]]) {
Direction = updateDirection(2, true);
updateYCOOR((-1 * Distance), true);
}
else if (ycoor < getYCoor[pointArray[i + 1]]) {
Direction = updateDirection(4, true);
updateYCOOR(Distance, true);
}
}
stop--;
}
else if ((exploredArray[i - 1] >= typeArray[i - 1]) && (exploredArray[i + 1] >= typeArray[i + 1])) {
stop++;
}
}
i++;
}
}
if (stop == 2) {
// DONE MAPPING!!!!
return 'E';
}
else {
return directionToRover(Direction);
}
}
static int gConfigure(int gFile)
{
CHECK(ITG3200_Init(gFile, ITG3200_ID, true));
struct ITG3200_Acquisition confAcquisition = {
.lowPassFilter = ITG3200_LOWPASSFILTER_42,
.sampleRateDivider = 0,
};
CHECK(ITG3200_ConfigureAcquisition(gFile, &confAcquisition));
struct ITG3200_Power confPower = {
.clockSource = ITG3200_CLOCKSOURCE_PLL_X,
};
CHECK(ITG3200_ConfigurePower(gFile, &confPower));
return 0;
}
int main(void)
{
int adapter;
char filename[20];
for (adapter = 0; adapter < MAX_ADAPTERS; ++adapter)
{
snprintf(filename, 20, "/dev/i2c-%d", adapter);
if (!access(filename, R_OK | W_OK))
break;
}
if (adapter == MAX_ADAPTERS)
{
fprintf(stderr, "No I2C adapter found\n");
return -1;
}
aFile = open(filename, O_RDWR);
if (aConfigure(aFile))
{
fprintf(stderr, "Failed to initialize accelerometer\n");
return closeAndExit(-1);
}
cFile = open(filename, O_RDWR);
if (cConfigure(cFile))
{
fprintf(stderr, "Failed to initialize compass\n");
return closeAndExit(-1);
}
gFile = open(filename, O_RDWR);
if (gConfigure(gFile))
{
fprintf(stderr, "Failed to initialize gyroscope\n");
return closeAndExit(-1);
}
ADXL345_ReadData(aFile, &aX.data, &aY.data, &aZ.data);
HMC5883L_ReadData(cFile, &cX.data, &cY.data, &cZ.data);
ITG3200_ReadData(gFile, &gX.data, &gY.data, &gZ.data);
ITG3200_ReadTemperature(gFile, &gT.data);
initCoordinate(&aX);
initCoordinate(&aY);
initCoordinate(&aZ);
initCoordinate(&cX);
initCoordinate(&cY);
initCoordinate(&cZ);
initCoordinate(&gX);
initCoordinate(&gY);
initCoordinate(&gZ);
initCoordinate(&gT);
setupHandlers();
initscr();
noecho();
curs_set(false);
running = 1;
while (running)
{
ADXL345_ReadData(aFile, &aX.data, &aY.data, &aZ.data);
HMC5883L_ReadData(cFile, &cX.data, &cY.data, &cZ.data);
ITG3200_ReadData(gFile, &gX.data, &gY.data, &gZ.data);
ITG3200_ReadTemperature(gFile, &gT.data);
updateCoordinate(&aX);
updateCoordinate(&aY);
updateCoordinate(&aZ);
updateCoordinate(&cX);
updateCoordinate(&cY);
updateCoordinate(&cZ);
updateCoordinate(&gX);
updateCoordinate(&gY);
updateCoordinate(&gZ);
updateCoordinate(&gT);
clear();
mvprintw(0, 0, "%16s: %8d X %8d Y %8d Z\n", "Accelerometer", aX.data, aY.data, aZ.data);
mvprintw(1, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Smoothed", aX.smoothed, aY.smoothed, aZ.smoothed);
mvprintw(2, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Min", aX.min, aY.min, aZ.min);
mvprintw(3, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Max", aX.max, aY.max, aZ.max);
mvprintw(4, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Normalized", aX.normalized, aY.normalized, aZ.normalized);
mvprintw(6, 0, "%16s: %8d X %8d Y %8d Z\n", "Compass", cX.data, cY.data, cZ.data);
mvprintw(7, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Smoothed", cX.smoothed, cY.smoothed, cZ.smoothed);
mvprintw(8, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Min", cX.min, cY.min, cZ.min);
mvprintw(9, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Max", cX.max, cY.max, cZ.max);
mvprintw(10, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Normalized", cX.normalized, cY.normalized, cZ.normalized);
mvprintw(12, 0, "%16s: %8d X %8d Y %8d Z\n", "Gyroscope", gX.data, gY.data, gZ.data);
mvprintw(13, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Smoothed", gX.smoothed, gY.smoothed, gZ.smoothed);
mvprintw(14, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Min", gX.min, gY.min, gZ.min);
mvprintw(15, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Max", gX.max, gY.max, gZ.max);
mvprintw(16, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Normalized", gX.normalized, gY.normalized, gZ.normalized);
mvprintw(18, 0, "%16s: %8d %8.1f °C\n", "Temperature", gT.data, ITG3200_ConvertTemperature(gT.data));
mvprintw(19, 0, "%16s: %8.1f %8.1f °C\n", "Smoothed", gT.smoothed, ITG3200_ConvertTemperature(gT.smoothed));
mvprintw(20, 0, "%16s: %8.1f %8.1f °C\n", "Min", gT.min, ITG3200_ConvertTemperature(gT.min));
mvprintw(21, 0, "%16s: %8.1f %8.1f °C\n", "Max", gT.max, ITG3200_ConvertTemperature(gT.max));
mvprintw(22, 0, "%16s: %8.1f\n", "Normalized", gT.normalized);
refresh();
usleep(DELAY);
}
endwin();
close(aFile);
close(cFile);
close(gFile);
return 0;
}
