Code-stuff

6 files changed, 362 insertions, 19 deletions

diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c
index ceeda60..47b1c69 100755
--- a/source/Concept/Framework/modules/executor/navigator.c
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -1,3 +1,138 @@
+<<<<<<< .mine
+#include "navigator.h"
+
+//-----------------------------------------------------------------------------
+void Navigator::Stop()
+{	
+        this->direction = 0;
+        this->targetAngle = 0;
+        //this->targetX = EMPTY_FLOAT;
+        //this->targetY = EMPTY_FLOAT;
+        this->robotSpeed = 0;
+        this->rotationSpeed = 0;
+
+        /*for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
+        {
+                (parent->GetModule<Engine>(i))->SetSpeed(0);
+                (parent->GetModule<Engine>(i))->SetEnabled(true);
+        }*/
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::Rotate(float newTargetAngle,float newRotationSpeed)
+{
+        this->rotationSpeed = min(newRotationSpeed, 255.0f);;
+        this->direction = 0;
+        this->targetAngle = newTargetAngle;
+        //this->targetX = EMPTY_FLOAT;
+        //this->targetY = EMPTY_FLOAT;
+        this->robotSpeed = 0;
+
+        /*for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
+        {
+                (parent->GetModule<Engine>(i))->SetSpeed(rotationSpeed);
+                (parent->GetModule<Engine>(i))->SetEnabled(true);
+        }*/
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed)
+{
+        this->rotationSpeed = min(rotationSpeed, 255.0f);
+        this->direction = newDirection*PI/180;
+        this->targetAngle = newAngle;
+        this->targetX = EMPTY_FLOAT;
+        this->targetY = EMPTY_FLOAT;
+        this->robotSpeed = newSpeed;
+
+        if(targetAngle == EMPTY_FLOAT)
+        {
+                rotationSpeed = 0;
+        }
+        else
+        {
+                rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - (parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN))->GetOrientation()));
+        }
+
+        CalculateEngines();
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed)
+{
+        if(newX < 0 || newY < 0) return;
+
+        Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+
+        this->rotationSpeed = min(rotationSpeed, 255.0f);
+        this->targetX = newX;
+        this->targetY = newY;
+        this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);;
+        this->targetAngle = newAngle;
+        this->robotSpeed = newSpeed;
+
+        if(targetAngle - locationeer->GetOrientation() > PI)
+        {
+                if(rotationSpeed > 0)
+                {
+                        rotationSpeed = -rotationSpeed;
+                }
+        }
+        else
+        {
+                if(rotationSpeed < 0)
+                {
+                        rotationSpeed = -rotationSpeed;
+                }
+        }
+
+        CalculateDirection();
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::Update()
+{
+        Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+
+        bool targetReached = false;
+        bool targetAngleReached = false;
+
+        if(HasTarget() && distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
+        {
+                targetX = EMPTY_FLOAT;
+                targetY = EMPTY_FLOAT;
+                direction = EMPTY_FLOAT;
+                robotSpeed = 0;
+
+                targetReached = true;
+        }
+
+        if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
+        {
+                targetAngle = EMPTY_FLOAT;
+                rotationSpeed = 0;
+
+                targetAngleReached = true;
+        }
+
+        if(targetReached && targetAngleReached)
+        {
+                Stop();
+        }
+        else if(targetReached || targetAngleReached)
+        {
+                CalculateDirection();
+        }
+
+        if(!(correctionCountdown--))
+        {
+                CalculateDirection();
+        }
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::CalculateDirection()
+=======
 #include "navigator.h"

 

 //-----------------------------------------------------------------------------

@@ -19,7 +154,135 @@ void Navigator::Stop()
 

 //-----------------------------------------------------------------------------

 void Navigator::Rotate(float rotationSpeed)

+>>>>>>> .r199
 {

+<<<<<<< .mine
+				Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+        correctionCountdown = CYCLES_PER_CORRECTION;
+
+        if(targetAngle == EMPTY_FLOAT && !HasTarget()) return;
+
+        if(HasTarget())
+        {
+                direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);
+        }
+
+        if(targetAngle != EMPTY_FLOAT)
+        {
+                rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - locationeer->GetOrientation()));
+        }
+
+        CalculateEngines();
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::CalculateEngines()
+{
+        Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+        if(direction != EMPTY_FLOAT)
+        {
+                float relativeDirection = this->direction - locationeer->GetOrientation();
+
+                float xDrive = cos(relativeDirection + PI / 6.0f);
+                float yDrive = sin(relativeDirection + PI / 6.0f);
+
+                float vLeft = xDrive;
+                float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
+                float vRight = (-xDrive - sqrt(3) * yDrive) / 2.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));
+
+                if(calcSpeed != 1.0f)
+                {
+                        speedCorrection = 1.0f / calcSpeed;
+                }
+
+                float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection;
+                if(maxOverallSpeed > maxEngineSpeed)
+                {
+                        robotSpeed = maxEngineSpeed / (maxSingleSpeed * 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));
+                float minSingleSpeed = min(min(vLeft, vBack), vRight);
+
+                if(rotationSpeed)
+                {
+                        if(this->rotationSpeed > 0)
+                        {
+                                if(maxEngineSpeed - maxSingleSpeed < this->rotationSpeed)
+                                {
+                                        vLeft += maxEngineSpeed - maxSingleSpeed;
+                                        vBack += maxEngineSpeed - maxSingleSpeed;
+                                        vRight += maxEngineSpeed - maxSingleSpeed;
+                                }
+                                else
+                                {
+                                        vLeft += this->rotationSpeed;
+                                        vBack += this->rotationSpeed;
+                                        vRight += this->rotationSpeed;
+                                }
+                        }
+                        else
+                        {
+                                if((minEngineSpeed - minSingleSpeed) < this->rotationSpeed)
+                                {
+                                        vLeft -= minEngineSpeed - minSingleSpeed;
+                                        vBack -= minEngineSpeed - minSingleSpeed;
+                                        vRight -= minEngineSpeed - minSingleSpeed;
+                                }
+                                else
+                                {
+                                        vLeft -= this->rotationSpeed;
+                                        vBack -= this->rotationSpeed;
+                                        vRight -= this->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;
+        }
+        else if(rotationSpeed)
+        {
+                Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
+                curEngine->SetSpeed(this->rotationSpeed);
+                curEngine->SetEnabled(true);
+                curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
+                curEngine->SetSpeed(this->rotationSpeed);
+                curEngine->SetEnabled(true);
+                curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
+                curEngine->SetSpeed(this->rotationSpeed);
+                curEngine->SetEnabled(true);
+                curEngine = NULL;
+        }	
+        else
+        {
+                Stop();
+        }
+}
+=======
 	this->rotationSpeed = min(rotationSpeed, 255.0f);;

 	this->direction = EMPTY_FLOAT;

 	this->targetAngle = EMPTY_FLOAT;

@@ -257,3 +520,4 @@ void Navigator::CalculateEngines()
 		Stop();

 	}

 }

