+++ Codework, Navigator&Positiontracker done (buggy?)

4 files changed, 123 insertions, 11 deletions

diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c
index a69c533..5dbf8bf 100755
--- a/source/Concept/Framework/modules/executor/navigator.c
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -54,6 +54,30 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float
 	}

 

 	CalculateEngines();

+

+	//dings,,,

+

+	/*float xDrive = cos(direction + PI / 6.0f);

+	float yDrive = sin(direction + PI / 6.0f);

+

+	float vLeft = xDrive;

+	float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;

+	float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;

+

+	vLeft = vLeft * this->robotSpeed + rotationSpeed;

+	vBack = vBack * this->robotSpeed + rotationSpeed;

+	vRight = vRight * this->robotSpeed + rotationSpeed;

+

+	Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);

+	curEngine->SetSpeed(vLeft);

+	curEngine->SetEnabled(true);

+	curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);

+	curEngine->SetSpeed(vBack);

+	curEngine->SetEnabled(true);

+	curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);

+	curEngine->SetSpeed(vRight);

+	curEngine->SetEnabled(true);

+	curEngine = NULL;*/

 }
 
 //-----------------------------------------------------------------------------

@@ -154,6 +178,8 @@ void Navigator::CalculateDirection()
 //-----------------------------------------------------------------------------

 void Navigator::CalculateEngines()

 {

+	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);

+	

 	if(direction != EMPTY_FLOAT)

 	{

 		float relativeDirection = this->direction - locationeer->GetOrientation();

@@ -165,15 +191,15 @@ void Navigator::CalculateEngines()
 		float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;

 		float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;

 

-		float speedCorrection = 1.0f;

+		/*float speedCorrection = 1.0f;

 

 		float maxEngineSpeed = 255.0f;

 		float minEngineSpeed = -255.0f;

 

 		float maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));

 

-		/*float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + 

-											vBack * (vBack + vLeft + vRight));*/

+		float calcSpeed = 1.0f;//sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + 

+		//									vBack * (vBack + vLeft + vRight));

 

 		if(calcSpeed != 1.0f)

 		{

@@ -184,14 +210,14 @@ void Navigator::CalculateEngines()
 		if(maxOverallSpeed > maxEngineSpeed)

 		{

 			robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection);

-		}

+		}*/

 		

 

-		vLeft = vLeft * this->robotSpeed * speedCorrection;

-		vBack = vBack * this->robotSpeed * speedCorrection;

-		vRight = vRight * this->robotSpeed * speedCorrection;

+		vLeft = vLeft * this->robotSpeed;// * speedCorrection;

+		vBack = vBack * this->robotSpeed;// * speedCorrection;

+		vRight = vRight * this->robotSpeed;// * speedCorrection;

 

-		maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));

+		/*maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));

 		float minSingleSpeed = min(min(vLeft, vBack), vRight);

 

 		if(rotationSpeed)

@@ -226,7 +252,7 @@ void Navigator::CalculateEngines()
 					vRight -= this->rotationSpeed;

 				}

 			}

-		}

+		}*/

 

 		Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);

 		curEngine->SetSpeed(vLeft);

diff --git a/source/Concept/Framework/modules/input/mouse_sensor.h b/source/Concept/Framework/modules/input/mouse_sensor.h
index 460e387..b3d1b1a 100755
--- a/source/Concept/Framework/modules/input/mouse_sensor.h
+++ b/source/Concept/Framework/modules/input/mouse_sensor.h
@@ -24,6 +24,8 @@ public:
 		this->configReset = 0;

 		this->configAwake = 0;

 		this->newImage = false;

+		this->positionX = 0;

+		this->positionY = 0;

 	}

 

 	Mouse_Sensor(uint32 sensorId)

@@ -47,6 +49,8 @@ public:
 				this->registerDeltaY = 0x02;

 				this->configReset = 0x80;

 				this->configAwake = 0x01;

+				this->positionX = -3.88f * TICKS_PER_CM;

+				this->positionY = -3.88f * TICKS_PER_CM;

 				break;

 			case IO_SENSOR_MOUSE_RIGHT:

 				this->hardwarePort = &PORTC;

