PositionControl.cpp

#include "PositionControl.h"

PositionControl::PositionControl(){

}

PositionControl::~PositionControl(){

}

void PositionControl::moveForward(){
	this->actionInProgress = true;

	this->speed = DEFAULT_DRIVE_SPEED;
	drive_rampStep(this->speed + this->speedCorrectionLeft, this->speed + this->speedCorrectionRight);

	sprint(loggerBuffer, "K9 moving forward(speed left: %f, speed right: %f)", this->speed + this->speedCorrectionLeft, this->speed + this->speedCorrectionRight);
	Logger::getInstance().log(loggerBuffer);
}

void PositionControl::moveBackward(){
	this->actionInProgress = true;

	this->speed = DEFAULT_DRIVE_SPEED;
	drive_rampStep((this->speed + this->speedCorrectionLeft) * (-1), (this->speed + this->speedCorrectionRight) * (-1));

	sprint(loggerBuffer, "K9 moving backwards(speed left: %f, speed right: %f)", (this->speed + this->speedCorrectionLeft) * (-1), (this->speed + this->speedCorrectionRight) * (-1));
	Logger::getInstance().log(loggerBuffer);
}

void PositionControl::stop(){
	// Start ramping to 0 speed
	this->speed = 0;
	drive_rampStep(this->speed, this->speed);

	static int prevTicksLeft = -1;
	static int prevTicksRight = -1;

	// Get current ticks
	int currentLeft;
	int currentRight;
	drive_getTicks(&currentLeft, &currentRight);

	// For initialisation of prevTicksLeft and prevTicksRight
	if(prevTicksLeft == -1){
		prevTicksLeft = currentLeft - 1; // - 1 for making sure the first pass in this function wont set the actionInProgress flag to false;
		prevTicksRight = currentRight - 1; // - 1 for making sure the first pass in this function wont set the actionInProgress flag to false;
	}

	// Log entry
	Logger::getInstance().log("K9 ramping down");

	// Set actionInProgress flag to false when completely stopped
	if(currentLeft - prevTicksLeft == 0 && currentRight - prevTicksRight == 0){
		this->actionInProgress = false;
		Logger::getInstance().log("K9 stopped");
	}

	// Set prev ticks current ticks
	prevTicksLeft = currentLeft;
	prevTicksRight = currentRight;
}

void PositionControl::turn(Direction direction){

}

void PositionControl::correct(float correctionLeft, float correctionRight){
	this->speedCorrectionLeft = correctionLeft;
	this->speedCorrectionRight = correctionRight;

	sprint(loggerBuffer, "Updating PositionControl correction(left: %f, right: %f)", correctionLeft, correctionRight);
	Logger::getInstance().log(loggerBuffer);
}

void PositionControl::correctAngle(int radials){

}

bool PositionControl::getMoveForwardFlag(){
	return this->moveForwardFlag;
}

bool PositionControl::getMoveBackwardFlag(){
	return this->moveBackwardFlag;
}

bool PositionControl::getStopFlag(){
	return this->stopFlag;
}

CoordSet PositionControl::getCurrentPosition(){
	return this->currentPosition;
}

void PositionControl::setCurrentPosition(CoordSet position){
	this->currentPosition = position;
}

CoordSet PositionControl::getPrevPosition(){
	return this->prevPosition;
}

void PositionControl::setPrevPosition(CoordSet position){
	this->prevPosition = position;
}

CoordSet PositionControl::getTargetPosition(){
	return this->targetPosition;
}

void PositionControl::setTargetPosition(CoordSet position){
	this->targetPosition = position;
}

Direction PositionControl::getCurrentOrientation(){
	return this->currentOrientation;
}

void PositionControl::setCurrentOrientation(Direction orientation){
	this->currentOrientation = orientation;
}

Direction PositionControl::getTargetOrientation(){
	return this->targetOrientation;
}

void PositionControl::setTargetOrientation(Direction orientation){
	this->targetOrientation = orientation;
}

int PositionControl::getSpeed(){
	return this->speed;
}

void PositionControl::setSpeed(int speed){
	this->speed = speed;
}

int PositionControl::getCurrentTicks(){
	return this->currentTicks;
}

void PositionControl::setCurrentTicks(int ticks){
	this->currentTicks = ticks;
}

int PositionControl::getPrevTicks(){

}

void PositionControl::setPrevTicks(int ticks){

}

int PositionControl::getTargetTicks(){
	return this->targetTicks;
}

void PositionControl::setTargetTicks(int ticks){
	this->targetTicks = ticks;
}

bool PositionControl::isAtTargetPosition(){
	if(currentPosition.x == targetPosition.x && currentPosition.y == targetPosition.y)
		return true;

	return false;
}

int PositionControl::calcSpeedFromTicks(){

}

void PositionControl::update(){
	static int prevTicksLeft;
	static int prevTicksRight;

	if(this->actionInProgress){
		// Get current ticks
		int currentLeft,
			currentRight;

		drive_getTicks(&currentLeft, &currentRight);

		// For initialisation of prevTicksLeft and prevTicksRight(static int is always initialized to 0 by compiler!)
		if(prevTicksLeft == 0 && prevTicksRight == 0){
			prevTicksLeft = currentLeft;
			prevTicksRight = currentRight;
		}

		////////////////////////////////////////////////////////
		// TODO: Implement code to take corrections into account
		////////////////////////////////////////////////////////
		/*
		//	-	Check if correction flag is true
		//	-	Calculate difference between left and right
		//	-	Calculate correction on local Y at the center of k9 based on difference between left and right. Use left as base for corection
		//	-	Calculate correction on local X at center of k9
		*/

		float	correctionX,
				correctionY;

		// Calulate ticks between previous and current ticks
		int diffLeft = currentLeft - prevTicksLeft;
		int diffRight = currentRight - prevTicksRight;

		//////////////////////////////////////////////////////////////////
		// TODO: Check if add/substaction of the corrections is done right
		//////////////////////////////////////////////////////////////////

		// Update current position based on the orientation
		switch(this->currentOrientation){
		case NORTH: this->currentPosition.x + correctionX;
					this->currentPosition.y + (this->actionTicksLeft * TRAVEL_DISTANCE_PER_TICK) + correctionY;
					break;
		case EAST:	this->currentPosition.x + (this->actionTicksLeft * TRAVEL_DISTANCE_PER_TICK) + correctionY;
					this->currentPosition.y + correctionX;
					break;
		case SOUTH:	this->currentPosition.x + correctionX;
					this->currentPosition.y - (this->actionTicksLeft * TRAVEL_DISTANCE_PER_TICK) + correctionY;
					break;
		case WEST:	this->currentPosition.x - (this->actionTicksLeft * TRAVEL_DISTANCE_PER_TICK) + correctionY;
					this->currentPosition.y + correctionX;
					break;
		}

		sprint(loggerBuffer, "Updating currentPosition(x: %f, y: %f)", this->currentPosition.x, this->currentPosition.y);
		Logger::getInstance().log(loggerBuffer);
	}
}