From 42a38959ff5c097463c2c95d993e5934f100d223 Mon Sep 17 00:00:00 2001 From: sicarius Date: Mon, 19 Feb 2007 20:57:03 +0000 Subject: Code-stuff --- .../Concept/Framework/modules/executor/navigator.c | 264 +++++++++++++++++++++ .../Concept/Framework/modules/executor/navigator.h | 73 ++++++ .../Framework/modules/input/distance_sensor.c | 12 +- .../Concept/Framework/modules/input/mouse_sensor.h | 16 +- source/Concept/Framework/modules/output/display.h | 5 +- source/Concept/Framework/modules/output/engine.h | 11 +- 6 files changed, 362 insertions(+), 19 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 ceeda60..47b1c69 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -1,3 +1,138 @@ +<<<<<<< .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" //----------------------------------------------------------------------------- @@ -19,7 +154,135 @@ void Navigator::Stop() //----------------------------------------------------------------------------- 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() +{ + 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->direction = EMPTY_FLOAT; this->targetAngle = EMPTY_FLOAT; @@ -257,3 +520,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 56e7b8f..848c9fa 100755 --- a/source/Concept/Framework/modules/executor/navigator.h +++ b/source/Concept/Framework/modules/executor/navigator.h @@ -1,3 +1,75 @@ +<<<<<<< .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 @@ -68,4 +140,5 @@ public: } }; +>>>>>>> .r199 #endif diff --git a/source/Concept/Framework/modules/input/distance_sensor.c b/source/Concept/Framework/modules/input/distance_sensor.c index 977784f..508eeed 100755 --- a/source/Concept/Framework/modules/input/distance_sensor.c +++ b/source/Concept/Framework/modules/input/distance_sensor.c @@ -9,24 +9,24 @@ float Distance_Sensor::GetDistance() *hardwareDDR |= pin;//Set pin output *hardwarePort |= pin;//Activate port usleep(10);//Wait for 10µs - //*hardwarePort &= ~pin;//Deactivate port + *hardwarePort &= ~pin;//Deactivate port *hardwareDDR &= ~pin;//Set pin input - (parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1); //Wait for response - while(!(PINC & pin)){asm volatile("nop");} + for(int i=0;(!(PINC & pin))&&(i < 1000);i++) {asm volatile("nop");} - (parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1); //Calculate duration of response - while(*hardwarePin & pin) + while((*hardwarePin & pin)&&(result < 300000)) { result++; asm volatile("nop"); } - (parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1); return (float(result) * DISTANCE_PER_VALUE); } diff --git a/source/Concept/Framework/modules/input/mouse_sensor.h b/source/Concept/Framework/modules/input/mouse_sensor.h index 30f9d53..460e387 100755 --- a/source/Concept/Framework/modules/input/mouse_sensor.h +++ b/source/Concept/Framework/modules/input/mouse_sensor.h @@ -42,9 +42,9 @@ public: this->pinSCK = (1 << 6); this->registerConfig = 0x00; this->registerPixelData = 0x08; - this->registerSqual = 0x4; - this->registerDeltaX = 0x3; - this->registerDeltaY = 0x2; + this->registerSqual = 0x04; + this->registerDeltaX = 0x03; + this->registerDeltaY = 0x02; this->configReset = 0x80; this->configAwake = 0x01; break; @@ -52,13 +52,13 @@ public: this->hardwarePort = &PORTC; this->hardwareDDR = &DDRC; this->hardwarePin = &PINC; - this->pinSDA = (1 << 5); - this->pinSCK = (1 << 7); + this->pinSDA = (1 << 7); + this->pinSCK = (1 << 5); this->registerConfig = 0x00; this->registerPixelData = 0x08; - this->registerSqual = 0x4; - this->registerDeltaX = 0x3; - this->registerDeltaY = 0x2; + this->registerSqual = 0x04; + this->registerDeltaX = 0x03; + this->registerDeltaY = 0x02; this->configReset = 0x80; this->configAwake = 0x01; break; diff --git a/source/Concept/Framework/modules/output/display.h b/source/Concept/Framework/modules/output/display.h index e221120..bbfa292 100755 --- a/source/Concept/Framework/modules/output/display.h +++ b/source/Concept/Framework/modules/output/display.h @@ -39,7 +39,7 @@ public: this->settingIllumination = 76; this->settingCursorPosition = 79; msleep(100); - uart1_init(103);//9600 BAUD at 16MHz Atmel + uart1_init(51);//19200 BAUD at 16MHz Atmel msleep(100); break; default: @@ -52,6 +52,8 @@ public: this->settingCursorPosition = 0; break; } + + Clear(); } protected: @@ -137,6 +139,7 @@ public: void NewLine() { SendCommand(commandNewLine); + ReturnCursor(); } bool GetCursorVisible() diff --git a/source/Concept/Framework/modules/output/engine.h b/source/Concept/Framework/modules/output/engine.h index 27b9905..7b7a044 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; - float curSpeed; + int curSpeed; //Hardware volatile uint8* hardwarePort; @@ -94,16 +94,19 @@ protected: } public: - float GetSpeed() + int GetSpeed() { return curSpeed; } - void SetSpeed(float newSpeed) + void SetSpeed(int newSpeed) { curSpeed = newSpeed; - *pwmSpeed = (abs((int16)(newSpeed / SPEED_PER_PWM))); + int pwm = abs(newSpeed); + if(pwm > 255) pwm = 255; + + *pwmSpeed = pwm; UpdateDirection(); } -- cgit v1.2.3