diff options
Diffstat (limited to 'source/Concept/Framework/modules')
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;
}
};
|