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

source/Concept/Framework/defines.h

@@ -58,9 +58,10 @@
 //Constants
 #define SPEED_PER_PWM 1
 #define DISTANCE_PER_VALUE 1
+#define TICKS_PER_CM 205.0f
 
 #define PI 3.14159265358979323846f
-#define CYCLES_PER_CORRECTION 200
+#define CYCLES_PER_CORRECTION 20
 #define EMPTY_FLOAT 81188.1484f
 
 //IO Module Names

source/Concept/Framework/main.c

@@ -145,27 +145,29 @@ int main()
 	Keyboard* ourKeyboard = localRobot->GetModule<Keyboard>(IO_KEYBOARD_MAIN);
 	uint32 i = 1;
 	Navigator* ourNavigator = localRobot->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
+	Position_Tracker* ourPosition_Tracker = localRobot->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+	
 	float rotation = 0;
 
-	Mouse_Sensor* mouse_left = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
+	//Mouse_Sensor* mouse_left = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
-	Mouse_Sensor* mouse_right = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
+	//Mouse_Sensor* mouse_right = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
 
 
 	//Run
 	while(true)
 	{
-		/*ourDisplay->Print(i++,1,1);
+		ourDisplay->Print(i++,1,1);
-		ourDisplay->Print(mouse_left->GetXMovement(),1,2);
+				
-		ourDisplay->Print("        ");
+		if(!(i % 200))
-		ourDisplay->Print(mouse_left->GetYMovement(),10,2);
+		{
-		ourDisplay->Print("        ");
+			ourDisplay->Clear();
-		ourDisplay->Print(mouse_right->GetXMovement(),1,3);
+			ourDisplay->PrintFloat(ourPosition_Tracker->GetPositionX(),1,2);
-		ourDisplay->Print("        ");
+			ourDisplay->PrintFloat(ourPosition_Tracker->GetPositionY(),1,3);
-		ourDisplay->Print(mouse_right->GetYMovement(),10,3);
+			ourDisplay->PrintFloat(ourPosition_Tracker->GetOrientation() * 180.0f / PI,1,4);
-		ourDisplay->Print("        ");
+		}
-		ourDisplay->Print("                    ",1,4); // clear
+
 		
-		distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
+		/*distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
 		value = distanceSensor->GetDistance();
 		ourDisplay->Print(value, 1, 4);
 		ourDisplay->Print(";");
@@ -180,47 +182,41 @@ int main()
 		distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_270_DEG);
 		value = distanceSensor->GetDistance();
 		ourDisplay->Print(value);
-		ourDisplay->Print(";");		
+		ourDisplay->Print(";");		*/
-
+		
-
-		msleep(500);*/
-
 		uint8 someInput = ourKeyboard->GetInput();
-		//ourDisplay->Clear();
-		ourDisplay->Print(i++,1,1);
 		//ourDisplay->Print("Ready to accept...", 1, 2);
-		ourDisplay->Print(someInput, 1, 3);
-		ourDisplay->PrintFloat(rotation, 1, 4);
 		switch(someInput)
 		{
 			case 0:
 				ourNavigator->Stop();
+				break;
 			case 1:
-				ourNavigator->Drive(225, rotation, 200, rotation);
+				ourNavigator->Drive(225, rotation, 100, rotation);
 				break;
 			case 2:
-				ourNavigator->Drive(180, rotation, 200, rotation);
+				ourNavigator->Drive(180, rotation, 100, rotation);
 				break;
 			case 3:
-				ourNavigator->Drive(135, rotation, 200, rotation);
+				ourNavigator->Drive(135, rotation, 100, rotation);
 				break;
 			case 4:
-				ourNavigator->Drive(270, rotation, 200, rotation);
+				ourNavigator->Drive(270, rotation, 100, rotation);
 				break;
 			case 5:
 				ourNavigator->Drive(0, rotation, 0, rotation);
 				break;
 			case 6:
-				ourNavigator->Drive(90, rotation, 200, rotation);
+				ourNavigator->Drive(90, rotation, 100, rotation);
 				break;
 			case 7:
-				ourNavigator->Drive(315, rotation, 200, rotation);
+				ourNavigator->Drive(315, rotation, 100, rotation);
 				break;
 			case 8:
-				ourNavigator->Drive(0, rotation, 200, rotation);
+				ourNavigator->Drive(0, rotation, 100, rotation);
 				break;
 			case 9:
-				ourNavigator->Drive(45, rotation, 200, rotation);
+				ourNavigator->Drive(45, rotation, 100, rotation);
 				break;
 			case 10:
 				rotation += 10;
@@ -230,8 +226,6 @@ int main()
 				break;
 		}
 		
-		msleep(50);
-		
 		/*
 		//ourDisplay->Clear();
 		ourDisplay->Print(i++,1,1);

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 + 
+		float calcSpeed = 1.0f;//sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + 
-											vBack * (vBack + vLeft + 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;
+		vLeft = vLeft * this->robotSpeed;// * speedCorrection;
-		vBack = vBack * this->robotSpeed * speedCorrection;
+		vBack = vBack * this->robotSpeed;// * speedCorrection;
-		vRight = vRight * 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);

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

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);
 }

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:

source/Concept/Framework/tools.h

@@ -14,6 +14,21 @@
 #define min(a, b) (((a) < (b)) ? (a) : (b))
 #define sign(a) (((a) < 0) ? (-1) : (1))
 
+inline float easyAngle(float angle)
+{
+	while(angle < 0)
+	{
+		angle += 2.0f * PI;
+	}
+
+	while(angle >= 2.0f * PI)
+	{
+		angle -= 2.0f * PI;
+	}
+
+	return angle;
+}
+
 inline void sleep(uint16 sec)
 {
   for (uint16 s = 0; s < sec; s++) {
@@ -48,7 +63,7 @@ inline float distance2d(float x1, float y1, float x2, float y2)
 
 inline float direction2d(float x1, float y1, float x2, float y2)
 {
-	return atan((y2 - y1) / (x2 - x1));
+	return atan2((y2 - y1), (x2 - x1));
 }
 
 #endif