The Last Day ?

source/Concept/Framework/atmega128io.h

@@ -6,6 +6,8 @@
 #include <avr/io.h>
 #include <avr/interrupt.h>
 
+#include "defines.h"
+
 /************************************************************************
 Title:    Interrupt UART library with receive/transmit circular buffers
 Author:   Peter Fleury <pfleury@gmx.ch>   http://jump.to/fleury

source/Concept/Framework/defines.h

@@ -65,6 +65,8 @@
 #define CYCLES_PER_CORRECTION 20
 #define EMPTY_FLOAT 81188.1484f
 
+#define BALL_HELD_INTENSITY 900
+
 #define WIRELESS_CODE "SPASS!"
 enum WirelessCommands
 {

source/Concept/Framework/main.c

@@ -159,6 +159,8 @@ int main()
 	//Debug code
 	Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN);
 	ourDisplay->Clear();
+	ourDisplay->Print("Roboter fertig gestartet",1,1);
+	sleep(1);
 	
 
 	IR_Sensor* ourSensor = NULL;
@@ -175,6 +177,7 @@ int main()
 	ourDribbler->SetSpeed(1);
 	ourDribbler->SetEnabled(true);
 	Aktuator* ourAktuator = localRobot->GetModule<Aktuator>(IO_AKTUATOR_MAIN);
+	Logic* ourLogic = localRobot->GetModule<Logic>(IO_LOGIC_MAIN);	
 
 	float rotation = 0;
 	float speed = 200;
@@ -182,31 +185,22 @@ int main()
 	//Mouse_Sensor* mouse_left = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
 	//Mouse_Sensor* mouse_right = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
 
+	ourDisplay->Clear();
 
 	//Run
 	while(true)
 	{
 		ourDisplay->Print(i++,1,1);
 
-		msleep(100);
+		//msleep(50);
 				
-		if(!(i % 1))
+		if(!(i % 20))
 		{
 			ourDisplay->Clear();
-			//ourDisplay->Print(ourPosition_Tracker->GetPositionX(),1,2);
-			//ourDisplay->Print(ourPosition_Tracker->GetPositionY(),1,3);
-			//ourDisplay->Print(ourPosition_Tracker->GetOrientation() * 180.0f / PI,1,4);
-		}
-
-		if(!(i % 50))
-		{
-			ourAktuator->Kick();
-		}
-
 
 			distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
 			value = distanceSensor->GetDistance();
-		ourDisplay->Print(value, 1, 2);
+			ourDisplay->Print(value, 1, 4);
 			ourDisplay->Print(";");
 			distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_90_DEG);
 			value = distanceSensor->GetDistance();
@@ -214,12 +208,18 @@ int main()
 			ourDisplay->Print(";");
 			distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_180_DEG);
 			value = distanceSensor->GetDistance();
-		ourDisplay->Print(value, 1, 3);
+			ourDisplay->Print(value);
 			ourDisplay->Print(";");
 			distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_270_DEG);
 			value = distanceSensor->GetDistance();
 			ourDisplay->Print(value);
 			ourDisplay->Print(";");
+		}
+
+		if(!(i % 20))
+		{
+			//ourAktuator->Kick();
+		}
 		
 		uint8 someInput = ourKeyboard->GetInput();
 		//ourDisplay->Print("Ready to accept...", 1, 2);
@@ -260,12 +260,15 @@ int main()
 				ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker
 				break;
 			case 12:
-				ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker
+				ourLogic->SetKeeper(true); // Reset Position_Tracker
 				break;
 		}
 		
 		//ourDisplay->Clear();
-		/*ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);
+		if(!(i % 20))
+		{
+			/*ourDisplay->Clear();
+			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);
 			value = ourSensor->GetIRIntensity();
 			ourDisplay->Print(value, 1, 2);
 			ourDisplay->Print(";");
@@ -296,7 +299,16 @@ int main()
 			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG);
 			value = ourSensor->GetIRIntensity();
 			ourDisplay->Print(value);
-		ourDisplay->Print(";");*/
+			ourDisplay->Print(";");
+			if(ourBallTracker->KnowsBallDirection())
+			{
+				ourDisplay->Print(ourBallTracker->GetBallDirection() * 180.0f / PI, 1, 4);
+			}
+			else
+			{
+				ourDisplay->Print("Not found oder so", 1, 4);
+			}*/
+		}
 		
 		localRobot->Update();
 

source/Concept/Framework/modules/executor/navigator.c

@@ -88,13 +88,34 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
 	CalculateDirection();
 }
 
-//-----------------------------------------------------------------------------
+void Navigator::RotateTo(float newAngle, float rotationSpeed) {
-void Navigator::Update()
+	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();
+}
+
+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;
 	}
