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?) --- source/Concept/Framework/RoboCode.aps | 2 +- source/Concept/Framework/defines.h | 3 +- source/Concept/Framework/main.c | 58 +++++++++----------- .../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 +++ source/Concept/Framework/tools.h | 17 +++++- 8 files changed, 168 insertions(+), 46 deletions(-) (limited to 'source') diff --git a/source/Concept/Framework/RoboCode.aps b/source/Concept/Framework/RoboCode.aps index 0cae013..83aaae6 100644 --- a/source/Concept/Framework/RoboCode.aps +++ b/source/Concept/Framework/RoboCode.aps @@ -1 +1 @@ -RoboCode16-Feb-2007 15:16:4619-Feb-2007 18:25:08241016-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-lmdefault0C:\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.c100030modules\output\display.c100031modules\input\mouse_sensor.c1299 75 1025 5566 033058 32861 33658 33107155 033058 32861 33658 3310745 033058 32861 33658 3310716 033058 32861 33658 3310711 033058 32861 33658 3310743 032287 32091 33013 325468 033058 32861 33784 3331673 033058 32861 33658 331072 032384 32233 33010 32534109 032296 32117 32926 3242227 032321 32148 32951 324536 032343 32177 32973 324821 032365 32206 32995 3251116 7132387 32235 33017 325409 032299 32119 32929 324243 032321 32148 32951 3245321 032343 32177 32973 324821 032365 32206 32995 3251171 0299 72 1025 52760 30Maximized32299 32119 32929 324241 0293 68 1019 523168 1932343 32177 32973 324823 0287 64 1013 51965 032387 32235 33017 325407 032299 32119 32929 324241 032321 32148 32951 324531 032343 32177 32973 324821 032365 32206 32995 3251139 0296 70 1022 525156 20300 100 930 4050 0322 129 952 4340 0 +RoboCode16-Feb-2007 15:16:4620-Feb-2007 21:59:37241016-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-lmdefault0C:\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.c100030modules\output\display.c100031modules\input\mouse_sensor.c100032c:\WinAVR\avr\include\math.h1291 76 1017 5576 0291 72 1025 55353 14Maximized33047 32859 33647 3310545 033047 32859 33647 3310516 033047 32859 33647 3310516 2233047 32859 33647 3310514 032276 32089 33002 325448 033047 32859 33773 33314107 6933047 32859 33647 331052 032373 32231 32999 32532101 3032285 32115 32915 3242027 032310 32146 32940 324516 032332 32175 32962 324801 032354 32204 32984 3250993 032376 32233 33006 325389 032288 32117 32918 324223 032310 32146 32940 3245121 032332 32175 32962 324801 032354 32204 32984 3250971 0288 70 1014 52568 2232288 32117 32918 324221 0282 66 1008 521159 432332 32175 32962 324803 0276 62 1002 51765 032376 32233 33006 3253855 032288 32117 32918 324221 032310 32146 32940 324511 032332 32175 32962 324801 032354 32204 32984 3250949 0285 68 1011 523184 43289 98 919 4030 0311 127 941 4320 0339 160 955 462144 0 diff --git a/source/Concept/Framework/defines.h b/source/Concept/Framework/defines.h index 8cf2d7a..586a3d8 100644 --- a/source/Concept/Framework/defines.h +++ b/source/Concept/Framework/defines.h @@ -58,9 +58,10 @@ //Constants #define SPEED_PER_PWM 1 #define DISTANCE_PER_VALUE 1 +#define TICKS_PER_CM 205.0f #define PI 3.14159265358979323846f -#define CYCLES_PER_CORRECTION 200 +#define CYCLES_PER_CORRECTION 20 #define EMPTY_FLOAT 81188.1484f //IO Module Names diff --git a/source/Concept/Framework/main.c b/source/Concept/Framework/main.c index 8a7d90f..b0fc96b 100755 --- a/source/Concept/Framework/main.c +++ b/source/Concept/Framework/main.c @@ -145,27 +145,29 @@ int main() Keyboard* ourKeyboard = localRobot->GetModule(IO_KEYBOARD_MAIN); uint32 i = 1; Navigator* ourNavigator = localRobot->GetModule(IO_NAVIGATOR_MAIN); + Position_Tracker* ourPosition_Tracker = localRobot->GetModule(IO_POSITION_TRACKER_MAIN); + float rotation = 0; - Mouse_Sensor* mouse_left = localRobot->GetModule(IO_SENSOR_MOUSE_LEFT); - Mouse_Sensor* mouse_right = localRobot->GetModule(IO_SENSOR_MOUSE_RIGHT); + //Mouse_Sensor* mouse_left = localRobot->GetModule(IO_SENSOR_MOUSE_LEFT); + //Mouse_Sensor* mouse_right = localRobot->GetModule(IO_SENSOR_MOUSE_RIGHT); //Run while(true) { - /*ourDisplay->Print(i++,1,1); - ourDisplay->Print(mouse_left->GetXMovement(),1,2); - ourDisplay->Print(" "); - ourDisplay->Print(mouse_left->GetYMovement(),10,2); - ourDisplay->Print(" "); - ourDisplay->Print(mouse_right->GetXMovement(),1,3); - ourDisplay->Print(" "); - ourDisplay->Print(mouse_right->GetYMovement(),10,3); - ourDisplay->Print(" "); - ourDisplay->Print(" ",1,4); // clear + ourDisplay->Print(i++,1,1); + + if(!(i % 200)) + { + ourDisplay->Clear(); + ourDisplay->PrintFloat(ourPosition_Tracker->GetPositionX(),1,2); + ourDisplay->PrintFloat(ourPosition_Tracker->GetPositionY(),1,3); + ourDisplay->PrintFloat(ourPosition_Tracker->GetOrientation() * 180.0f / PI,1,4); + } + - distanceSensor = localRobot->GetModule(IO_SENSOR_DISTANCE_0_DEG); + /*distanceSensor = localRobot->GetModule(IO_SENSOR_DISTANCE_0_DEG); value = distanceSensor->GetDistance(); ourDisplay->Print(value, 1, 4); ourDisplay->Print(";"); @@ -180,47 +182,41 @@ int main() distanceSensor = localRobot->GetModule(IO_SENSOR_DISTANCE_270_DEG); value = distanceSensor->GetDistance(); ourDisplay->Print(value); - ourDisplay->Print(";"); - - - msleep(500);*/ - + ourDisplay->Print(";"); */ + uint8 someInput = ourKeyboard->GetInput(); - //ourDisplay->Clear(); - ourDisplay->Print(i++,1,1); //ourDisplay->Print("Ready to accept...", 1, 2); - ourDisplay->Print(someInput, 1, 3); - ourDisplay->PrintFloat(rotation, 1, 4); switch(someInput) { case 0: ourNavigator->Stop(); + break; case 1: - ourNavigator->Drive(225, rotation, 200, rotation); + ourNavigator->Drive(225, rotation, 100, rotation); break; case 2: - ourNavigator->Drive(180, rotation, 200, rotation); + ourNavigator->Drive(180, rotation, 100, rotation); break; case 3: - ourNavigator->Drive(135, rotation, 200, rotation); + ourNavigator->Drive(135, rotation, 100, rotation); break; case 4: - ourNavigator->Drive(270, rotation, 200, rotation); + ourNavigator->Drive(270, rotation, 100, rotation); break; case 5: ourNavigator->Drive(0, rotation, 0, rotation); break; case 6: - ourNavigator->Drive(90, rotation, 200, rotation); + ourNavigator->Drive(90, rotation, 100, rotation); break; case 7: - ourNavigator->Drive(315, rotation, 200, rotation); + ourNavigator->Drive(315, rotation, 100, rotation); break; case 8: - ourNavigator->Drive(0, rotation, 200, rotation); + ourNavigator->Drive(0, rotation, 100, rotation); break; case 9: - ourNavigator->Drive(45, rotation, 200, rotation); + ourNavigator->Drive(45, rotation, 100, rotation); break; case 10: rotation += 10; @@ -230,8 +226,6 @@ int main() break; } - msleep(50); - /* //ourDisplay->Clear(); ourDisplay->Print(i++,1,1); 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: diff --git a/source/Concept/Framework/tools.h b/source/Concept/Framework/tools.h index 735e773..2607001 100644 --- a/source/Concept/Framework/tools.h +++ b/source/Concept/Framework/tools.h @@ -14,6 +14,21 @@ #define min(a, b) (((a) < (b)) ? (a) : (b)) #define sign(a) (((a) < 0) ? (-1) : (1)) +inline float easyAngle(float angle) +{ + while(angle < 0) + { + angle += 2.0f * PI; + } + + while(angle >= 2.0f * PI) + { + angle -= 2.0f * PI; + } + + return angle; +} + inline void sleep(uint16 sec) { for (uint16 s = 0; s < sec; s++) { @@ -48,7 +63,7 @@ inline float distance2d(float x1, float y1, float x2, float y2) inline float direction2d(float x1, float y1, float x2, float y2) { - return atan((y2 - y1) / (x2 - x1)); + return atan2((y2 - y1), (x2 - x1)); } #endif -- cgit v1.2.3