From 22433e52a7b0f2e5c548edb915ea11a38a8077ad Mon Sep 17 00:00:00 2001 From: sicarius Date: Tue, 20 Feb 2007 21:01:02 +0000 Subject: +++ Codework, Navigator&Positiontracker done (buggy?) --- .../Concept/Framework/modules/executor/navigator.c | 44 +++++++++++---- .../Concept/Framework/modules/input/mouse_sensor.h | 22 +++++++- .../modules/interpreter/position_tracker.c | 62 +++++++++++++++++++++- .../modules/interpreter/position_tracker.h | 6 +++ 4 files changed, 123 insertions(+), 11 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 a69c533..5dbf8bf 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -54,6 +54,30 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float } CalculateEngines(); + + //dings,,, + + /*float xDrive = cos(direction + PI / 6.0f); + float yDrive = sin(direction + PI / 6.0f); + + float vLeft = xDrive; + float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f; + float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f; + + vLeft = vLeft * this->robotSpeed + rotationSpeed; + vBack = vBack * this->robotSpeed + rotationSpeed; + vRight = vRight * this->robotSpeed + rotationSpeed; + + Engine* curEngine = parent->GetModule(IO_ENGINE_DRIVE_LEFT); + curEngine->SetSpeed(vLeft); + curEngine->SetEnabled(true); + curEngine = parent->GetModule(IO_ENGINE_DRIVE_BACK); + curEngine->SetSpeed(vBack); + curEngine->SetEnabled(true); + curEngine = parent->GetModule(IO_ENGINE_DRIVE_RIGHT); + curEngine->SetSpeed(vRight); + curEngine->SetEnabled(true); + curEngine = NULL;*/ } //----------------------------------------------------------------------------- @@ -154,6 +178,8 @@ void Navigator::CalculateDirection() //----------------------------------------------------------------------------- void Navigator::CalculateEngines() { + Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); + if(direction != EMPTY_FLOAT) { float relativeDirection = this->direction - locationeer->GetOrientation(); @@ -165,15 +191,15 @@ void Navigator::CalculateEngines() float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f; float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f; - float speedCorrection = 1.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));*/ + float calcSpeed = 1.0f;//sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + + // vBack * (vBack + vLeft + vRight)); if(calcSpeed != 1.0f) { @@ -184,14 +210,14 @@ void Navigator::CalculateEngines() if(maxOverallSpeed > maxEngineSpeed) { robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection); - } + }*/ - vLeft = vLeft * this->robotSpeed * speedCorrection; - vBack = vBack * this->robotSpeed * speedCorrection; - vRight = vRight * this->robotSpeed * 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)); + /*maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight)); float minSingleSpeed = min(min(vLeft, vBack), vRight); if(rotationSpeed) @@ -226,7 +252,7 @@ void Navigator::CalculateEngines() vRight -= this->rotationSpeed; } } - } + }*/ Engine* curEngine = parent->GetModule(IO_ENGINE_DRIVE_LEFT); curEngine->SetSpeed(vLeft); diff --git a/source/Concept/Framework/modules/input/mouse_sensor.h b/source/Concept/Framework/modules/input/mouse_sensor.h index 460e387..b3d1b1a 100755 --- a/source/Concept/Framework/modules/input/mouse_sensor.h +++ b/source/Concept/Framework/modules/input/mouse_sensor.h @@ -24,6 +24,8 @@ public: this->configReset = 0; this->configAwake = 0; this->newImage = false; + this->positionX = 0; + this->positionY = 0; } Mouse_Sensor(uint32 sensorId) @@ -47,6 +49,8 @@ public: this->registerDeltaY = 0x02; this->configReset = 0x80; this->configAwake = 0x01; + this->positionX = -3.88f * TICKS_PER_CM; + this->positionY = -3.88f * TICKS_PER_CM; break; case IO_SENSOR_MOUSE_RIGHT: this->hardwarePort = &PORTC; @@ -61,6 +65,8 @@ public: this->registerDeltaY = 0x02; this->configReset = 0x80; this->configAwake = 0x01; + this->positionX = -3.88f * TICKS_PER_CM; + this->positionY = 3.88f * TICKS_PER_CM; break; default: this->hardwarePort = NULL; @@ -101,8 +107,10 @@ protected: uint8 registerDeltaY; uint8 configReset; uint8 configAwake; + //Information + float positionX; + float positionY; -public: void WriteByte(uint8 newByte) { *hardwareDDR |= pinSDA;//Set SDA output @@ -180,6 +188,8 @@ public: return pixel; } + +public: uint8 GetSqual() { return Read(registerSqual); @@ -194,6 +204,16 @@ public: { return (int8)(Read(registerDeltaY)); } + + float GetPositionX() + { + return positionX; + } + + float GetPositionY() + { + return positionY; + } }; #endif diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c index 262069a..c8cd52d 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.c +++ b/source/Concept/Framework/modules/interpreter/position_tracker.c @@ -3,5 +3,65 @@ //----------------------------------------------------------------------------- void Position_Tracker::Update() { - //insert code here + Mouse_Sensor* mouseLeft = parent->GetModule(IO_SENSOR_MOUSE_LEFT); + Mouse_Sensor* mouseRight = parent->GetModule(IO_SENSOR_MOUSE_RIGHT); + + int8 leftX = mouseLeft->GetXMovement(); + int8 leftY = mouseLeft->GetYMovement(); + float distanceLeft = sqrt(leftX * leftX + leftY * leftY); + float angleLeft = easyAngle(atan2(leftY, leftX) + (225.0f * PI / 180.0f)); + + float movementLeftX = cos(angleLeft) * distanceLeft; + float movementLeftY = sin(angleLeft) * distanceLeft; + + if(!leftX && !leftY) + { + movementLeftX = 0; + movementLeftY = 0; + } + + 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)); + + float movementRightX = cos(angleRight) * distanceRight; + float movementRightY = sin(angleRight) * distanceRight; + + if(!rightX && !rightY) + { + movementRightX = 0; + movementRightY = 0; + } + + float movementDifferenceX = movementRightX - movementLeftX; + float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY()); + + float robotMovementX = movementDifferenceX / 2.0f; + float robotMovementY = movementDifferenceY / 2.0f; + robotMovementX += movementLeftX; + robotMovementY += movementLeftY + mouseLeft->GetPositionY(); + float robotDistance = sqrt(robotMovementX * robotMovementX + robotMovementY * robotMovementY); + + float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f); + + 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) + { + absoluteDiffX = 0; + absoluteDiffY = 0; + } + + //(parent->GetModule(IO_DISPLAY_MAIN))->Print(" ", 5, 1); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print(absoluteDiffX, 5, 1); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print(absoluteDiffY, 12, 1); + + this->positionX += absoluteDiffX; + this->positionY += absoluteDiffY; + + this->orientation += orientationDiff; + + this->orientation = easyAngle(this->orientation); } diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h index b31dd8f..e93ef0c 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.h +++ b/source/Concept/Framework/modules/interpreter/position_tracker.h @@ -10,12 +10,18 @@ public: { this->parent = NULL; this->moduleId = 0; + this->positionX = 0; + this->positionY = 0; + this->orientation = 0; } Position_Tracker(uint32 trackerId) { this->parent = NULL; this->moduleId = trackerId; + this->positionX = 0; + this->positionY = 0; + this->orientation = 0; } protected: -- cgit v1.2.3