The Last Day ?

9 files changed, 159 insertions, 94 deletions

diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c
index 928a5ff..552c494 100755
--- a/source/Concept/Framework/modules/executor/navigator.c
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -86,15 +86,36 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
 	}

 

 	CalculateDirection();

+}

+

+void Navigator::RotateTo(float newAngle, float rotationSpeed) {

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

+

+	this->rotationSpeed = min(rotationSpeed, 255.0f);	

+	this->targetAngle = newAngle * PI / 180.0f;

+

+	if(targetAngle - locationeer->GetOrientation() > PI)

+	{

+		if(rotationSpeed > 0)

+		{

+			rotationSpeed = -rotationSpeed;

+		}

+	}

+	else

+	{

+		if(rotationSpeed < 0)

+		{

+			rotationSpeed = -rotationSpeed;

+		}

+	}

+

+	CalculateDirection();

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

-void Navigator::Update()

-{

+

+bool Navigator::TargetReached() {

 	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)

 	{

@@ -106,6 +127,14 @@ void Navigator::Update()
 		targetReached = true;

 	}

 

+	return targetReached;

+}

+

+bool Navigator::AngleReached() {

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

+

+	bool targetAngleReached = false;

+

 	if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)

 	{

 		targetAngle = EMPTY_FLOAT;

@@ -113,15 +142,20 @@ void Navigator::Update()
 

 		targetAngleReached = true;

 	}

-

-	if(targetReached && targetAngleReached)

+	return targetAngleReached;

+}

+
+//-----------------------------------------------------------------------------

+void Navigator::Update()

+{

+	if(TargetReached() && AngleReached())

 	{

 		Stop();

 	}

-	else if(targetReached || targetAngleReached)

+	/*else if(HasTarget() && !TargetReached())

 	{

 		CalculateDirection();

-	}

+	}*/

 

 	if(!(correctionCountdown--))

 	{

diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h
index 81919f9..7f4b36d 100755
--- a/source/Concept/Framework/modules/executor/navigator.h
+++ b/source/Concept/Framework/modules/executor/navigator.h
@@ -49,6 +49,7 @@ public:
 	void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed);

 

 	void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed);

+	void RotateTo(float newAngle,float roationSpeed);

 

 	void Rotate(float rotationSpeed);

 

@@ -65,6 +66,9 @@ public:
 	{

 		return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);

 	}

+

+	bool TargetReached();

+	bool AngleReached();

 };

 
 #endif
diff --git a/source/Concept/Framework/modules/input/distance_sensor.c b/source/Concept/Framework/modules/input/distance_sensor.c
index 1ab3755..239e63b 100755
--- a/source/Concept/Framework/modules/input/distance_sensor.c
+++ b/source/Concept/Framework/modules/input/distance_sensor.c
@@ -1,22 +1,12 @@
-#include "distance_sensor.h"

