From ec2a18e931cdaa6c62a8843c29dc93c4a823a2cd Mon Sep 17 00:00:00 2001 From: masterm Date: Mon, 19 Feb 2007 22:02:01 +0000 Subject: +++ corrected svn cripplement --- .../Concept/Framework/modules/executor/navigator.c | 298 ++------------------- .../Concept/Framework/modules/executor/navigator.h | 76 +----- .../Framework/modules/input/distance_sensor.c | 8 +- source/Concept/Framework/modules/output/engine.h | 8 +- 4 files changed, 23 insertions(+), 367 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 47b1c69..a69c533 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -1,138 +1,3 @@ -<<<<<<< .mine -#include "navigator.h" - -//----------------------------------------------------------------------------- -void Navigator::Stop() -{ - this->direction = 0; - this->targetAngle = 0; - //this->targetX = EMPTY_FLOAT; - //this->targetY = EMPTY_FLOAT; - this->robotSpeed = 0; - this->rotationSpeed = 0; - - /*for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) - { - (parent->GetModule(i))->SetSpeed(0); - (parent->GetModule(i))->SetEnabled(true); - }*/ -} - -//----------------------------------------------------------------------------- -void Navigator::Rotate(float newTargetAngle,float newRotationSpeed) -{ - this->rotationSpeed = min(newRotationSpeed, 255.0f);; - this->direction = 0; - this->targetAngle = newTargetAngle; - //this->targetX = EMPTY_FLOAT; - //this->targetY = EMPTY_FLOAT; - this->robotSpeed = 0; - - /*for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) - { - (parent->GetModule(i))->SetSpeed(rotationSpeed); - (parent->GetModule(i))->SetEnabled(true); - }*/ -} - -//----------------------------------------------------------------------------- -void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed) -{ - this->rotationSpeed = min(rotationSpeed, 255.0f); - this->direction = newDirection*PI/180; - this->targetAngle = newAngle; - this->targetX = EMPTY_FLOAT; - this->targetY = EMPTY_FLOAT; - this->robotSpeed = newSpeed; - - if(targetAngle == EMPTY_FLOAT) - { - rotationSpeed = 0; - } - else - { - rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - (parent->GetModule(IO_POSITION_TRACKER_MAIN))->GetOrientation())); - } - - CalculateEngines(); -} - -//----------------------------------------------------------------------------- -void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed) -{ - if(newX < 0 || newY < 0) return; - - Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); - - this->rotationSpeed = min(rotationSpeed, 255.0f); - this->targetX = newX; - this->targetY = newY; - this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);; - this->targetAngle = newAngle; - this->robotSpeed = newSpeed; - - if(targetAngle - locationeer->GetOrientation() > PI) - { - if(rotationSpeed > 0) - { - rotationSpeed = -rotationSpeed; - } - } - else - { - if(rotationSpeed < 0) - { - rotationSpeed = -rotationSpeed; - } - } - - CalculateDirection(); -} - -//----------------------------------------------------------------------------- -void Navigator::Update() -{ - 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) - { - targetX = EMPTY_FLOAT; - targetY = EMPTY_FLOAT; - direction = EMPTY_FLOAT; - robotSpeed = 0; - - targetReached = true; - } - - if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f) - { - targetAngle = EMPTY_FLOAT; - rotationSpeed = 0; - - targetAngleReached = true; - } - - if(targetReached && targetAngleReached) - { - Stop(); - } - else if(targetReached || targetAngleReached) - { - CalculateDirection(); - } - - if(!(correctionCountdown--)) - { - CalculateDirection(); - } -} - -//----------------------------------------------------------------------------- -void Navigator::CalculateDirection() -======= #include "navigator.h" //----------------------------------------------------------------------------- @@ -150,140 +15,12 @@ void Navigator::Stop() (parent->GetModule(i))->SetSpeed(0); (parent->GetModule(i))->SetEnabled(true); } -} - -//----------------------------------------------------------------------------- -void Navigator::Rotate(float rotationSpeed) ->>>>>>> .r199 -{ -<<<<<<< .mine - Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); - correctionCountdown = CYCLES_PER_CORRECTION; - - if(targetAngle == EMPTY_FLOAT && !HasTarget()) return; - - if(HasTarget()) - { - direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY); - } - - if(targetAngle != EMPTY_FLOAT) - { - rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - locationeer->GetOrientation())); - } - - CalculateEngines(); } -//----------------------------------------------------------------------------- -void Navigator::CalculateEngines() +//----------------------------------------------------------------------------- +void Navigator::Rotate(float rotationSpeed) { - Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); - if(direction != EMPTY_FLOAT) - { - float relativeDirection = this->direction - locationeer->GetOrientation(); - - float xDrive = cos(relativeDirection + PI / 6.0f); - float yDrive = sin(relativeDirection + PI / 6.0f); - - float vLeft = xDrive; - float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f; - float vRight = (-xDrive - sqrt(3) * yDrive) / 2.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)); - - if(calcSpeed != 1.0f) - { - speedCorrection = 1.0f / calcSpeed; - } - - float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection; - if(maxOverallSpeed > maxEngineSpeed) - { - robotSpeed = maxEngineSpeed / (maxSingleSpeed * 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)); - float minSingleSpeed = min(min(vLeft, vBack), vRight); - - if(rotationSpeed) - { - if(this->rotationSpeed > 0) - { - if(maxEngineSpeed - maxSingleSpeed < this->rotationSpeed) - { - vLeft += maxEngineSpeed - maxSingleSpeed; - vBack += maxEngineSpeed - maxSingleSpeed; - vRight += maxEngineSpeed - maxSingleSpeed; - } - else - { - vLeft += this->rotationSpeed; - vBack += this->rotationSpeed; - vRight += this->rotationSpeed; - } - } - else - { - if((minEngineSpeed - minSingleSpeed) < this->rotationSpeed) - { - vLeft -= minEngineSpeed - minSingleSpeed; - vBack -= minEngineSpeed - minSingleSpeed; - vRight -= minEngineSpeed - minSingleSpeed; - } - else - { - vLeft -= this->rotationSpeed; - vBack -= this->rotationSpeed; - vRight -= this->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; - } - else if(rotationSpeed) - { - Engine* curEngine = parent->GetModule(IO_ENGINE_DRIVE_LEFT); - curEngine->SetSpeed(this->rotationSpeed); - curEngine->SetEnabled(true); - curEngine = parent->GetModule(IO_ENGINE_DRIVE_BACK); - curEngine->SetSpeed(this->rotationSpeed); - curEngine->SetEnabled(true); - curEngine = parent->GetModule(IO_ENGINE_DRIVE_RIGHT); - curEngine->SetSpeed(this->rotationSpeed); - curEngine->SetEnabled(true); - curEngine = NULL; - } - else - { - Stop(); - } -} -======= - this->rotationSpeed = min(rotationSpeed, 255.0f);; + this->rotationSpeed = min(fabs(rotationSpeed), 255.0f) * sign(rotationSpeed); this->direction = EMPTY_FLOAT; this->targetAngle = EMPTY_FLOAT; this->targetX = EMPTY_FLOAT; @@ -295,14 +32,14 @@ void Navigator::CalculateEngines() (parent->GetModule(i))->SetSpeed(rotationSpeed); (parent->GetModule(i))->SetEnabled(true); } -} - +} + //----------------------------------------------------------------------------- void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed) { this->rotationSpeed = min(rotationSpeed, 255.0f); - this->direction = newDirection; - this->targetAngle = newAngle; + this->direction = newDirection * PI / 180.0f; + this->targetAngle = newAngle * PI / 180.0f; this->targetX = EMPTY_FLOAT; this->targetY = EMPTY_FLOAT; this->robotSpeed = newSpeed; @@ -317,8 +54,8 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float } CalculateEngines(); -} - +} + //----------------------------------------------------------------------------- void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed) { @@ -330,7 +67,7 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, this->targetX = newX; this->targetY = newY; this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);; - this->targetAngle = newAngle; + this->targetAngle = newAngle * PI / 180.0f; this->robotSpeed = newSpeed; if(targetAngle - locationeer->GetOrientation() > PI) @@ -349,8 +86,8 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, } CalculateDirection(); -} - +} + //----------------------------------------------------------------------------- void Navigator::Update() { @@ -390,8 +127,8 @@ void Navigator::Update() { CalculateDirection(); } -} - +} + //----------------------------------------------------------------------------- void Navigator::CalculateDirection() { @@ -412,8 +149,8 @@ void Navigator::CalculateDirection() } CalculateEngines(); -} - +} + //----------------------------------------------------------------------------- void Navigator::CalculateEngines() { @@ -519,5 +256,4 @@ void Navigator::CalculateEngines() { Stop(); } -} ->>>>>>> .r199 +} diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h index 848c9fa..81919f9 100755 --- a/source/Concept/Framework/modules/executor/navigator.h +++ b/source/Concept/Framework/modules/executor/navigator.h @@ -1,79 +1,6 @@ -<<<<<<< .mine -#ifndef _NAVIGATOR_H -#define _NAVIGATOR_H - -//#include -#include "../../stdafx.h" - -class Navigator : public IO_Module -{ -public: - Navigator() - { - this->parent = NULL; - this->moduleId = 0; - this->correctionCountdown = CYCLES_PER_CORRECTION; - this->direction = EMPTY_FLOAT; - this->targetAngle = EMPTY_FLOAT; - this->targetX = EMPTY_FLOAT; - this->targetY = EMPTY_FLOAT; - this->robotSpeed = 0; - this->rotationSpeed = 0; - } - - Navigator(uint32 navigatorId) - { - this->parent = NULL; - this->moduleId = navigatorId; - this->correctionCountdown = CYCLES_PER_CORRECTION; - this->direction = EMPTY_FLOAT; - this->targetAngle = EMPTY_FLOAT; - this->targetX = EMPTY_FLOAT; - this->targetY = EMPTY_FLOAT; - this->robotSpeed = 0; - this->rotationSpeed = 0; - } - -protected: - uint16 correctionCountdown; - float direction; - float targetAngle; - float targetX; - float targetY; - float robotSpeed; - float rotationSpeed; - -public: - void Update(); - - void Stop(); - - void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed); - - void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed); - - void Rotate(float rotationSpeed); - - void SetSpeed(float newSpeed) - { - this->robotSpeed = newSpeed; - } - - void CalculateDirection(); - - void CalculateEngines(); - - bool HasTarget() - { - return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT); - } -}; - -======= #ifndef _NAVIGATOR_H #define _NAVIGATOR_H -//#include #include "../../stdafx.h" class Navigator : public IO_Module @@ -139,6 +66,5 @@ public: return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT); } }; - ->>>>>>> .r199 + #endif diff --git a/source/Concept/Framework/modules/input/distance_sensor.c b/source/Concept/Framework/modules/input/distance_sensor.c index 508eeed..262d35f 100755 --- a/source/Concept/Framework/modules/input/distance_sensor.c +++ b/source/Concept/Framework/modules/input/distance_sensor.c @@ -12,12 +12,8 @@ float Distance_Sensor::GetDistance() *hardwarePort &= ~pin;//Deactivate port *hardwareDDR &= ~pin;//Set pin input - //(parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1); - //Wait for response - for(int i=0;(!(PINC & pin))&&(i < 1000);i++) {asm volatile("nop");} - - //(parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1); + for(uint16 i = 0; (!(*hardwarePin & pin)) && (i < 1000); i++) { asm volatile("nop"); } //Calculate duration of response while((*hardwarePin & pin)&&(result < 300000)) @@ -26,7 +22,5 @@ float Distance_Sensor::GetDistance() asm volatile("nop"); } - //(parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1); - return (float(result) * DISTANCE_PER_VALUE); } diff --git a/source/Concept/Framework/modules/output/engine.h b/source/Concept/Framework/modules/output/engine.h index 7b7a044..4c022bb 100755 --- a/source/Concept/Framework/modules/output/engine.h +++ b/source/Concept/Framework/modules/output/engine.h @@ -58,7 +58,7 @@ public: protected: bool enabled; - int curSpeed; + int16 curSpeed; //Hardware volatile uint8* hardwarePort; @@ -94,16 +94,16 @@ protected: } public: - int GetSpeed() + int16 GetSpeed() { return curSpeed; } - void SetSpeed(int newSpeed) + void SetSpeed(int16 newSpeed) { curSpeed = newSpeed; - int pwm = abs(newSpeed); + uint16 pwm = abs(newSpeed); if(pwm > 255) pwm = 255; *pwmSpeed = pwm; -- cgit v1.2.3