From f544ab78229d3a4d54b910135ba61bb816009589 Mon Sep 17 00:00:00 2001 From: sicarius Date: Thu, 22 Feb 2007 20:59:02 +0000 Subject: The Last Day ? --- .../Concept/Framework/modules/executor/navigator.c | 52 ++++++++++-- .../Concept/Framework/modules/executor/navigator.h | 4 + .../Framework/modules/input/distance_sensor.c | 96 +++++++++++++--------- .../Framework/modules/input/distance_sensor.h | 47 ++++------- source/Concept/Framework/modules/input/ir_sensor.h | 4 +- .../Framework/modules/interpreter/ball_tracker.c | 15 ++++ .../Framework/modules/interpreter/ball_tracker.h | 10 ++- .../modules/interpreter/position_tracker.c | 21 +++-- .../modules/interpreter/position_tracker.h | 4 +- 9 files changed, 159 insertions(+), 94 deletions(-) (limited to 'source/Concept/Framework/modules') 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 @@ -85,16 +85,37 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, } } + CalculateDirection(); +} + +void Navigator::RotateTo(float newAngle, float rotationSpeed) { + Position_Tracker* locationeer = parent->GetModule(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(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(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(IO_DISPLAY_MAIN))->Print(" ", 5, 1); + /*(parent->GetModule(IO_DISPLAY_MAIN))->Clear(); + (parent->GetModule(IO_DISPLAY_MAIN))->Print(this->orientation * 180.0f / PI, 5, 1); + (parent->GetModule(IO_DISPLAY_MAIN))->Print(this->positionX, 1, 2); + (parent->GetModule(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; } }; -- cgit v1.2.3