-
-/*!
- * SRF10 initialsieren
- */
- 
-void Distance_Sensor::srf10_init(void){
-	srf10_set_range(SRF10_MAX_RANGE);
-	//srf10_set_range(6);	//Mit diesem Wert muss man spielen um das Optimum zu ermitteln
-return;
-}
-
-/*!
- * Verstaerkungsfaktor setzen
- * @param gain Verstaerkungsfaktor
- */
- 
-void Distance_Sensor::srf10_set_gain(unsigned char gain){
-    if(gain>16) { gain=16; }
+#include "distance_sensor.h"
+

+//-----------------------------------------------------------------------------
+void Distance_Sensor::SetSignalFactor(uint8 factor)

+{
+  if(factor > 16)

+	{

+	 	factor = 16;

+	}
 
 	uint8 temp[2];
 	uint8 state;
@@ -24,30 +14,59 @@ void Distance_Sensor::srf10_set_gain(unsigned char gain){
 	
 	state = SUCCESS;
 
-	tx_frame[0].slave_adr = this->slaveAddr+W;
+	tx_frame[0].slave_adr = this->slaveAddr + W;
 	tx_frame[0].size = 2;
 	tx_frame[0].data_ptr = temp;
 	tx_frame[0].data_ptr[0] = 1;
-	tx_frame[0].data_ptr[1] = gain;
+	tx_frame[0].data_ptr[1] = factor;
 
 	tx_frame[1].slave_adr = OWN_ADR;
 
 	state = Send_to_TWI(tx_frame);
+}

+

+//-----------------------------------------------------------------------------
+void Distance_Sensor::SetSlaveAddress(uint8 newSlaveAddress)

+{
+	uint8 temp[2];
+	uint8 state;
+	tx_type tx_frame[2];
+	
+	state = SUCCESS;
+
+	tx_frame[0].slave_adr = this->slaveAddr + W;
+	tx_frame[0].size = 2;
+	tx_frame[0].data_ptr = temp;

+	tx_frame[0].data_ptr[0] = 0;		

+	tx_frame[0].data_ptr[1] = 160;		

+	tx_frame[1].slave_adr = OWN_ADR;
+	state = Send_to_TWI(tx_frame);

+

+	msleep(60);

+

+	tx_frame[0].data_ptr[1] = 170;	
+	state = Send_to_TWI(tx_frame);

+

+	msleep(60);

+

+	tx_frame[0].data_ptr[1] = 165;	
+	state = Send_to_TWI(tx_frame);

+

+	msleep(60);

+

+	tx_frame[0].data_ptr[1] = newSlaveAddress;
+	state = Send_to_TWI(tx_frame);
 }
 
-/*!
- * Reichweite setzen, hat auch Einfluss auf die Messdauer
- * @param millimeters Reichweite in mm
- */
- 
-void Distance_Sensor::srf10_set_range(unsigned int millimeters){
+//-----------------------------------------------------------------------------
+void Distance_Sensor::SetRange(unsigned int millimeters){
 	uint8 temp[2];
 	uint8 state;
 	tx_type tx_frame[2];
 	
 	state = SUCCESS;
 
-    millimeters= (millimeters/43); 
+	millimeters = (millimeters/43); 
 
 	tx_frame[0].slave_adr = this->slaveAddr+W;
 	tx_frame[0].size = 2;
@@ -85,14 +104,9 @@ uint8 Distance_Sensor::srf10_ping(uint8 metric_unit){
 	
 	return state;
 }
-
-/*!
- * Register auslesen
- * @param srf10_register welches Register soll ausgelsen werden
- * @return Inhalt des Registers
- */
-
-uint8 Distance_Sensor::srf10_read_register(uint8 srf10_register){
+

+//-----------------------------------------------------------------------------
+uint8 Distance_Sensor::ReadRegister(uint8 registerToRead){
 	uint8 temp;
 	uint8 value;
 	uint8 state;
@@ -104,7 +118,7 @@ uint8 Distance_Sensor::srf10_read_register(uint8 srf10_register){
 	tx_frame[0].slave_adr = this->slaveAddr+W;
 	tx_frame[0].size = 1;
 	tx_frame[0].data_ptr = &temp;
-	tx_frame[0].data_ptr[0] = srf10_register;
+	tx_frame[0].data_ptr[0] = registerToRead;
 
 	tx_frame[1].slave_adr = this->slaveAddr+R;
 	tx_frame[1].size = 1;
@@ -130,10 +144,10 @@ uint16 Distance_Sensor::srf10_get_measure(){
 	state = SUCCESS;
 
 	state = srf10_ping(SRF10_CENTIMETERS);
-	usleep(10);									//Optimierungs Potential
-	lob=srf10_read_register(SRF10_LOB);
-	usleep(10);									//Optimierungs Potential
-	hib=srf10_read_register(SRF1sr0_HIB);
+	msleep(10);									//Optimierungs Potential
+	lob=ReadRegister(SRF10_LOB);
+	msleep(10);									//Optimierungs Potential
+	hib=ReadRegister(SRF10_HIB);
 	
 	return (hib*256)+lob;
 }
diff --git a/source/Concept/Framework/modules/input/distance_sensor.h b/source/Concept/Framework/modules/input/distance_sensor.h
index 9fb2c58..0bb2baf 100755
--- a/source/Concept/Framework/modules/input/distance_sensor.h
+++ b/source/Concept/Framework/modules/input/distance_sensor.h
@@ -9,9 +9,9 @@
 #define SRF10_MIN_RANGE       0      /*!< Minimale Reichweite 43mm */
 #define SRF10_MAX_RANGE       5977   /*!< Maximale Reichweite 5977mm */
 
-#define SRF10_INCHES          0X50	 /*!< Messung in INCHES */
-#define SRF10_CENTIMETERS     0X51	 /*!< Messung in CM */
-#define SRF10_MICROSECONDS    0X52	 /*!< Messung in Millisekunden */
+#define SRF10_INCHES          0x50	 /*!< Messung in INCHES */
+#define SRF10_CENTIMETERS     0x51	 /*!< Messung in CM */
+#define SRF10_MICROSECONDS    0x52	 /*!< Messung in Millisekunden */
 
 #define SRF10_COMMAND         0		 /*!< W=Befehls-Register R=Firmware*/
 #define SRF10_LIGHT           1		 /*!< W=Verstaerkungsfaktor R=Nicht benutzt */
@@ -50,55 +50,44 @@ public:
 			default:
 				this->slaveAddr = 0;

 				break;

-		}
-
-		// initialiate the sensor
-		srf10_init();

+		}

+

+		//SetRange(100);

+		SetRange(2000);

+		SetSignalFactor(0x06);

 	}

 

 protected:

 	//Hardware
-	slaveAddr;
-
-	/*!
- * SRF10 initialsieren
- */
-extern void		srf10_init(void);
-
-/*!
- * Verstaerkungsfaktor setzen
- * @param gain Verstaerkungsfaktor
- */
-extern void		srf10_set_gain(uint8 gain);
-
-/*!
- * Reichweite setzen, hat auch Einfluss auf die Messdauer
- * @param millimeters Reichweite in mm
- */
-extern void		srf10_set_range(uint16 millimeters);
+	uint8 slaveAddr;
+

+	void SetSignalFactor(uint8 factor);
+

+	void SetRange(uint16 millimeters);
 
 /*!
  * Messung ausloesen
  * @param metric_unit 0x50 in Zoll, 0x51 in cm, 0x52 ms
  * @return Resultat der Aktion
  */
-extern uint8	srf10_ping(uint8 metric_unit);
+ uint8	srf10_ping(uint8 metric_unit);
 
 /*!
  * Register auslesen
  * @param srf10_register welches Register soll ausgelsen werden
  * @return Inhalt des Registers
  */
-extern uint8	srf10_read_register(uint8 SRF10_register);
+ uint8	ReadRegister(uint8 registerToRead);
 
 /*!
  * Messung starten Ergebniss aufbereiten und zurueckgeben
  * @return Messergebniss
  */
-extern uint16	srf10_get_measure(void);
+ uint16	srf10_get_measure(void);
 

 public:

-	float GetDistance();

+	uint16 GetDistance();

+	void SetSlaveAddress(uint8 newSlaveAddress);

 };

 

 #endif
diff --git a/source/Concept/Framework/modules/input/ir_sensor.h b/source/Concept/Framework/modules/input/ir_sensor.h
index 74396ec..c8f692b 100755
--- a/source/Concept/Framework/modules/input/ir_sensor.h
+++ b/source/Concept/Framework/modules/input/ir_sensor.h
@@ -34,11 +34,11 @@ public:
 				break;

 			case IO_SENSOR_IR_100_DEG:

 				this->channel = 3;

-				this->intensityCorrection = 40;

+				this->intensityCorrection = 80;

 				break;

 			case IO_SENSOR_IR_180_DEG:

 				this->channel = 4;

-				this->intensityCorrection = 50;

+				this->intensityCorrection = 70;

 				break;

 			case IO_SENSOR_IR_260_DEG:

 				this->channel = 5;

diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.c b/source/Concept/Framework/modules/interpreter/ball_tracker.c
index 2d85b96..16fdfb4 100755
--- a/source/Concept/Framework/modules/interpreter/ball_tracker.c
+++ b/source/Concept/Framework/modules/interpreter/ball_tracker.c
@@ -21,6 +21,21 @@ void Ball_Tracker::Update()
 		{

 			greatestIntensity = i;

 		}

+

+		if(i == 0)

+		{

+			if(intensity[i] > BALL_HELD_INTENSITY) // Ball derzeit sehr nah dran

+			{

+				ballHeld = true;

+				ballHeldCounter = 100;

+			}

+			else if(ballHeldCounter > 0) // Oder vor kurzem erst sehr nah dran

+			{

+				ballHeld = true;

+				ballHeldCounter--;		

+			}

+			else ballHeld = false; // ansonsten hat er den Ball nicht

+		}

 	}

 

 	if(intensity[greatestIntensity])

diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.h b/source/Concept/Framework/modules/interpreter/ball_tracker.h
index c62f05e..cb90ff2 100755
--- a/source/Concept/Framework/modules/interpreter/ball_tracker.h
+++ b/source/Concept/Framework/modules/interpreter/ball_tracker.h
@@ -11,6 +11,8 @@ public:
 		this->parent = NULL;

 		this->moduleId = 0;

 		this->direction = EMPTY_FLOAT;

+		this->ballHeldCounter = 0;

+		this->ballHeld = false;

 	}

 

 	Ball_Tracker(uint32 trackerId)

@@ -18,10 +20,14 @@ public:
 		this->parent = NULL;

 		this->moduleId = trackerId;

 		this->direction = EMPTY_FLOAT;

+		this->ballHeldCounter = 0;

+		this->ballHeld = false;

 	}

 

 protected:

 	float direction;

