From da9860f9a414d8ab403496ac74a51afc5ad3e48b Mon Sep 17 00:00:00 2001 From: sicarius Date: Sun, 18 Feb 2007 17:57:03 +0000 Subject: +++ Additional Codework --- source/Concept/Framework/RoboCode.aps | 2 +- source/Concept/Framework/default/Makefile | 7 +- source/Concept/Framework/main.c | 53 ++++- .../Concept/Framework/modules/executor/navigator.c | 223 ++++++++++++++------- .../Concept/Framework/modules/executor/navigator.h | 14 +- source/Concept/Framework/tools.h | 9 + 6 files changed, 218 insertions(+), 90 deletions(-) (limited to 'source') diff --git a/source/Concept/Framework/RoboCode.aps b/source/Concept/Framework/RoboCode.aps index 4822e36..bb686cb 100644 --- a/source/Concept/Framework/RoboCode.aps +++ b/source/Concept/Framework/RoboCode.aps @@ -1 +1 @@ -RoboCode16-Feb-2007 15:16:4617-Feb-2007 21:48:40241016-Feb-2007 15:16:4644, 12, 0, 462AVR GCCdefault\RoboCode.elfY:\Concept\Framework\falseR00R01R02R03R04R05R06R07R08R09R10R11R12R13R14R15R16R17R18R19R20R21R22R23R24R25R26R27R28R29R30R31000modules\io_module.catmega128io.cmain.crobot.ctools.cmodules\input\distance_sensor.cmodules\input\ir_sensor.cmodules\input\keyboard.cmodules\input\mouse_sensor.cmodules\input\sensor.cmodules\interpreter\position_tracker.cmodules\output\display.cmodules\output\dribbler.cmodules\output\engine.cmodules\output\kicker.cmodules\output\led.cmodules\interpreter\ball_tracker.cmodules\executor\navigator.cstdafx.htools.hatmega128io.hdefines.hrobot.hmodules\input\distance_sensor.hmodules\input\ir_sensor.hmodules\input\keyboard.hmodules\input\mouse_sensor.hmodules\input\sensor.hmodules\output\display.hmodules\output\dribbler.hmodules\output\engine.hmodules\output\kicker.hmodules\output\led.hmodules\interpreter\position_tracker.hmodules\io_module.hmodules\interpreter\ball_tracker.hmodules\executor\navigator.hdefaultNOatmega128100RoboCode.elfdefault\1modules\modules\executor\modules\input\modules\interpreter\modules\logic\modules\output\-Wall -gdwarf-2 -O3 -fsigned-char default0C:\WinAVR\bin\avr-g++.exeC:\WinAVR\utils\bin\make.exeY:\Concept\Framework\stdafx.hY:\Concept\Framework\tools.hY:\Concept\Framework\atmega128io.hY:\Concept\Framework\defines.hY:\Concept\Framework\robot.hY:\Concept\Framework\modules\input\distance_sensor.hY:\Concept\Framework\modules\input\ir_sensor.hY:\Concept\Framework\modules\input\keyboard.hY:\Concept\Framework\modules\input\mouse_sensor.hY:\Concept\Framework\modules\input\sensor.hY:\Concept\Framework\modules\output\display.hY:\Concept\Framework\modules\output\dribbler.hY:\Concept\Framework\modules\output\engine.hY:\Concept\Framework\modules\output\kicker.hY:\Concept\Framework\modules\output\led.hY:\Concept\Framework\modules\interpreter\position_tracker.hY:\Concept\Framework\modules\io_module.hY:\Concept\Framework\modules\interpreter\ball_tracker.hY:\Concept\Framework\modules\executor\navigator.hY:\Concept\Framework\modules\io_module.cY:\Concept\Framework\atmega128io.cY:\Concept\Framework\main.cY:\Concept\Framework\robot.cY:\Concept\Framework\tools.cY:\Concept\Framework\modules\input\distance_sensor.cY:\Concept\Framework\modules\input\ir_sensor.cY:\Concept\Framework\modules\input\keyboard.cY:\Concept\Framework\modules\input\mouse_sensor.cY:\Concept\Framework\modules\input\sensor.cY:\Concept\Framework\modules\interpreter\position_tracker.cY:\Concept\Framework\modules\output\display.cY:\Concept\Framework\modules\output\dribbler.cY:\Concept\Framework\modules\output\engine.cY:\Concept\Framework\modules\output\kicker.cY:\Concept\Framework\modules\output\led.cY:\Concept\Framework\modules\interpreter\ball_tracker.cY:\Concept\Framework\modules\executor\navigator.c00000stdafx.h100001defines.h100002robot.h100003robot.cpp100004atmega128io.h100005main.cpp100006tools.cpp100007tools.h100008atmega128io.cpp100009modules\interpreter\position_tracker.cpp100010modules\interpreter\position_tracker.h100011modules\io_module.h100012modules\output\display.h100013modules\input\ir_sensor.h100014modules\output\engine.h100015modules\input\sensor.h100016modules\input\keyboard.h100017modules\io_module.c100018robot.c100019modules\input\ir_sensor.c100020modules\output\dribbler.h100021modules\output\kicker.h100022modules\output\led.h100023modules\input\distance_sensor.h100024modules\input\mouse_sensor.h100025modules\input\sensor.c100026main.c100027modules\interpreter\ball_tracker.h100028modules\interpreter\ball_tracker.c100029modules\interpreter\position_tracker.c100030modules\input\distance_sensor.c100031modules\output\engine.c100032modules\output\dribbler.c100033modules\executor\navigator.h100034modules\executor\navigator.c1310 73 1036 52822 2232650 32233 33250 32479155 1932672 32255 33272 3250145 032694 32277 33294 3252316 032716 32299 33316 3254519 032606 32189 33206 3243597 032650 32233 33250 3247917 032602 32168 33328 326234 1732694 32277 33294 325232 0399 216 1025 5174 0311 100 941 40531 2336 131 966 4366 0358 160 988 46542 16380 189 1010 49417 2402 218 1032 52322 12314 102 944 4073 16336 131 966 43618 9358 160 988 4651 0380 189 1010 49491 34402 218 1032 5230 0314 102 944 40724 13336 131 966 4363 16358 160 988 4653 16380 189 1010 49447 30402 218 1032 523187 20314 102 944 4071 0336 131 966 436125 14358 160 988 46519 25380 189 1010 49467 38402 218 1032 5237 0314 102 944 40727 0336 131 966 4360 0358 160 988 4650 0402 218 1021 52335 15310 72 1025 52778 34Maximized +RoboCode16-Feb-2007 15:16:4618-Feb-2007 18:45:02241016-Feb-2007 15:16:4644, 12, 0, 462AVR GCCdefault\RoboCode.elfY:\Concept\Framework\falseR00R01R02R03R04R05R06R07R08R09R10R11R12R13R14R15R16R17R18R19R20R21R22R23R24R25R26R27R28R29R30R31000modules\io_module.catmega128io.cmain.crobot.ctools.cmodules\input\distance_sensor.cmodules\input\ir_sensor.cmodules\input\keyboard.cmodules\input\mouse_sensor.cmodules\input\sensor.cmodules\interpreter\position_tracker.cmodules\output\display.cmodules\output\dribbler.cmodules\output\engine.cmodules\output\kicker.cmodules\output\led.cmodules\interpreter\ball_tracker.cmodules\executor\navigator.cstdafx.htools.hatmega128io.hdefines.hrobot.hmodules\input\distance_sensor.hmodules\input\ir_sensor.hmodules\input\keyboard.hmodules\input\mouse_sensor.hmodules\input\sensor.hmodules\output\display.hmodules\output\dribbler.hmodules\output\engine.hmodules\output\kicker.hmodules\output\led.hmodules\interpreter\position_tracker.hmodules\io_module.hmodules\interpreter\ball_tracker.hmodules\executor\navigator.hdefaultNOatmega128100RoboCode.elfdefault\0modules\modules\executor\modules\input\modules\interpreter\modules\logic\modules\output\libm.a-Wall -gdwarf-2 -DF_CPU=16000000ULUL -O3 -fsigned-char -lm default0C:\WinAVR\bin\avr-g++.exeC:\WinAVR\utils\bin\make.exeY:\Concept\Framework\stdafx.hY:\Concept\Framework\tools.hY:\Concept\Framework\atmega128io.hY:\Concept\Framework\defines.hY:\Concept\Framework\robot.hY:\Concept\Framework\modules\input\distance_sensor.hY:\Concept\Framework\modules\input\ir_sensor.hY:\Concept\Framework\modules\input\keyboard.hY:\Concept\Framework\modules\input\mouse_sensor.hY:\Concept\Framework\modules\input\sensor.hY:\Concept\Framework\modules\output\display.hY:\Concept\Framework\modules\output\dribbler.hY:\Concept\Framework\modules\output\engine.hY:\Concept\Framework\modules\output\kicker.hY:\Concept\Framework\modules\output\led.hY:\Concept\Framework\modules\interpreter\position_tracker.hY:\Concept\Framework\modules\io_module.hY:\Concept\Framework\modules\interpreter\ball_tracker.hY:\Concept\Framework\modules\executor\navigator.hY:\Concept\Framework\modules\io_module.cY:\Concept\Framework\atmega128io.cY:\Concept\Framework\main.cY:\Concept\Framework\robot.cY:\Concept\Framework\tools.cY:\Concept\Framework\modules\input\distance_sensor.cY:\Concept\Framework\modules\input\ir_sensor.cY:\Concept\Framework\modules\input\keyboard.cY:\Concept\Framework\modules\input\mouse_sensor.cY:\Concept\Framework\modules\input\sensor.cY:\Concept\Framework\modules\interpreter\position_tracker.cY:\Concept\Framework\modules\output\display.cY:\Concept\Framework\modules\output\dribbler.cY:\Concept\Framework\modules\output\engine.cY:\Concept\Framework\modules\output\kicker.cY:\Concept\Framework\modules\output\led.cY:\Concept\Framework\modules\interpreter\ball_tracker.cY:\Concept\Framework\modules\executor\navigator.c00000stdafx.h100001defines.h100002robot.h100003atmega128io.h100004tools.h100005modules\interpreter\position_tracker.h100006modules\io_module.h100007modules\output\display.h100008modules\input\ir_sensor.h100009modules\output\engine.h100010modules\input\sensor.h100011modules\input\keyboard.h100012modules\io_module.c100013robot.c100014modules\input\ir_sensor.c100015modules\output\dribbler.h100016modules\output\kicker.h100017modules\output\led.h100018modules\input\distance_sensor.h100019modules\input\mouse_sensor.h100020modules\input\sensor.c100021main.c100022modules\interpreter\ball_tracker.h100023modules\interpreter\ball_tracker.c100024modules\interpreter\position_tracker.c100025modules\input\distance_sensor.c100026modules\output\engine.c100027modules\output\dribbler.c100028modules\executor\navigator.h100029modules\executor\navigator.c1299 73 1025 5546 033064 32865 33664 33111155 033064 32865 33664 3311145 033064 32865 33664 3311116 033064 32865 33664 3311111 033064 32865 33664 331115 032293 32095 33019 325508 033064 32865 33790 33320102 033064 32865 33664 331112 032390 32237 33016 3253855 032302 32121 32932 3242627 032327 32152 32957 324576 032349 32181 32979 324861 032371 32210 33001 3251517 032393 32239 33023 325449 032305 32123 32935 324283 032327 32152 32957 3245718 032349 32181 32979 324861 032371 32210 33001 3251571 032393 32239 33023 3254416 032305 32123 32935 324281 0299 72 1025 527177 4Maximized32349 32181 32979 324863 0293 68 1019 52352 032393 32239 33023 325447 032305 32123 32935 324281 032327 32152 32957 324571 032349 32181 32979 324861 032371 32210 33001 325151 032393 32239 33023 32544241 3 diff --git a/source/Concept/Framework/default/Makefile b/source/Concept/Framework/default/Makefile index d0fbfb0..fc917c5 100644 --- a/source/Concept/Framework/default/Makefile +++ b/source/Concept/Framework/default/Makefile @@ -13,7 +13,7 @@ COMMON = -mmcu=$(MCU) ## Compile options common for all C compilation units. CFLAGS = $(COMMON) -CFLAGS += -Wall -gdwarf-2 -O3 -fsigned-char +CFLAGS += -Wall -gdwarf-2 -DF_CPU=16000000ULUL -O3 -fsigned-char CFLAGS += -MD -MP -MT $(*F).o -MF dep/$(@F).d ## Assembly specific flags @@ -22,7 +22,7 @@ ASMFLAGS += -x assembler-with-cpp -Wa,-gdwarf2 ## Linker flags LDFLAGS = $(COMMON) -LDFLAGS += +LDFLAGS += -lm ## Intel Hex file production flags @@ -36,6 +36,9 @@ HEX_EEPROM_FLAGS += --change-section-lma .eeprom=0 ## Include Directories INCLUDES = -I"Y:\Concept\Framework\modules" -I"Y:\Concept\Framework\modules\executor" -I"Y:\Concept\Framework\modules\input" -I"Y:\Concept\Framework\modules\interpreter" -I"Y:\Concept\Framework\modules\logic" -I"Y:\Concept\Framework\modules\output" +## Libraries +LIBS = -lm + ## Objects that must be built in order to link OBJECTS = io_module.o atmega128io.o main.o robot.o tools.o distance_sensor.o ir_sensor.o keyboard.o mouse_sensor.o sensor.o position_tracker.o display.o dribbler.o engine.o kicker.o led.o ball_tracker.o navigator.o diff --git a/source/Concept/Framework/main.c b/source/Concept/Framework/main.c index fa6d718..7822933 100755 --- a/source/Concept/Framework/main.c +++ b/source/Concept/Framework/main.c @@ -132,13 +132,54 @@ int main() uint16 value = 0; int8 value2 = 0; Display* ourDisplay = localRobot->GetModule(IO_DISPLAY_MAIN); + Keyboard* ourKeyboard = localRobot->GetModule(IO_KEYBOARD_MAIN); uint32 i = 1; - Ball_Tracker* ourBallTracker = localRobot->GetModule(IO_BALL_TRACKER_MAIN); + Navigator* ourNavigator = localRobot->GetModule(IO_NAVIGATOR_MAIN); + float rotation = 0; + + ourDisplay->Print("Starting...", 1, 1); //Run while(true) { - msleep(500); + uint8 someInput = ourKeyboard->GetInput(); + switch(someInput) + { + case 1: + ourNavigator->Drive(225.0f * PI / 180.0f, rotation, 200, rotation); + break; + case 2: + ourNavigator->Drive(180.0f * PI / 180.0f, rotation, 200, rotation); + break; + case 3: + ourNavigator->Drive(135.0f * PI / 180.0f, rotation, 200, rotation); + break; + case 4: + ourNavigator->Drive(270.0f * PI / 180.0f, rotation, 200, rotation); + break; + case 5: + ourNavigator->Stop(); + break; + case 6: + ourNavigator->Drive(90.0f * PI / 180.0f, rotation, 200, rotation); + break; + case 7: + ourNavigator->Drive(315.0f * PI / 180.0f, rotation, 200, rotation); + break; + case 8: + ourNavigator->Drive(0.0f * PI / 180.0f, rotation, 200, rotation); + break; + case 9: + ourNavigator->Drive(45.0f * PI / 180.0f, rotation, 200, rotation); + break; + case 10: + rotation -= 10; + break; + case 12: + rotation += 10; + break; + } + msleep(50);/* ourDisplay->Clear(); ourDisplay->Print(i++); ourSensor = localRobot->GetModule(IO_SENSOR_IR_0_DEG); @@ -172,15 +213,11 @@ int main() ourSensor = localRobot->GetModule(IO_SENSOR_IR_330_DEG); value = ourSensor->GetIRIntensity(); ourDisplay->Print(value); - ourDisplay->Print(";"); - value2 = (localRobot->GetModule(IO_SENSOR_MOUSE_LEFT))->GetXMovement(); - ourDisplay->Print(value2, 10, 4); - value2 = (localRobot->GetModule(IO_SENSOR_MOUSE_LEFT))->GetYMovement(); - ourDisplay->Print(value2, 15, 4); + ourDisplay->Print(";");*/ localRobot->Update(); - ourDisplay->PrintFloat(ourBallTracker->GetBallDirection(), 1, 4); + //ourDisplay->PrintFloat(ourBallTracker->GetBallDirection(), 1, 4); } //Cleanup diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index 459d4c1..691b1d0 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -7,7 +7,7 @@ void Navigator::Stop() this->targetAngle = -1.0f; this->targetX = -1.0f; this->targetY = -1.0f; - this->speed = 0; + this->robotSpeed = 0; this->rotationSpeed = 0; for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) @@ -20,12 +20,12 @@ void Navigator::Stop() //----------------------------------------------------------------------------- void Navigator::Rotate(float rotationSpeed) { - this->rotationSpeed = rotationSpeed; + this->rotationSpeed = min(rotationSpeed, 255.0f);; this->direction = -1.0f; this->targetAngle = -1.0f; this->targetX = -1.0f; this->targetY = -1.0f; - this->speed = 0; + this->robotSpeed = 0; for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) { @@ -34,6 +34,66 @@ void Navigator::Rotate(float rotationSpeed) } } +//----------------------------------------------------------------------------- +void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed) +{ + this->rotationSpeed = min(rotationSpeed, 255.0f); + this->direction = newDirection; + this->targetAngle = newAngle; + this->targetX = -1.0f; + this->targetY = -1.0f; + this->robotSpeed = newSpeed; + + if(targetAngle - (parent->GetModule(IO_POSITION_TRACKER_MAIN))->GetOrientation() > PI) + { + if(rotationSpeed > 0) + { + rotationSpeed = -rotationSpeed; + } + } + else + { + if(rotationSpeed < 0) + { + rotationSpeed = -rotationSpeed; + } + } + + Update(); +} + +//----------------------------------------------------------------------------- +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; + } + } + + Update(); +} + //----------------------------------------------------------------------------- void Navigator::Update() { @@ -103,97 +163,110 @@ void Navigator::Update() //Calculate directional/rotational engine speed if(hasDistanceToDrive) { + float relativeDirection = this->direction - locationeer->GetOrientation(); - - float maxRobotSpeed = 255.0f * sqrt(2) / 2.0f; - - if(speed > maxRobotSpeed) + if(targetX >= 0 && targetY >= 0) { - speed = maxRobotSpeed; + float directionToTarget = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY); + relativeDirection = directionToTarget - locationeer->GetOrientation(); } - maxMotorSpeed = speed / (sqrt(2) / 2); + float xDrive = cos(relativeDirection); + float yDrive = sin(relativeDirection); + float vLeft = (-yDrive + sqrt(3) * xDrive) / 2.0f; + float vBack = yDrive; + float vRight = (-yDrive - sqrt(3) * xDrive)/2; + float speedCorrection = 1.0f; - relAngel = direction - orientation - - robotSpeed = sin(45) * maxMotorSpeed - maxMotorSpeed = robotSpeed / sin(45) + float maxEngineSpeed = 255.0f; + float minEngineSpeed = -255.0f; - if(relAngel > 45) - { - sin(relAngel) * (speed / sin(45)) * sin(relAngel + 45); + float maxSingleSpeed = max(max(vLeft, vBack), vRight); - back = speed / sin(relAngel); - } - else + float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + + vBack * (vBack + vLeft + vRight)); + + if(calcSpeed != 1.0f) { - + speedCorrection = 1.0f / calcSpeed; } - left = - back = sin(relAngel) * speed - - - direction = 0: - orientation = 0: - left = speed - right = -speed - back = 0 + float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection; + if(maxOverallSpeed > maxEngineSpeed) + { + robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection); + } + - direction = 0: - orientation = 90: - left = speed - right = speed - back = (sinVcos)(45) * speed + vLeft = vLeft * this->robotSpeed * speedCorrection; + vBack = vBack * this->robotSpeed * speedCorrection; + vRight = vRight * this->robotSpeed * speedCorrection; - direction = 0: - orientation = 45: - left = speed - right = 0 - back = -speed + maxSingleSpeed = max(max(vLeft, vBack), vRight); + float minSingleSpeed = min(min(vLeft, vBack), vRight); - direction = 0: - orientation = 180: - left = -speed - right = speed - back = 0 + if(hasOrientationToAdjust) + { + 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(!hasOrientationToAdjust) + else if(hasOrientationToAdjust) { - Stop(); - } + 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(); } } - -// Aktualieren ohne Parameter -/*void Navigator::Update() { - // Richtung in x und y-Kompontente zerlegen - double y = cos((double)direction*0.01745); // richtung ist winkel - double x = sin((double)direction*0.01745); - - // Abweichung der Ausrichtung ermitteln(als winkel) - int w = sensor.GetAusrichtung() - ausrichtung; - - // Stärke der einzelnen Motoren berechnen - double v0 = (-x+sqrt(3)*y)/2; - double v1 = x; - double v2 = (-x-sqrt(3)*y)/2; - - // Ausgerechnete Stärke an die Motoren übergeben - board.motor(0,(int)((double)v0*speed +w)); - board.motor(1,(int)((double)v1*speed +w)); - board.motor(2,(int)((double)v2*speed +w)); -} - -// Aktualieren mit allen Parametern -void Navigator::Drive(float newDirection, float newAngle, float newSpeed) { - SetDirection(newDirection); - SetAngle(newAngle); - SetSpeed(newSpeed); - Update(); // Und anwenden -}*/ diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h index ed8b3c8..8da52f6 100755 --- a/source/Concept/Framework/modules/executor/navigator.h +++ b/source/Concept/Framework/modules/executor/navigator.h @@ -1,6 +1,7 @@ #ifndef _NAVIGATOR_H #define _NAVIGATOR_H +//#include #include "../../stdafx.h" class Navigator : public IO_Module @@ -14,7 +15,7 @@ public: this->targetAngle = -1.0f; this->targetX = -1.0f; this->targetY = -1.0f; - this->speed = 0; + this->robotSpeed = 0; this->rotationSpeed = 0; } @@ -26,7 +27,7 @@ public: this->targetAngle = -1.0f; this->targetX = -1.0f; this->targetY = -1.0f; - this->speed = 0; + this->robotSpeed = 0; this->rotationSpeed = 0; } @@ -35,7 +36,7 @@ protected: float targetAngle; float targetX; float targetY; - float speed; + float robotSpeed; float rotationSpeed; public: @@ -45,9 +46,14 @@ public: void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed); - void DriveTo(float newX, float newY, float newAngle); + void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed); void Rotate(float rotationSpeed); + + void SetSpeed(float newSpeed) + { + this->robotSpeed = newSpeed; + } }; #endif diff --git a/source/Concept/Framework/tools.h b/source/Concept/Framework/tools.h index 0ca4dda..ca19993 100644 --- a/source/Concept/Framework/tools.h +++ b/source/Concept/Framework/tools.h @@ -9,6 +9,10 @@ void operator delete(void* p); #endif +#define max(a, b) (((a) > (b)) ? (a) : (b)) +#define min(a, b) (((a) < (b)) ? (a) : (b)) +#define sign(a) (((a) < 0) ? (-1) : (1)) + inline void sleep(int sec) { for (int s=0; s