+	return targetAngleReached;
+}
 
-	if(targetReached && targetAngleReached)
+//-----------------------------------------------------------------------------
+void Navigator::Update()
+{
+	if(TargetReached() && AngleReached())
 	{
 		Stop();
 	}
-	else if(targetReached || targetAngleReached)
+	/*else if(HasTarget() && !TargetReached())
 	{
 		CalculateDirection();
-	}
+	}*/
 
 	if(!(correctionCountdown--))
 	{

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

source/Concept/Framework/modules/input/distance_sensor.c

@@ -1,22 +1,12 @@
 #include "distance_sensor.h"
 
-/*!
+//-----------------------------------------------------------------------------
- * SRF10 initialsieren
+void Distance_Sensor::SetSignalFactor(uint8 factor)
- */
+{
- 
+  if(factor > 16)
-void Distance_Sensor::srf10_init(void){
+	{
-	srf10_set_range(SRF10_MAX_RANGE);
+	 	factor = 16;
-	//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; }
 
 	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);
 }
 
-/*!
+//-----------------------------------------------------------------------------
- * Reichweite setzen, hat auch Einfluss auf die Messdauer
+void Distance_Sensor::SetSlaveAddress(uint8 newSlaveAddress)
- * @param millimeters Reichweite in mm
+{
- */
- 
-void Distance_Sensor::srf10_set_range(unsigned int millimeters){
 	uint8 temp[2];
 	uint8 state;
 	tx_type tx_frame[2];
 	
 	state = SUCCESS;
 
-    millimeters= (millimeters/43); 
+	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);
+}
+
+//-----------------------------------------------------------------------------
+void Distance_Sensor::SetRange(unsigned int millimeters){
+	uint8 temp[2];
+	uint8 state;
+	tx_type tx_frame[2];
+	
+	state = SUCCESS;
+
+	millimeters = (millimeters/43); 
 
 	tx_frame[0].slave_adr = this->slaveAddr+W;
 	tx_frame[0].size = 2;
@@ -86,13 +105,8 @@ uint8 Distance_Sensor::srf10_ping(uint8 metric_unit){
 	return state;
 }
 
-/*!
+//-----------------------------------------------------------------------------
- * Register auslesen
+uint8 Distance_Sensor::ReadRegister(uint8 registerToRead){
- * @param srf10_register welches Register soll ausgelsen werden
- * @return Inhalt des Registers
- */
-
-uint8 Distance_Sensor::srf10_read_register(uint8 srf10_register){
 	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
+	msleep(10);									//Optimierungs Potential
-	lob=srf10_read_register(SRF10_LOB);
+	lob=ReadRegister(SRF10_LOB);
-	usleep(10);									//Optimierungs Potential
+	msleep(10);									//Optimierungs Potential
-	hib=srf10_read_register(SRF1sr0_HIB);
+	hib=ReadRegister(SRF10_HIB);
 	
 	return (hib*256)+lob;
 }

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_INCHES          0x50	 /*!< Messung in INCHES */
-#define SRF10_CENTIMETERS     0X51	 /*!< Messung in CM */
+#define SRF10_CENTIMETERS     0x51	 /*!< Messung in CM */
-#define SRF10_MICROSECONDS    0X52	 /*!< Messung in Millisekunden */
+#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 */
@@ -52,53 +52,42 @@ public:
 				break;
 		}
 
-		// initialiate the sensor
+		//SetRange(100);
-		srf10_init();
+		SetRange(2000);
+		SetSignalFactor(0x06);
 	}
 
 protected:
 	//Hardware
-	slaveAddr;
+	uint8 slaveAddr;
 
-	/*!
+	void SetSignalFactor(uint8 factor);
- * SRF10 initialsieren
- */
-extern void		srf10_init(void);
 
-/*!
+	void SetRange(uint16 millimeters);
- * 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);
 
 /*!
  * 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

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;

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

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

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

source/Concept/Framework/modules/interpreter/position_tracker.h

@@ -51,8 +51,8 @@ public:
 
 	// returns orientation
 	float GetOrientation()	{
-		return 0.0f; //tmp!!!!!!!!!!
+		//return 0.0f; //tmp!!!!!!!!!!
-		//return orientation;
+		return orientation;
 	}
 };
 

source/Concept/Framework/robot.c

@@ -92,6 +92,8 @@ void Robot::Update()
 
 	GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN)->Update();
 
+	GetModule<Logic>(IO_LOGIC_MAIN)->Update();
+
 	GetModule<Navigator>(IO_NAVIGATOR_MAIN)->Update();
 
 	//insert code here