From f544ab78229d3a4d54b910135ba61bb816009589 Mon Sep 17 00:00:00 2001 From: sicarius Date: Thu, 22 Feb 2007 20:59:02 +0000 Subject: The Last Day ? --- source/Concept/Framework/RoboCode.aps | 2 +- source/Concept/Framework/atmega128io.h | 4 +- source/Concept/Framework/defines.h | 2 + source/Concept/Framework/main.c | 130 +++++++++++---------- .../Concept/Framework/modules/executor/navigator.c | 52 +++++++-- .../Concept/Framework/modules/executor/navigator.h | 4 + .../Framework/modules/input/distance_sensor.c | 96 ++++++++------- .../Framework/modules/input/distance_sensor.h | 47 +++----- source/Concept/Framework/modules/input/ir_sensor.h | 4 +- .../Framework/modules/interpreter/ball_tracker.c | 15 +++ .../Framework/modules/interpreter/ball_tracker.h | 10 +- .../modules/interpreter/position_tracker.c | 21 ++-- .../modules/interpreter/position_tracker.h | 4 +- source/Concept/Framework/robot.c | 2 + 14 files changed, 238 insertions(+), 155 deletions(-) diff --git a/source/Concept/Framework/RoboCode.aps b/source/Concept/Framework/RoboCode.aps index 609c3cf..ca29afc 100644 --- a/source/Concept/Framework/RoboCode.aps +++ b/source/Concept/Framework/RoboCode.aps @@ -1 +1 @@ -RoboCode16-Feb-2007 15:16:4621-Feb-2007 23:27:30241016-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.cmodules\executor\aktuator.cmodules\wireless.cmodules\logic\logic.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.hmodules\executor\aktuator.hmodules\wireless.hmodules\logic\logic.hdefaultNOatmega128100RoboCode.elfdefault\1modules\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\executor\aktuator.hY:\Concept\Framework\modules\wireless.hY:\Concept\Framework\modules\logic\logic.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.cY:\Concept\Framework\modules\executor\aktuator.cY:\Concept\Framework\modules\wireless.cY:\Concept\Framework\modules\logic\logic.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.h100033modules\executor\aktuator.h100034modules\executor\aktuator.c100035modules\wireless.h100036modules\wireless.c100037atmega128io.c100038modules\logic\logic.c100039modules\logic\logic.h100040modules\output\kicker.c1291 77 1017 55825 18288 70 1022 55178 133044 32857 33644 331037 1333044 32857 33644 3310316 033044 32857 33644 3310317 133044 32857 33644 3310348 2332273 32087 32999 325428 033044 32857 33770 3331284 2733044 32857 33644 3310344 3332370 32229 32996 32530101 032282 32113 32912 3241823 732307 32144 32937 3244955 3032329 32173 32959 324781 032351 32202 32981 3250787 2432373 32231 33003 325369 032285 32115 32915 32420102 2032307 32144 32937 3244968 1632329 32173 32959 324781 032351 32202 32981 3250722 28285 68 1011 52368 2232285 32115 32915 324201 0279 64 1005 519227 1032329 32173 32959 3247834 2273 60 999 515125 332373 32231 33003 3253663 20291 72 1025 55330 22Maximized32307 32144 32937 324491 032329 32173 32959 324781 032351 32202 32981 3250763 10282 66 1008 521176 108286 96 916 4010 0308 125 938 4300 0336 158 952 460167 4361 189 977 49117 30383 218 999 5209 60405 247 1021 54948 21295 102 911 4041 0317 131 933 433227 4339 160 955 4628 3361 189 977 49118 26383 218 999 5200 0 +RoboCode16-Feb-2007 15:16:4622-Feb-2007 19:27:13241016-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.cmodules\executor\aktuator.cmodules\wireless.cmodules\logic\logic.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.hmodules\executor\aktuator.hmodules\wireless.hmodules\logic\logic.hdefaultNOatmega128100RoboCode.elfdefault\1modules\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\executor\aktuator.hY:\Concept\Framework\modules\wireless.hY:\Concept\Framework\modules\logic\logic.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.cY:\Concept\Framework\modules\executor\aktuator.cY:\Concept\Framework\modules\wireless.cY:\Concept\Framework\modules\logic\logic.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.h100033modules\executor\aktuator.h100034modules\executor\aktuator.c100035modules\wireless.h100036modules\wireless.c100037atmega128io.c100038modules\logic\logic.c100039modules\logic\logic.h100040modules\output\kicker.c1291 79 1017 56025 0282 66 1016 54768 033038 32853 33638 330997 033038 32853 33638 33099201 1033038 32853 33638 3309917 033038 32853 33638 3309943 4732267 32083 32993 325388 033038 32853 33764 3330884 033038 32853 33638 3309940 3332364 32225 32990 3252666 1832276 32109 32906 3241423 032301 32140 32931 3244555 032323 32169 32953 324741 032345 32198 32975 3250396 1632367 32227 32997 325329 032279 32111 32909 32416102 032301 32140 32931 3244568 032323 32169 32953 324741 0291 72 1025 55356 32Maximized279 64 1005 519199 1532279 32111 32909 324161 0273 60 999 515219 532323 32169 32953 3247427 0267 56 993 51131 4288 70 1022 55147 27285 68 1019 54952 3132301 32140 32931 324451 032323 32169 32953 324741 032345 32198 32975 3250363 0276 62 1002 517176 0280 92 910 3970 0302 121 932 4260 0330 154 946 456167 0355 185 971 48717 0377 214 993 51611 0399 243 1015 54548 0289 98 905 4001 0311 127 927 429657 43333 156 949 45823 19355 185 971 48728 7377 214 993 5161 0 diff --git a/source/Concept/Framework/atmega128io.h b/source/Concept/Framework/atmega128io.h index a3384a4..7f94c6c 100644 --- a/source/Concept/Framework/atmega128io.h +++ b/source/Concept/Framework/atmega128io.h @@ -6,6 +6,8 @@ #include #include +#include "defines.h" + /************************************************************************ Title: Interrupt UART library with receive/transmit circular buffers Author: Peter Fleury http://jump.to/fleury @@ -197,7 +199,7 @@ extern void uart1_puts_p(const char *s ); uint8 slave_adr; /*!< Slave Adresse and W/R byte */ uint8 size; /*!< Anzahl der Bytes, die gesendet oder empfagen werden sollen */ uint8 *data_ptr; /*!< Pointer zum Sende und Empfangs Puffer */ - }tx_type; + }tx_type; /*! * Hier wird der eigentliche TWI-Treiber angesprochen diff --git a/source/Concept/Framework/defines.h b/source/Concept/Framework/defines.h index 6e62143..9a7c12b 100644 --- a/source/Concept/Framework/defines.h +++ b/source/Concept/Framework/defines.h @@ -65,6 +65,8 @@ #define CYCLES_PER_CORRECTION 20 #define EMPTY_FLOAT 81188.1484f +#define BALL_HELD_INTENSITY 900 + #define WIRELESS_CODE "SPASS!" enum WirelessCommands { diff --git a/source/Concept/Framework/main.c b/source/Concept/Framework/main.c index 1875847..ffc4cc9 100755 --- a/source/Concept/Framework/main.c +++ b/source/Concept/Framework/main.c @@ -159,6 +159,8 @@ int main() //Debug code Display* ourDisplay = localRobot->GetModule(IO_DISPLAY_MAIN); ourDisplay->Clear(); + ourDisplay->Print("Roboter fertig gestartet",1,1); + sleep(1); IR_Sensor* ourSensor = NULL; @@ -175,51 +177,49 @@ int main() ourDribbler->SetSpeed(1); ourDribbler->SetEnabled(true); Aktuator* ourAktuator = localRobot->GetModule(IO_AKTUATOR_MAIN); - + Logic* ourLogic = localRobot->GetModule(IO_LOGIC_MAIN); + float rotation = 0; float speed = 200; //Mouse_Sensor* mouse_left = localRobot->GetModule(IO_SENSOR_MOUSE_LEFT); //Mouse_Sensor* mouse_right = localRobot->GetModule(IO_SENSOR_MOUSE_RIGHT); + ourDisplay->Clear(); //Run while(true) { ourDisplay->Print(i++,1,1); - msleep(100); + //msleep(50); - if(!(i % 1)) + if(!(i % 20)) { ourDisplay->Clear(); - //ourDisplay->Print(ourPosition_Tracker->GetPositionX(),1,2); - //ourDisplay->Print(ourPosition_Tracker->GetPositionY(),1,3); - //ourDisplay->Print(ourPosition_Tracker->GetOrientation() * 180.0f / PI,1,4); + + distanceSensor = localRobot->GetModule(IO_SENSOR_DISTANCE_0_DEG); + value = distanceSensor->GetDistance(); + ourDisplay->Print(value, 1, 4); + ourDisplay->Print(";"); + distanceSensor = localRobot->GetModule(IO_SENSOR_DISTANCE_90_DEG); + value = distanceSensor->GetDistance(); + ourDisplay->Print(value); + ourDisplay->Print(";"); + distanceSensor = localRobot->GetModule(IO_SENSOR_DISTANCE_180_DEG); + value = distanceSensor->GetDistance(); + ourDisplay->Print(value); + ourDisplay->Print(";"); + distanceSensor = localRobot->GetModule(IO_SENSOR_DISTANCE_270_DEG); + value = distanceSensor->GetDistance(); + ourDisplay->Print(value); + ourDisplay->Print(";"); } - if(!(i % 50)) + if(!(i % 20)) { - ourAktuator->Kick(); + //ourAktuator->Kick(); } - - - distanceSensor = localRobot->GetModule(IO_SENSOR_DISTANCE_0_DEG); - value = distanceSensor->GetDistance(); - ourDisplay->Print(value, 1, 2); - ourDisplay->Print(";"); - distanceSensor = localRobot->GetModule(IO_SENSOR_DISTANCE_90_DEG); - value = distanceSensor->GetDistance(); - ourDisplay->Print(value); - ourDisplay->Print(";"); - distanceSensor = localRobot->GetModule(IO_SENSOR_DISTANCE_180_DEG); - value = distanceSensor->GetDistance(); - ourDisplay->Print(value, 1, 3); - ourDisplay->Print(";"); - distanceSensor = localRobot->GetModule(IO_SENSOR_DISTANCE_270_DEG); - value = distanceSensor->GetDistance(); - ourDisplay->Print(value); - ourDisplay->Print(";"); uint8 someInput = ourKeyboard->GetInput(); //ourDisplay->Print("Ready to accept...", 1, 2); @@ -260,43 +260,55 @@ int main() ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker break; case 12: - ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker + ourLogic->SetKeeper(true); // Reset Position_Tracker break; } //ourDisplay->Clear(); - /*ourSensor = localRobot->GetModule(IO_SENSOR_IR_0_DEG); - value = ourSensor->GetIRIntensity(); - ourDisplay->Print(value, 1, 2); - ourDisplay->Print(";"); - ourSensor = localRobot->GetModule(IO_SENSOR_IR_30_DEG); - value = ourSensor->GetIRIntensity(); - ourDisplay->Print(value); - ourDisplay->Print(";"); - ourSensor = localRobot->GetModule(IO_SENSOR_IR_60_DEG); - value = ourSensor->GetIRIntensity(); - ourDisplay->Print(value); - ourDisplay->Print(";"); - ourSensor = localRobot->GetModule(IO_SENSOR_IR_100_DEG); - value = ourSensor->GetIRIntensity(); - ourDisplay->Print(value); - ourDisplay->Print(";"); - ourSensor = localRobot->GetModule(IO_SENSOR_IR_180_DEG); - value = ourSensor->GetIRIntensity(); - ourDisplay->Print(value, 1, 3); - ourDisplay->Print(";"); - ourSensor = localRobot->GetModule(IO_SENSOR_IR_260_DEG); - value = ourSensor->GetIRIntensity(); - ourDisplay->Print(value); - ourDisplay->Print(";"); - ourSensor = localRobot->GetModule(IO_SENSOR_IR_300_DEG); - value = ourSensor->GetIRIntensity(); - ourDisplay->Print(value); - ourDisplay->Print(";"); - ourSensor = localRobot->GetModule(IO_SENSOR_IR_330_DEG); - value = ourSensor->GetIRIntensity(); - ourDisplay->Print(value); - ourDisplay->Print(";");*/ + if(!(i % 20)) + { + /*ourDisplay->Clear(); + ourSensor = localRobot->GetModule(IO_SENSOR_IR_0_DEG); + value = ourSensor->GetIRIntensity(); + ourDisplay->Print(value, 1, 2); + ourDisplay->Print(";"); + ourSensor = localRobot->GetModule(IO_SENSOR_IR_30_DEG); + value = ourSensor->GetIRIntensity(); + ourDisplay->Print(value); + ourDisplay->Print(";"); + ourSensor = localRobot->GetModule(IO_SENSOR_IR_60_DEG); + value = ourSensor->GetIRIntensity(); + ourDisplay->Print(value); + ourDisplay->Print(";"); + ourSensor = localRobot->GetModule(IO_SENSOR_IR_100_DEG); + value = ourSensor->GetIRIntensity(); + ourDisplay->Print(value); + ourDisplay->Print(";"); + ourSensor = localRobot->GetModule(IO_SENSOR_IR_180_DEG); + value = ourSensor->GetIRIntensity(); + ourDisplay->Print(value, 1, 3); + ourDisplay->Print(";"); + ourSensor = localRobot->GetModule(IO_SENSOR_IR_260_DEG); + value = ourSensor->GetIRIntensity(); + ourDisplay->Print(value); + ourDisplay->Print(";"); + ourSensor = localRobot->GetModule(IO_SENSOR_IR_300_DEG); + value = ourSensor->GetIRIntensity(); + ourDisplay->Print(value); + ourDisplay->Print(";"); + ourSensor = localRobot->GetModule(IO_SENSOR_IR_330_DEG); + value = ourSensor->GetIRIntensity(); + ourDisplay->Print(value); + ourDisplay->Print(";"); + if(ourBallTracker->KnowsBallDirection()) + { + ourDisplay->Print(ourBallTracker->GetBallDirection() * 180.0f / PI, 1, 4); + } + else + { + ourDisplay->Print("Not found oder so", 1, 4); + }*/ + } localRobot->Update(); diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index 928a5ff..552c494 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -85,16 +85,37 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, } } + CalculateDirection(); +} + +void Navigator::RotateTo(float newAngle, float rotationSpeed) { + Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); + + this->rotationSpeed = min(rotationSpeed, 255.0f); + this->targetAngle = newAngle * PI / 180.0f; + + if(targetAngle - locationeer->GetOrientation() > PI) + { + if(rotationSpeed > 0) + { + rotationSpeed = -rotationSpeed; + } + } + else + { + if(rotationSpeed < 0) + { + rotationSpeed = -rotationSpeed; + } + } + CalculateDirection(); } - -//----------------------------------------------------------------------------- -void Navigator::Update() -{ + +bool Navigator::TargetReached() { 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) { @@ -106,6 +127,14 @@ void Navigator::Update() targetReached = true; } + return targetReached; +} + +bool Navigator::AngleReached() { + Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); + + bool targetAngleReached = false; + if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f) { targetAngle = EMPTY_FLOAT; @@ -113,15 +142,20 @@ void Navigator::Update() targetAngleReached = true; } - - if(targetReached && targetAngleReached) + return targetAngleReached; +} + +//----------------------------------------------------------------------------- +void Navigator::Update() +{ + if(TargetReached() && AngleReached()) { Stop(); } - else if(targetReached || targetAngleReached) + /*else if(HasTarget() && !TargetReached()) { CalculateDirection(); - } + }*/ if(!(correctionCountdown--)) { diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h index 81919f9..7f4b36d 100755 --- a/source/Concept/Framework/modules/executor/navigator.h +++ b/source/Concept/Framework/modules/executor/navigator.h @@ -49,6 +49,7 @@ public: void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed); void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed); + void RotateTo(float newAngle,float roationSpeed); void Rotate(float rotationSpeed); @@ -65,6 +66,9 @@ public: { return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT); } + + bool TargetReached(); + bool AngleReached(); }; #endif diff --git a/source/Concept/Framework/modules/input/distance_sensor.c b/source/Concept/Framework/modules/input/distance_sensor.c index 1ab3755..239e63b 100755 --- a/source/Concept/Framework/modules/input/distance_sensor.c +++ b/source/Concept/Framework/modules/input/distance_sensor.c @@ -1,22 +1,12 @@ -#include "distance_sensor.h" - -/*! - * SRF10 initialsieren - */ - -void Distance_Sensor::srf10_init(void){ - srf10_set_range(SRF10_MAX_RANGE); - //srf10_set_range(6); //Mit diesem Wert muss man spielen um das Optimum zu ermitteln -return; -} - -/*! - * Verstaerkungsfaktor setzen - * @param gain Verstaerkungsfaktor - */ - -void Distance_Sensor::srf10_set_gain(unsigned char gain){ - if(gain>16) { gain=16; } +#include "distance_sensor.h" + +//----------------------------------------------------------------------------- +void Distance_Sensor::SetSignalFactor(uint8 factor) +{ + if(factor > 16) + { + factor = 16; + } uint8 temp[2]; uint8 state; @@ -24,30 +14,59 @@ void Distance_Sensor::srf10_set_gain(unsigned char gain){ state = SUCCESS; - tx_frame[0].slave_adr = this->slaveAddr+W; + tx_frame[0].slave_adr = this->slaveAddr + W; tx_frame[0].size = 2; tx_frame[0].data_ptr = temp; tx_frame[0].data_ptr[0] = 1; - tx_frame[0].data_ptr[1] = gain; + tx_frame[0].data_ptr[1] = factor; tx_frame[1].slave_adr = OWN_ADR; + state = Send_to_TWI(tx_frame); +} + +//----------------------------------------------------------------------------- +void Distance_Sensor::SetSlaveAddress(uint8 newSlaveAddress) +{ + uint8 temp[2]; + uint8 state; + tx_type tx_frame[2]; + + state = SUCCESS; + + tx_frame[0].slave_adr = this->slaveAddr + W; + tx_frame[0].size = 2; + tx_frame[0].data_ptr = temp; + tx_frame[0].data_ptr[0] = 0; + tx_frame[0].data_ptr[1] = 160; + tx_frame[1].slave_adr = OWN_ADR; + state = Send_to_TWI(tx_frame); + + msleep(60); + + tx_frame[0].data_ptr[1] = 170; + state = Send_to_TWI(tx_frame); + + msleep(60); + + tx_frame[0].data_ptr[1] = 165; + state = Send_to_TWI(tx_frame); + + msleep(60); + + tx_frame[0].data_ptr[1] = newSlaveAddress; state = Send_to_TWI(tx_frame); } -/*! - * Reichweite setzen, hat auch Einfluss auf die Messdauer - * @param millimeters Reichweite in mm - */ - -void Distance_Sensor::srf10_set_range(unsigned int millimeters){ +//----------------------------------------------------------------------------- +void Distance_Sensor::SetRange(unsigned int millimeters){ uint8 temp[2]; uint8 state; tx_type tx_frame[2]; state = SUCCESS; - millimeters= (millimeters/43); + millimeters = (millimeters/43); tx_frame[0].slave_adr = this->slaveAddr+W; tx_frame[0].size = 2; @@ -85,14 +104,9 @@ uint8 Distance_Sensor::srf10_ping(uint8 metric_unit){ return state; } - -/*! - * Register auslesen - * @param srf10_register welches Register soll ausgelsen werden - * @return Inhalt des Registers - */ - -uint8 Distance_Sensor::srf10_read_register(uint8 srf10_register){ + +//----------------------------------------------------------------------------- +uint8 Distance_Sensor::ReadRegister(uint8 registerToRead){ uint8 temp; uint8 value; uint8 state; @@ -104,7 +118,7 @@ uint8 Distance_Sensor::srf10_read_register(uint8 srf10_register){ tx_frame[0].slave_adr = this->slaveAddr+W; tx_frame[0].size = 1; tx_frame[0].data_ptr = &temp; - tx_frame[0].data_ptr[0] = srf10_register; + tx_frame[0].data_ptr[0] = registerToRead; tx_frame[1].slave_adr = this->slaveAddr+R; tx_frame[1].size = 1; @@ -130,10 +144,10 @@ uint16 Distance_Sensor::srf10_get_measure(){ state = SUCCESS; state = srf10_ping(SRF10_CENTIMETERS); - usleep(10); //Optimierungs Potential - lob=srf10_read_register(SRF10_LOB); - usleep(10); //Optimierungs Potential - hib=srf10_read_register(SRF1sr0_HIB); + msleep(10); //Optimierungs Potential + lob=ReadRegister(SRF10_LOB); + msleep(10); //Optimierungs Potential + hib=ReadRegister(SRF10_HIB); return (hib*256)+lob; } diff --git a/source/Concept/Framework/modules/input/distance_sensor.h b/source/Concept/Framework/modules/input/distance_sensor.h index 9fb2c58..0bb2baf 100755 --- a/source/Concept/Framework/modules/input/distance_sensor.h +++ b/source/Concept/Framework/modules/input/distance_sensor.h @@ -9,9 +9,9 @@ #define SRF10_MIN_RANGE 0 /*!< Minimale Reichweite 43mm */ #define SRF10_MAX_RANGE 5977 /*!< Maximale Reichweite 5977mm */ -#define SRF10_INCHES 0X50 /*!< Messung in INCHES */ -#define SRF10_CENTIMETERS 0X51 /*!< Messung in CM */ -#define SRF10_MICROSECONDS 0X52 /*!< Messung in Millisekunden */ +#define SRF10_INCHES 0x50 /*!< Messung in INCHES */ +#define SRF10_CENTIMETERS 0x51 /*!< Messung in CM */ +#define SRF10_MICROSECONDS 0x52 /*!< Messung in Millisekunden */ #define SRF10_COMMAND 0 /*!< W=Befehls-Register R=Firmware*/ #define SRF10_LIGHT 1 /*!< W=Verstaerkungsfaktor R=Nicht benutzt */ @@ -50,55 +50,44 @@ public: default: this->slaveAddr = 0; break; - } - - // initialiate the sensor - srf10_init(); + } + + //SetRange(100); + SetRange(2000); + SetSignalFactor(0x06); } protected: //Hardware - slaveAddr; - - /*! - * SRF10 initialsieren - */ -extern void srf10_init(void); - -/*! - * Verstaerkungsfaktor setzen - * @param gain Verstaerkungsfaktor - */ -extern void srf10_set_gain(uint8 gain); - -/*! - * Reichweite setzen, hat auch Einfluss auf die Messdauer - * @param millimeters Reichweite in mm - */ -extern void srf10_set_range(uint16 millimeters); + uint8 slaveAddr; + + void SetSignalFactor(uint8 factor); + + void SetRange(uint16 millimeters); /*! * Messung ausloesen * @param metric_unit 0x50 in Zoll, 0x51 in cm, 0x52 ms * @return Resultat der Aktion */ -extern uint8 srf10_ping(uint8 metric_unit); + uint8 srf10_ping(uint8 metric_unit); /*! * Register auslesen * @param srf10_register welches Register soll ausgelsen werden * @return Inhalt des Registers */ -extern uint8 srf10_read_register(uint8 SRF10_register); + uint8 ReadRegister(uint8 registerToRead); /*! * Messung starten Ergebniss aufbereiten und zurueckgeben * @return Messergebniss */ -extern uint16 srf10_get_measure(void); + uint16 srf10_get_measure(void); public: - float GetDistance(); + uint16 GetDistance(); + void SetSlaveAddress(uint8 newSlaveAddress); }; #endif diff --git a/source/Concept/Framework/modules/input/ir_sensor.h b/source/Concept/Framework/modules/input/ir_sensor.h index 74396ec..c8f692b 100755 --- a/source/Concept/Framework/modules/input/ir_sensor.h +++ b/source/Concept/Framework/modules/input/ir_sensor.h @@ -34,11 +34,11 @@ public: break; case IO_SENSOR_IR_100_DEG: this->channel = 3; - this->intensityCorrection = 40; + this->intensityCorrection = 80; break; case IO_SENSOR_IR_180_DEG: this->channel = 4; - this->intensityCorrection = 50; + this->intensityCorrection = 70; break; case IO_SENSOR_IR_260_DEG: this->channel = 5; diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.c b/source/Concept/Framework/modules/interpreter/ball_tracker.c index 2d85b96..16fdfb4 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.c +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.c @@ -21,6 +21,21 @@ void Ball_Tracker::Update() { greatestIntensity = i; } + + if(i == 0) + { + if(intensity[i] > BALL_HELD_INTENSITY) // Ball derzeit sehr nah dran + { + ballHeld = true; + ballHeldCounter = 100; + } + else if(ballHeldCounter > 0) // Oder vor kurzem erst sehr nah dran + { + ballHeld = true; + ballHeldCounter--; + } + else ballHeld = false; // ansonsten hat er den Ball nicht + } } if(intensity[greatestIntensity]) diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.h b/source/Concept/Framework/modules/interpreter/ball_tracker.h index c62f05e..cb90ff2 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.h +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.h @@ -11,6 +11,8 @@ public: this->parent = NULL; this->moduleId = 0; this->direction = EMPTY_FLOAT; + this->ballHeldCounter = 0; + this->ballHeld = false; } Ball_Tracker(uint32 trackerId) @@ -18,10 +20,14 @@ public: this->parent = NULL; this->moduleId = trackerId; this->direction = EMPTY_FLOAT; + this->ballHeldCounter = 0; + this->ballHeld = false; } protected: float direction; + uint8 ballHeldCounter; + bool ballHeld; public: void Update(); @@ -33,12 +39,12 @@ public: bool KnowsBallDirection() { - return direction != EMPTY_FLOAT; + return (direction != EMPTY_FLOAT); } bool RobotHasBall() { - //fill me! + return ballHeld; } }; diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c index a64ab60..f6d67ac 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.c +++ b/source/Concept/Framework/modules/interpreter/position_tracker.c @@ -10,6 +10,10 @@ void Position_Tracker::Update() // Generate a vector for the left mouse int8 leftX = mouseLeft->GetXMovement(); int8 leftY = mouseLeft->GetYMovement(); + // Generate a vector for the right mouse + int8 rightX = mouseRight->GetXMovement(); + int8 rightY = mouseRight->GetYMovement(); + float distanceLeft = sqrt(leftX * leftX + leftY * leftY); float angleLeft = easyAngle(atan2(leftY, leftX) + (225.0f * PI / 180.0f)); @@ -23,9 +27,6 @@ void Position_Tracker::Update() movementLeftY = 0; } - // Generate a vector for the right mouse - 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)); @@ -50,13 +51,9 @@ void Position_Tracker::Update() float robotMovementY = movementDifferenceY / 2.0f; robotMovementX += movementLeftX + mouseLeft->GetPositionX() + (-mouseLeft->GetPositionX() * cos(orientationDiff)); robotMovementY += movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff)); - //float robotDistance = sqrt(robotMovementX * robotMovementX + robotMovementY * robotMovementY); - - float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotMovementX - sin(this->orientation + (orientationDiff / 2.0f)) * robotMovementY; - float absoluteDiffY = sin(this->orientation + (orientationDiff / 2.0f) + PI / 2.0f) * robotMovementY + cos(this->orientation + (orientationDiff / 2.0f) - PI / 2.0f) * robotMovementX; - //float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementX); - //float absoluteDiffY = sin(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementY); + float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotMovementX + sin(this->orientation + (orientationDiff / 2.0f)) * robotMovementY; + float absoluteDiffY = sin(this->orientation + (orientationDiff / 2.0f) + PI / 2.0f) * robotMovementY - cos(this->orientation + (orientationDiff / 2.0f) - PI / 2.0f) * robotMovementX; if(!robotMovementX && !robotMovementY) { @@ -74,4 +71,10 @@ void Position_Tracker::Update() this->orientation += orientationDiff; this->orientation = easyAngle(this->orientation); + + //(parent->GetModule(IO_DISPLAY_MAIN))->Print(" ", 5, 1); + /*(parent->GetModule(IO_DISPLAY_MAIN))->Clear(); + (parent->GetModule(IO_DISPLAY_MAIN))->Print(this->orientation * 180.0f / PI, 5, 1); + (parent->GetModule(IO_DISPLAY_MAIN))->Print(this->positionX, 1, 2); + (parent->GetModule(IO_DISPLAY_MAIN))->Print(this->positionY, 1, 3);*/ } diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h index 47d0740..56b16c4 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.h +++ b/source/Concept/Framework/modules/interpreter/position_tracker.h @@ -51,8 +51,8 @@ public: // returns orientation float GetOrientation() { - return 0.0f; //tmp!!!!!!!!!! - //return orientation; + //return 0.0f; //tmp!!!!!!!!!! + return orientation; } }; diff --git a/source/Concept/Framework/robot.c b/source/Concept/Framework/robot.c index 9985463..a630d4a 100755 --- a/source/Concept/Framework/robot.c +++ b/source/Concept/Framework/robot.c @@ -92,6 +92,8 @@ void Robot::Update() GetModule(IO_POSITION_TRACKER_MAIN)->Update(); + GetModule(IO_LOGIC_MAIN)->Update(); + GetModule(IO_NAVIGATOR_MAIN)->Update(); //insert code here -- cgit v1.2.3