+	uint8 ballHeldCounter;

+	bool ballHeld;

 

 public:

 	void Update();

@@ -33,12 +39,12 @@ public:
 

 	bool KnowsBallDirection()

 	{

-		return direction != EMPTY_FLOAT;

+		return (direction != EMPTY_FLOAT);

 	}

 

 	bool RobotHasBall()

 	{

-		//fill me!

+		return ballHeld;

 	}

 };

 

diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c
index a64ab60..f6d67ac 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.c
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.c
@@ -10,6 +10,10 @@ void Position_Tracker::Update()
 	// Generate a vector for the left mouse

 	int8 leftX = mouseLeft->GetXMovement();

 	int8 leftY = mouseLeft->GetYMovement();

+	// Generate a vector for the right mouse

+	int8 rightX = mouseRight->GetXMovement();

+	int8 rightY = mouseRight->GetYMovement();

+

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

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

 

@@ -23,9 +27,6 @@ void Position_Tracker::Update()
 		movementLeftY = 0;

 	}

 

-	// Generate a vector for the right mouse

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

 

@@ -50,13 +51,9 @@ void Position_Tracker::Update()
 	float robotMovementY = movementDifferenceY / 2.0f;

 	robotMovementX += movementLeftX + mouseLeft->GetPositionX() + (-mouseLeft->GetPositionX() * cos(orientationDiff));

 	robotMovementY += movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff));

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

-

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

-	float absoluteDiffY	= sin(this->orientation + (orientationDiff / 2.0f) + PI / 2.0f) * robotMovementY + cos(this->orientation + (orientationDiff / 2.0f) - PI / 2.0f) * robotMovementX;

 

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

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

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

+	float absoluteDiffY	= sin(this->orientation + (orientationDiff / 2.0f) + PI / 2.0f) * robotMovementY - cos(this->orientation + (orientationDiff / 2.0f) - PI / 2.0f) * robotMovementX;

 

 	if(!robotMovementX && !robotMovementY)

 	{

@@ -74,4 +71,10 @@ void Position_Tracker::Update()
 	this->orientation += orientationDiff;

 

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

+

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

+	/*(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Clear();

+	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(this->orientation * 180.0f / PI, 5, 1);

+	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(this->positionX, 1, 2);

+	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(this->positionY, 1, 3);*/

 }

diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h
index 47d0740..56b16c4 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.h
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.h
@@ -51,8 +51,8 @@ public:
 

 	// returns orientation

 	float GetOrientation()	{

-		return 0.0f; //tmp!!!!!!!!!!

-		//return orientation;

+		//return 0.0f; //tmp!!!!!!!!!!

+		return orientation;

 	}

 };