+>>>>>>> .r199
diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h
index 56e7b8f..848c9fa 100755
--- a/source/Concept/Framework/modules/executor/navigator.h
+++ b/source/Concept/Framework/modules/executor/navigator.h
@@ -1,3 +1,75 @@
+<<<<<<< .mine
+#ifndef _NAVIGATOR_H
+#define _NAVIGATOR_H
+
+//#include <math.h>
+#include "../../stdafx.h"
+
+class Navigator : public IO_Module
+{
+public:
+        Navigator()
+        {
+                this->parent = NULL;
+                this->moduleId = 0;
+                this->correctionCountdown = CYCLES_PER_CORRECTION;
+                this->direction = EMPTY_FLOAT;
+                this->targetAngle = EMPTY_FLOAT;
+                this->targetX = EMPTY_FLOAT;
+                this->targetY = EMPTY_FLOAT;
+                this->robotSpeed = 0;
+                this->rotationSpeed = 0;
+        }
+
+        Navigator(uint32 navigatorId)
+        {
+                this->parent = NULL;
+                this->moduleId = navigatorId;
+                this->correctionCountdown = CYCLES_PER_CORRECTION;
+                this->direction = EMPTY_FLOAT;
+                this->targetAngle = EMPTY_FLOAT;
+                this->targetX = EMPTY_FLOAT;
+                this->targetY = EMPTY_FLOAT;
+                this->robotSpeed = 0;
+                this->rotationSpeed = 0;
+        }
+
+protected:
+        uint16 correctionCountdown;
+        float direction; 
+        float targetAngle;
+        float targetX;
+        float targetY;
+        float robotSpeed;
+        float rotationSpeed;
+
+public:
+        void Update();	
+
+        void Stop();
+
+        void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed);
+
+        void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed);
+
+        void Rotate(float rotationSpeed);
+
+        void SetSpeed(float newSpeed)
+        {
+                this->robotSpeed = newSpeed;
+        }
+
+        void CalculateDirection();
+
+        void CalculateEngines();
+
+        bool HasTarget()
+        {
+                return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);
+        }
+};
+
+=======
 #ifndef _NAVIGATOR_H

 #define _NAVIGATOR_H

 

@@ -68,4 +140,5 @@ public:
 	}

 };

 

+>>>>>>> .r199
 #endif