@@ -61,6 +65,8 @@ public:
 				this->registerDeltaY = 0x02;

 				this->configReset = 0x80;

 				this->configAwake = 0x01;

+				this->positionX = -3.88f * TICKS_PER_CM;

+				this->positionY = 3.88f * TICKS_PER_CM;

 				break;

 			default:

 				this->hardwarePort = NULL;

@@ -101,8 +107,10 @@ protected:
 	uint8 registerDeltaY;

 	uint8 configReset;

 	uint8 configAwake;

+  //Information

+	float positionX;

+	float positionY;

 

-public:

 	void WriteByte(uint8 newByte)

 	{

 		*hardwareDDR |= pinSDA;//Set SDA output

@@ -180,6 +188,8 @@ public:
 		return pixel;

 	}

 

+

+public:	

 	uint8 GetSqual()

 	{

 		return Read(registerSqual);

@@ -194,6 +204,16 @@ public:
 	{

 		return (int8)(Read(registerDeltaY));

 	}

+

+	float GetPositionX()

+	{

+		return positionX;

+	}

+

+	float GetPositionY()

+	{

+		return positionY;

+	}

 };

 

 #endif
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c
index 262069a..c8cd52d 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.c
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.c
@@ -3,5 +3,65 @@
 //-----------------------------------------------------------------------------

 void Position_Tracker::Update()

 {

-	//insert code here

+	Mouse_Sensor* mouseLeft = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);

+	Mouse_Sensor* mouseRight = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);

+

+	int8 leftX = mouseLeft->GetXMovement();

+	int8 leftY = mouseLeft->GetYMovement();

+	float distanceLeft = sqrt(leftX * leftX + leftY * leftY);

+	float angleLeft = easyAngle(atan2(leftY, leftX) + (225.0f * PI / 180.0f));

+

+	float movementLeftX = cos(angleLeft) * distanceLeft;

+	float movementLeftY = sin(angleLeft) * distanceLeft;

+

+	if(!leftX && !leftY)

+	{

+		movementLeftX = 0;

+		movementLeftY = 0;

+	}

+

+	int8 rightX = mouseRight->GetXMovement();

+	int8 rightY = mouseRight->GetYMovement();

+	float distanceRight = sqrt(rightX * rightX + rightY * rightY);

+	float angleRight = easyAngle(atan2(rightY, rightX) - (45.0f * PI / 180.0f));

+

+	float movementRightX = cos(angleRight) * distanceRight;

+	float movementRightY = sin(angleRight) * distanceRight;

+

+	if(!rightX && !rightY)

+	{

+		movementRightX = 0;

+		movementRightY = 0;

+	}

+

+	float movementDifferenceX = movementRightX - movementLeftX;

+	float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY());

+

+	float robotMovementX = movementDifferenceX / 2.0f;

+	float robotMovementY = movementDifferenceY / 2.0f;

+	robotMovementX += movementLeftX;

+	robotMovementY += movementLeftY + mouseLeft->GetPositionY();

+	float robotDistance = sqrt(robotMovementX * robotMovementX + robotMovementY * robotMovementY);

+

+	float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f);

+

+	float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementX);

+	float absoluteDiffY	= sin(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementY);

+

+	if(!robotMovementX && !robotMovementY)

+	{

+		absoluteDiffX = 0;

+		absoluteDiffY = 0;

+	}

+

+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("            ", 5, 1);

+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffX, 5, 1);

+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffY, 12, 1);

+

+	this->positionX += absoluteDiffX;

+	this->positionY += absoluteDiffY;

+

+	this->orientation += orientationDiff;

+

+	this->orientation = easyAngle(this->orientation);

 }

diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h
index b31dd8f..e93ef0c 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.h
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.h
@@ -10,12 +10,18 @@ public:
 	{

 		this->parent = NULL;

 		this->moduleId = 0;

+		this->positionX = 0;

+		this->positionY = 0;

+		this->orientation = 0;

 	}

 

 	Position_Tracker(uint32 trackerId)

 	{

 		this->parent = NULL;

 		this->moduleId = trackerId;

+		this->positionX = 0;

+		this->positionY = 0;

+		this->orientation = 0;

 	}

 

 protected: