Logic.cpp

#include "Logic.h"

void findGoalRoutine(){
	Logger::getInstance().log("Entered findGoalRoutine()");

	while(1){
		// Update sensors
		float sensorLeftMeasurement = pingCm(PIN_PING_SENSOR_LEFT);
		float sensorRightMeasurement = pingCm(PIN_PING_SENSOR_RIGHT);
		float sensorFrontMeasurement = pingCm(PIN_PING_SENSOR_FRONT);

		k9->sensorLeft->update(sensorLeftMeasurement);
		k9->sensorRight->update(sensorRightMeasurement);
		k9->sensorFront->update(sensorFrontMeasurement);

		sprint(loggerBuffer, "Updating sensor measurements(left: %f, right: %f, front: %f)", sensorLeftMeasurement, sensorRightMeasurement, sensorFrontMeasurement);
		Logger::getInstance().log(loggerBuffer);

		// Update current position and correction
		k9->positionControl->update();

		float	correctionLeft,
				correctionRight;

		calcMovementCorrection(correctionLeft, correctionRight);
		k9->positionControl->correct(correctionLeft, correctionRight);
		

		// Keep going forward until close to a wall
		if(k9->sensorFront->getLastMeasurement() >= LOGIC_MAX_DISTANCE_TO_FRONT_WALL){
			k9->positionControl->moveForward();

			// Check left sensor for deviation 
			if(k9->sensorLeft->isLastMeasurementDeviating){
				if(k9->sensorHelperLeft == NULL)
					k9->sensorHelperLeft = new SensorHelper();

				// Add sensor deviation point
				k9->sensorHelperLeft->addSensorDeviationPoint(k9->sensorLeft->getWallPos(k9->positionControl->getCurrentPosition()), 
																k9->sensorLeft->getPrevMeasurement());

				// Analyse to see if the helper recognizes a maze scenario, delete helper when true
				if(k9->sensorHelperLeft->analyze()){
					delete k9->sensorHelperLeft;
					k9->sensorHelperLeft = NULL;
				}
			}

			// Check right sensor for deviation 
			if(k9->sensorRight->isLastMeasurementDeviating){
				if(k9->sensorHelperRight == NULL)
					k9->sensorHelperRight = new SensorHelper();

				// Add sensor deviation point
				k9->sensorHelperRight->addSensorDeviationPoint(k9->sensorRight->getWallPos(k9->positionControl->getCurrentPosition()), 
																k9->sensorRight->getPrevMeasurement());

				// Analyse to see if the helper recognizes a maze scenario, delete helper when true
				if(k9->sensorHelperRight->analyze()){
					delete k9->sensorHelperRight;
					k9->sensorHelperRight = NULL;
				}
			}


		}else{ // When there is a wall in front
			// Check if the same node doesn't exist yet
			// Make a new node
			// Node* newNode = new Node();
			// If there is a opening on the left and/or right
			// Add line(s) to node
			// Set position of node to current position
			// Set node to marked
			// Push node into graph

		}
	}
}

void calcMovementCorrection(float& correctionLeft, float& correctionRight){
	// Get measurement for calculation the correction
	float prevmeasurementLeft = k9->sensorLeft->getPrevMeasurement();
	float prevMeasurementRight = k9->sensorRight->getPrevMeasurement();
	float lastMeasurementLeft = k9->sensorLeft->getLastMeasurement();
	float lastMeasurementRight = k9->sensorRight->getLastMeasurement();

	float measurementDifference;

	// Get previous and current position
	CoordSet prevPosition = k9->positionControl->getPrevPosition();
	CoordSet lastPosition = k9->positionControl->getCurrentPosition();
	Direction orientation = k9->positionControl->getCurrentOrientation();

	float distanceTraveled;

	// Measurement operation flags
	bool	plusAxisX,
			minusAxisX,
			plusAxisY,
			minusAxisY;

	// Use left sensor unless the last measurement is deviation
	if(!k9->sensorLeft->isLastMeasurementDeviating()){
		// Get measurement operation
		SensorHelper::calcMeasurementOperationFromOrientations(k9->sensorLeft->getOrientation(), orientation, plusAxisX, minusAxisX, plusAxisY, minusAxisY);

		// Calculate travelled distance and the measurement distance(measurementDifference is positive when moving away from the wall and negative when moving towards it)
		if(plusAxisX){
			distanceTraveled = (lastPosition.y - prevPosition.y) * (-1);
			measurementDifference = lastMeasurementLeft - prevmeasurementLeft;
		}
		if(minusAxisX){
			distanceTraveled = lastPosition.y - prevPosition.y;
			measurementDifference = (lastMeasurementLeft - prevmeasurementLeft) * (-1);
		}
		if(plusAxisY){
			distanceTraveled = (lastPosition.x - prevPosition.x) * (-1);
			measurementDifference = lastMeasurementLeft - prevmeasurementLeft;
		}
		if(minusAxisY){
			distanceTraveled = lastPosition.x - prevPosition.x;
			measurementDifference = (lastMeasurementLeft - prevmeasurementLeft) * (-1);
		}
		
		// Return the correction based on wether k9 is moving towards or away from the wall
		correctionLeft = 0; // Always 0 because left wheel is used for calculating current position, so correction is only on the right wheel
		correctionRight = distanceTraveled * measurementDifference * 5; // Modifier for getting a realistic speedcorrection;
	}

	// Use right sensor unless the last measurement is deviation
	if(!k9->sensorRight->isLastMeasurementDeviating()){
		// Get measurement operation
		SensorHelper::calcMeasurementOperationFromOrientations(k9->sensorRight->getOrientation(), orientation, plusAxisX, minusAxisX, plusAxisY, minusAxisY);

		// Calculate travelled distance and the measurement distance(measurementDifference is positive when moving away from the wall and negative when moving towards it)
		if(plusAxisX){
			distanceTraveled = lastPosition.y - prevPosition.y;
			measurementDifference = lastMeasurementLeft - prevmeasurementLeft;
		}
		if(minusAxisX){
			distanceTraveled = (lastPosition.y - prevPosition.y) * (-1);
			measurementDifference = (lastMeasurementLeft - prevmeasurementLeft) * (-1);
		}
		if(plusAxisY){
			distanceTraveled = lastPosition.x - prevPosition.x;
			measurementDifference = lastMeasurementLeft - prevmeasurementLeft;
		}
		if(minusAxisY){
			distanceTraveled = (lastPosition.x - prevPosition.x) * (-1);
			measurementDifference = (lastMeasurementLeft - prevmeasurementLeft) * (-1);
		}
		
		// Return the correction based on wether k9 is moving towards or away from the wall
		correctionLeft = 0; // Always 0 because left wheel is used for calculating current position, so correction is only on the right wheel
		correctionRight = distanceTraveled * measurementDifference * 5; // Modifier for getting a realistic speedcorrection;
	}
}