diff --git a/source/Concept/Framework/modules/input/distance_sensor.c b/source/Concept/Framework/modules/input/distance_sensor.c
index 977784f..508eeed 100755
--- a/source/Concept/Framework/modules/input/distance_sensor.c
+++ b/source/Concept/Framework/modules/input/distance_sensor.c
@@ -9,24 +9,24 @@ float Distance_Sensor::GetDistance()
 	*hardwareDDR |= pin;//Set pin output

 	*hardwarePort |= pin;//Activate port

 	usleep(10);//Wait for 10µs

-	//*hardwarePort &= ~pin;//Deactivate port

+	*hardwarePort &= ~pin;//Deactivate port

 	*hardwareDDR &= ~pin;//Set pin input

 

-	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1);

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

 

 	//Wait for response

-	while(!(PINC & pin)){asm volatile("nop");}

+	for(int i=0;(!(PINC & pin))&&(i < 1000);i++) {asm volatile("nop");}

 

-	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1);

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

 	

 	//Calculate duration of response

-	while(*hardwarePin & pin) 

+	while((*hardwarePin & pin)&&(result < 300000)) 

 	{

 		result++;

 		asm volatile("nop");

 	}

 

-	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1);

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

 

 	return (float(result) * DISTANCE_PER_VALUE);

 }

diff --git a/source/Concept/Framework/modules/input/mouse_sensor.h b/source/Concept/Framework/modules/input/mouse_sensor.h
index 30f9d53..460e387 100755
--- a/source/Concept/Framework/modules/input/mouse_sensor.h
+++ b/source/Concept/Framework/modules/input/mouse_sensor.h
@@ -42,9 +42,9 @@ public:
 				this->pinSCK = (1 << 6);

 				this->registerConfig = 0x00;

 				this->registerPixelData = 0x08;

-				this->registerSqual = 0x4;

-				this->registerDeltaX = 0x3;

-				this->registerDeltaY = 0x2;

+				this->registerSqual = 0x04;

+				this->registerDeltaX = 0x03;

+				this->registerDeltaY = 0x02;

 				this->configReset = 0x80;

 				this->configAwake = 0x01;

 				break;

@@ -52,13 +52,13 @@ public:
 				this->hardwarePort = &PORTC;

 				this->hardwareDDR = &DDRC;

 				this->hardwarePin = &PINC;

-				this->pinSDA = (1 << 5);

-				this->pinSCK = (1 << 7);

+				this->pinSDA = (1 << 7);

+				this->pinSCK = (1 << 5);

 				this->registerConfig = 0x00;

 				this->registerPixelData = 0x08;

-				this->registerSqual = 0x4;

-				this->registerDeltaX = 0x3;

-				this->registerDeltaY = 0x2;

+				this->registerSqual = 0x04;

+				this->registerDeltaX = 0x03;

+				this->registerDeltaY = 0x02;

 				this->configReset = 0x80;

 				this->configAwake = 0x01;

 				break;

diff --git a/source/Concept/Framework/modules/output/display.h b/source/Concept/Framework/modules/output/display.h
index e221120..bbfa292 100755
--- a/source/Concept/Framework/modules/output/display.h
+++ b/source/Concept/Framework/modules/output/display.h
@@ -39,7 +39,7 @@ public:
 				this->settingIllumination = 76;

 				this->settingCursorPosition = 79;

 				msleep(100);

-				uart1_init(103);//9600 BAUD at 16MHz Atmel

+				uart1_init(51);//19200 BAUD at 16MHz Atmel

 				msleep(100);

 				break;

 			default:

@@ -52,6 +52,8 @@ public:
 				this->settingCursorPosition = 0;

 				break;

 		}

+

+		Clear();

 	}

 

 protected:

@@ -137,6 +139,7 @@ public:
 	void NewLine()

 	{

 		SendCommand(commandNewLine);

+		ReturnCursor();

 	}

 

 	bool GetCursorVisible()

diff --git a/source/Concept/Framework/modules/output/engine.h b/source/Concept/Framework/modules/output/engine.h
index 27b9905..7b7a044 100755
--- a/source/Concept/Framework/modules/output/engine.h
+++ b/source/Concept/Framework/modules/output/engine.h
@@ -58,7 +58,7 @@ public:
 

 protected:

 	bool enabled;

-	float curSpeed;

+	int curSpeed;

 

 	//Hardware

 	volatile uint8* hardwarePort;

@@ -94,16 +94,19 @@ protected:
 	}

 

 public:

-	float GetSpeed()

+	int GetSpeed()

 	{

 		return curSpeed;

 	}

 

-	void SetSpeed(float newSpeed)

+	void SetSpeed(int newSpeed)

 	{

 		curSpeed = newSpeed;

 

-		*pwmSpeed = (abs((int16)(newSpeed / SPEED_PER_PWM)));

+		int pwm = abs(newSpeed);

+		if(pwm > 255) pwm = 255;

+

+		*pwmSpeed = pwm;

 

 		UpdateDirection();

 	}