From f61eb900879fe941460a284ff9e4681b0958b944 Mon Sep 17 00:00:00 2001 From: sicarius Date: Mon, 26 Feb 2007 21:25:01 +0000 Subject: SoccerTeam, Stuff from Magdeburg --- source/Concept/Framework/RoboCode.aps | 2 +- source/Concept/Framework/default/Makefile | 10 +- source/Concept/Framework/defines.h | 46 +++- source/Concept/Framework/main.c | 165 +++++++------ .../Concept/Framework/modules/executor/navigator.c | 6 +- .../Concept/Framework/modules/executor/navigator.h | 15 ++ .../Concept/Framework/modules/input/mouse_sensor.h | 10 + .../Framework/modules/interpreter/ball_tracker.c | 15 +- .../Framework/modules/interpreter/ball_tracker.h | 7 +- .../modules/interpreter/command_handler.c | 170 +++++++++++++- .../modules/interpreter/command_handler.h | 17 +- .../modules/interpreter/position_tracker.c | 16 ++ .../modules/interpreter/position_tracker.h | 14 +- source/Concept/Framework/modules/logic/logic.c | 257 +++++++++++++++++++-- source/Concept/Framework/modules/logic/logic.h | 23 +- source/Concept/Framework/robot.c | 8 +- source/Concept/Framework/stdafx.h | 1 + 17 files changed, 663 insertions(+), 119 deletions(-) diff --git a/source/Concept/Framework/RoboCode.aps b/source/Concept/Framework/RoboCode.aps index ca29afc..2219005 100644 --- a/source/Concept/Framework/RoboCode.aps +++ b/source/Concept/Framework/RoboCode.aps @@ -1 +1 @@ -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 +RoboCode16-Feb-2007 15:16:4624-Feb-2007 18:46:56241016-Feb-2007 15:16:4644, 12, 0, 462AVR GCCdefault\RoboCode.elfZ:\rc2007-soccer\source\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.cmodules\interpreter\command_handler.cmodules\interpreter\obstacle_tracker.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.hmodules\interpreter\command_handler.hmodules\interpreter\obstacle_tracker.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:\Programme\dev\WinAVR\bin\avr-g++.exeC:\Programme\dev\WinAVR\utils\bin\make.exeZ:\rc2007-soccer\source\Concept\Framework\stdafx.hZ:\rc2007-soccer\source\Concept\Framework\tools.hZ:\rc2007-soccer\source\Concept\Framework\atmega128io.hZ:\rc2007-soccer\source\Concept\Framework\defines.hZ:\rc2007-soccer\source\Concept\Framework\robot.hZ:\rc2007-soccer\source\Concept\Framework\modules\input\distance_sensor.hZ:\rc2007-soccer\source\Concept\Framework\modules\input\ir_sensor.hZ:\rc2007-soccer\source\Concept\Framework\modules\input\keyboard.hZ:\rc2007-soccer\source\Concept\Framework\modules\input\mouse_sensor.hZ:\rc2007-soccer\source\Concept\Framework\modules\input\sensor.hZ:\rc2007-soccer\source\Concept\Framework\modules\output\display.hZ:\rc2007-soccer\source\Concept\Framework\modules\output\dribbler.hZ:\rc2007-soccer\source\Concept\Framework\modules\output\engine.hZ:\rc2007-soccer\source\Concept\Framework\modules\output\kicker.hZ:\rc2007-soccer\source\Concept\Framework\modules\output\led.hZ:\rc2007-soccer\source\Concept\Framework\modules\interpreter\position_tracker.hZ:\rc2007-soccer\source\Concept\Framework\modules\io_module.hZ:\rc2007-soccer\source\Concept\Framework\modules\interpreter\ball_tracker.hZ:\rc2007-soccer\source\Concept\Framework\modules\executor\navigator.hZ:\rc2007-soccer\source\Concept\Framework\modules\executor\aktuator.hZ:\rc2007-soccer\source\Concept\Framework\modules\wireless.hZ:\rc2007-soccer\source\Concept\Framework\modules\logic\logic.hZ:\rc2007-soccer\source\Concept\Framework\modules\interpreter\command_handler.hZ:\rc2007-soccer\source\Concept\Framework\modules\interpreter\obstacle_tracker.hZ:\rc2007-soccer\source\Concept\Framework\modules\io_module.cZ:\rc2007-soccer\source\Concept\Framework\atmega128io.cZ:\rc2007-soccer\source\Concept\Framework\main.cZ:\rc2007-soccer\source\Concept\Framework\robot.cZ:\rc2007-soccer\source\Concept\Framework\tools.cZ:\rc2007-soccer\source\Concept\Framework\modules\input\distance_sensor.cZ:\rc2007-soccer\source\Concept\Framework\modules\input\ir_sensor.cZ:\rc2007-soccer\source\Concept\Framework\modules\input\keyboard.cZ:\rc2007-soccer\source\Concept\Framework\modules\input\mouse_sensor.cZ:\rc2007-soccer\source\Concept\Framework\modules\input\sensor.cZ:\rc2007-soccer\source\Concept\Framework\modules\interpreter\position_tracker.cZ:\rc2007-soccer\source\Concept\Framework\modules\output\display.cZ:\rc2007-soccer\source\Concept\Framework\modules\output\dribbler.cZ:\rc2007-soccer\source\Concept\Framework\modules\output\engine.cZ:\rc2007-soccer\source\Concept\Framework\modules\output\kicker.cZ:\rc2007-soccer\source\Concept\Framework\modules\output\led.cZ:\rc2007-soccer\source\Concept\Framework\modules\interpreter\ball_tracker.cZ:\rc2007-soccer\source\Concept\Framework\modules\executor\navigator.cZ:\rc2007-soccer\source\Concept\Framework\modules\executor\aktuator.cZ:\rc2007-soccer\source\Concept\Framework\modules\wireless.cZ:\rc2007-soccer\source\Concept\Framework\modules\logic\logic.cZ:\rc2007-soccer\source\Concept\Framework\modules\interpreter\command_handler.cZ:\rc2007-soccer\source\Concept\Framework\modules\interpreter\obstacle_tracker.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.c100032modules\executor\aktuator.h100033modules\executor\aktuator.c100034modules\wireless.h100035modules\wireless.c100036atmega128io.c100037modules\logic\logic.c100038modules\logic\logic.h100039modules\output\kicker.c100040modules\interpreter\command_handler.c100041modules\interpreter\command_handler.h100042modules\interpreter\obstacle_tracker.h100043modules\interpreter\obstacle_tracker.c100044modules\input\keyboard.c100045modules\output\led.c1376 84 1102 56524 0352 56 1086 53764 032997 32843 33597 330897 032997 32843 33597 33089201 032997 32843 33597 3308917 032997 32843 33597 3308955 032337 32073 33063 325288 032997 32843 33723 33298189 032997 32843 33597 3308940 032434 32215 33060 3251666 032346 32099 32976 3240423 032371 32130 33001 3243555 032393 32159 33023 324641 032415 32188 33045 3249398 032437 32217 33067 325229 032349 32101 32979 32406102 032371 32130 33001 3243568 032393 32159 33023 324641 0361 62 1095 54356 0349 54 1075 50951 032349 32101 32979 324061 0232 72 1025 550230 69Maximized32393 32159 33023 3246427 0337 46 1063 501139 0358 60 1092 54179 0355 58 1089 53952 032371 32130 33001 324351 032393 32159 33023 324641 032415 32188 33045 3249381 0346 52 1072 50728 0350 82 980 3870 0372 111 1002 4160 0400 144 1016 44627 0425 175 1041 47711 0447 204 1063 50640 0469 233 1085 5351 0359 88 975 3901 0381 117 997 41928 0403 146 1019 44830 0425 175 1041 4771 0447 204 1063 506160 0363 66 873 37953 0230 98 874 41540 0252 127 896 44483 0274 156 918 4731 0299 187 974 4861 0 diff --git a/source/Concept/Framework/default/Makefile b/source/Concept/Framework/default/Makefile index 0411c61..e34755b 100644 --- a/source/Concept/Framework/default/Makefile +++ b/source/Concept/Framework/default/Makefile @@ -34,13 +34,13 @@ 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" +INCLUDES = -I"Z:\rc2007-soccer\source\Concept\Framework\modules" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\executor" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\input" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\interpreter" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\logic" -I"Z:\rc2007-soccer\source\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 aktuator.o wireless.o logic.o +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 aktuator.o wireless.o logic.o command_handler.o obstacle_tracker.o ## Objects explicitly added by the user LINKONLYOBJECTS = @@ -112,6 +112,12 @@ wireless.o: ../modules/wireless.c logic.o: ../modules/logic/logic.c $(CC) $(INCLUDES) $(CFLAGS) -c $< +command_handler.o: ../modules/interpreter/command_handler.c + $(CC) $(INCLUDES) $(CFLAGS) -c $< + +obstacle_tracker.o: ../modules/interpreter/obstacle_tracker.c + $(CC) $(INCLUDES) $(CFLAGS) -c $< + ##Link $(TARGET): $(OBJECTS) $(CC) $(LDFLAGS) $(OBJECTS) $(LINKONLYOBJECTS) $(LIBDIRS) $(LIBS) -o $(TARGET) diff --git a/source/Concept/Framework/defines.h b/source/Concept/Framework/defines.h index 596126d..a86282c 100644 --- a/source/Concept/Framework/defines.h +++ b/source/Concept/Framework/defines.h @@ -60,6 +60,9 @@ #define DISTANCE_PER_VALUE 1 #define TICKS_PER_CM 205.0f //#define TICKS_PER_CM 90.0f +#define ULTRASONIC_PER_CM 2.0f + +#define WIRELESS_ACTIVE false #define PI 3.14159265358979323846f #define CYCLES_PER_CORRECTION 20 @@ -67,8 +70,39 @@ #define BALL_HELD_INTENSITY 900 +#define KEEPER_LEFT_ANGLE 20.0f * PI / 180.0f +#define KEEPER_RIGHT_ANGLE (2 * PI) - (20.0f * PI / 180.0f) + #define COMMAND_BUFFER_SIZE 20 +#define DEFAULT_SPEED 200 +#define DEFAULT_ROTATION_SPEED 200 + +#define AREA_BOUNDS_X 20000.0f +#define AREA_BOUNDS_Y 5000.0f + +#define WALL_DISTANCE_TOLERANCE 30.0f +#define ENEMY_ROBOT_RADIUS 40.0f + +#define HOME_LOC_X -500.0f +#define HOME_LOC_Y 0 +#define ENEMY_LOC_X 500.0f +#define ENEMY_LOC_Y 0 +#define DEFENCE_L_LOC_X -500.0f +#define DEFENCE_L_LOC_Y 70.0f +#define DEFENCE_R_LOC_X -500.0f +#define DEFENCE_R_LOC_Y -70.0f + +#define DISTANCE_TOLERANCE 1.0f +#define ORIENTATION_TOLERANCE 0.1f + +#define MAX_OBSTACLE_COUNT 20 + +#define OBSTACLE_DECAY 100 + +#define BALL_DIRECTION_FRONT_L 45.0f * PI / 180.0f +#define BALL_DIRECTION_FRONT_R 315.0f * PI / 180.0f + #define WIRELESS_CODE "SPASS!" enum WirelessCommands { @@ -183,9 +217,17 @@ enum IOModuleNames IO_POSITION_TRACKER_END, - //Position Tracker + //Obstacle Tracker + + IO_OBSTACLE_TRACKER_START = IO_POSITION_TRACKER_END, + + IO_OBSTACLE_TRACKER_MAIN, + + IO_OBSTACLE_TRACKER_END, + + //Ball Tracker - IO_BALL_TRACKER_START = IO_POSITION_TRACKER_END, + IO_BALL_TRACKER_START = IO_OBSTACLE_TRACKER_END, IO_BALL_TRACKER_MAIN, diff --git a/source/Concept/Framework/main.c b/source/Concept/Framework/main.c index ba0e280..27edce3 100755 --- a/source/Concept/Framework/main.c +++ b/source/Concept/Framework/main.c @@ -130,6 +130,14 @@ int main() newCommandHandler = NULL; } + //Init Obstacle Tracker + for(uint8 i = IO_OBSTACLE_TRACKER_START; i < IO_OBSTACLE_TRACKER_END; i++) + { + Obstacle_Tracker* newObstacleTracker = new Obstacle_Tracker(i); + localRobot->AddModule(newObstacleTracker); + newObstacleTracker = NULL; + } + //Init Position Tracker for(uint8 i = IO_POSITION_TRACKER_START; i < IO_POSITION_TRACKER_END; i++) { @@ -179,33 +187,62 @@ int main() Command_Handler* ourCommandHandler = localRobot->GetModule(IO_COMMAND_HANDLER_MAIN); uint32 i = 1; Navigator* ourNavigator = localRobot->GetModule(IO_NAVIGATOR_MAIN); - Position_Tracker* ourPosition_Tracker = localRobot->GetModule(IO_POSITION_TRACKER_MAIN); + Position_Tracker* ourPositionTracker = localRobot->GetModule(IO_POSITION_TRACKER_MAIN); Ball_Tracker* ourBallTracker = localRobot->GetModule(IO_BALL_TRACKER_MAIN); Dribbler* ourDribbler = localRobot->GetModule(IO_DRIBBLER_MAIN); - ourDribbler->SetSpeed(1); - ourDribbler->SetEnabled(true); + //ourDribbler->SetSpeed(1); + //ourDribbler->SetEnabled(true); Aktuator* ourAktuator = localRobot->GetModule(IO_AKTUATOR_MAIN); Logic* ourLogic = localRobot->GetModule(IO_LOGIC_MAIN); + Wireless* ourWireless = localRobot->GetModule(IO_WIRELESS_MAIN); - float rotation = 0; - float speed = 200; + //(localRobot->GetModule(IO_ENGINE_DRIVE_BACK))->SetSpeed(200); - //Mouse_Sensor* mouse_left = localRobot->GetModule(IO_SENSOR_MOUSE_LEFT); - //Mouse_Sensor* mouse_right = localRobot->GetModule(IO_SENSOR_MOUSE_RIGHT); + //(localRobot->GetModule(IO_ENGINE_DRIVE_LEFT))->SetSpeed(200); + //(localRobot->GetModule(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true); - ourDisplay->Clear(); + //sleep(2); + + //(localRobot->GetModule(IO_ENGINE_DRIVE_RIGHT))->SetSpeed(200); + //(localRobot->GetModule(IO_ENGINE_DRIVE_RIGHT))->SetEnabled(true); + + //sleep(2); + + + + //(localRobot->GetModule(IO_ENGINE_DRIVE_LEFT))->SetSpeed(200); + //(localRobot->GetModule(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true); + + /*(localRobot->GetModule(IO_DRIBBLER_MAIN))->SetSpeed(200); + (localRobot->GetModule(IO_DRIBBLER_MAIN))->SetEnabled(true);*/ + + /*(localRobot->GetModule(IO_ENGINE_DRIVE_RIGHT))->SetSpeed(80); + (localRobot->GetModule(IO_ENGINE_DRIVE_RIGHT))->SetEnabled(true);*/ + + (localRobot->GetModule(IO_ENGINE_DRIVE_BACK))->SetSpeed(255); + (localRobot->GetModule(IO_ENGINE_DRIVE_BACK))->SetEnabled(true); + + (localRobot->GetModule(IO_ENGINE_DRIVE_LEFT))->SetSpeed(255); + (localRobot->GetModule(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true); + + sleep(2); //Run while(true) { - ourDisplay->Print(i++,1,1); - - //msleep(50); - - if(ourCommandHandler->displayDistanceSensors && !(i % 20)) + i++; + + /*if(!(i % 20)) { ourDisplay->Clear(); + msleep(10); + ourCommandHandler->PrintCommand(); + } + + localRobot->Update(); + if(ourCommandHandler->displayDistanceSensors && !(i % 20)) + { distanceSensor = localRobot->GetModule(IO_SENSOR_DISTANCE_0_DEG); value = distanceSensor->GetDistance(); ourDisplay->Print(value, 1, 4); @@ -223,69 +260,43 @@ int main() ourDisplay->Print(value); ourDisplay->Print(";"); } - - /*uint8 someInput = ourKeyboard->GetInput(); - //ourDisplay->Print("Ready to accept...", 1, 2); - switch(someInput) + + if(ourCommandHandler->ticksPerCmOffset && !(i % 20)) { - case 0: - ourNavigator->Stop(); - return 0; - break; - case 1: - ourNavigator->Drive(225, rotation, speed, rotation); - break; - case 2: - ourNavigator->Drive(180, rotation, speed, rotation); - break; - case 3: - ourNavigator->Drive(135, rotation, speed, rotation); - break; - case 4: - ourNavigator->Drive(270, rotation, speed, rotation); - break; - case 5: - ourNavigator->Drive(0, rotation, 0, rotation); - break; - case 6: - ourNavigator->Drive(90, rotation, speed, rotation); - break; - case 7: - ourNavigator->Drive(315, rotation, speed, rotation); - break; - case 8: - ourNavigator->Drive(0, rotation, speed, rotation); - break; - case 9: - ourNavigator->Drive(45, rotation, speed, rotation); - break; - case 10: - ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker - break; - case 11: - ourLogic->SetKeeper(true); // Reset Position_Tracker - break; - }*/ - - //ourDisplay->Clear(); - if(!(i % 20)) + ourDisplay->Print(ourCommandHandler->ticksPerCmOffset * 10.0f, 10, 1); + } + + if(ourCommandHandler->displayPositionTracker && !(i % 20)) + { + ourDisplay->Print(ourPositionTracker->GetPositionX(), 1, 2); + ourDisplay->Print(ourPositionTracker->GetPositionY(), 1, 3); + ourDisplay->Print(ourPositionTracker->GetOrientation(), 1, 4); + } + + if(ourCommandHandler->displayPositionTracker && !(i % 20)) + { + ourDisplay->Print(ourPositionTracker->GetPositionX(), 1, 2); + ourDisplay->Print(ourPositionTracker->GetPositionY(), 1, 3); + ourDisplay->Print(ourPositionTracker->GetOrientation() * 180.0f / PI, 1, 4); + } + + if(ourCommandHandler->displayBallTracker && !(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(value, 6, 2); ourDisplay->Print(";"); ourSensor = localRobot->GetModule(IO_SENSOR_IR_60_DEG); value = ourSensor->GetIRIntensity(); - ourDisplay->Print(value); + ourDisplay->Print(value, 11, 2); ourDisplay->Print(";"); ourSensor = localRobot->GetModule(IO_SENSOR_IR_100_DEG); value = ourSensor->GetIRIntensity(); - ourDisplay->Print(value); + ourDisplay->Print(value, 16, 2); ourDisplay->Print(";"); ourSensor = localRobot->GetModule(IO_SENSOR_IR_180_DEG); value = ourSensor->GetIRIntensity(); @@ -293,15 +304,15 @@ int main() ourDisplay->Print(";"); ourSensor = localRobot->GetModule(IO_SENSOR_IR_260_DEG); value = ourSensor->GetIRIntensity(); - ourDisplay->Print(value); + ourDisplay->Print(value, 6, 3); ourDisplay->Print(";"); ourSensor = localRobot->GetModule(IO_SENSOR_IR_300_DEG); value = ourSensor->GetIRIntensity(); - ourDisplay->Print(value); + ourDisplay->Print(value, 11, 3); ourDisplay->Print(";"); ourSensor = localRobot->GetModule(IO_SENSOR_IR_330_DEG); value = ourSensor->GetIRIntensity(); - ourDisplay->Print(value); + ourDisplay->Print(value, 16, 3); ourDisplay->Print(";"); if(ourBallTracker->KnowsBallDirection()) { @@ -309,15 +320,27 @@ int main() } else { - ourDisplay->Print("Not found oder so", 1, 4); - }*/ + ourDisplay->Print("Ball not found", 1, 4); + } + + if(ourBallTracker->HasBall()) { + ourDisplay->Print("Has Ball", 5, 4); + } } - - localRobot->Update(); - //ourDisplay->Print(ourBallTracker->GetBallDirection() * 180.0f / PI, 1, 4); + //if(!(i % 20)) + //{ + // ourDisplay->Print(ourWireless->Recieve(), 10, 1); + //} - //ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0); + if(ourBallTracker->KnowsBallDirection()) + { + ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0); + } + else + { + ourNavigator->Stop(); + }*/ } //Cleanup diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index 7c4618a..8320eca 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -59,7 +59,7 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float //----------------------------------------------------------------------------- void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed) { - if(newX < 0 || newY < 0) return; + if(newX == EMPTY_FLOAT || newY == EMPTY_FLOAT) return; Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); @@ -125,7 +125,7 @@ bool Navigator::TargetReached() bool targetReached = false; - if(!HasTarget() || (distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)) + if(!HasTarget() || (distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < DISTANCE_TOLERANCE)) { targetReached = true; } @@ -230,7 +230,7 @@ void Navigator::CalculateEngines() vBack = ((float)vBack * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed; vRight = ((float)vRight * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed; - //(parent->GetModule(IO_DISPLAY_MAIN))->Print(vLeft,10,2); + (parent->GetModule(IO_DISPLAY_MAIN))->Print(vBack,10,2); //(parent->GetModule(IO_DISPLAY_MAIN))->Print(vBack,10,3); //(parent->GetModule(IO_DISPLAY_MAIN))->Print(vRight,10,4); diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h index 53fa74b..989bc04 100755 --- a/source/Concept/Framework/modules/executor/navigator.h +++ b/source/Concept/Framework/modules/executor/navigator.h @@ -67,6 +67,21 @@ public: return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT); } + float GetTargetX() + { + return targetX; + } + + float GetTargetY() + { + return targetY; + } + + float GetDirection() + { + return direction; + } + bool HasTargetAngle() { return (targetAngle != EMPTY_FLOAT); diff --git a/source/Concept/Framework/modules/input/mouse_sensor.h b/source/Concept/Framework/modules/input/mouse_sensor.h index b3d1b1a..1f2922a 100755 --- a/source/Concept/Framework/modules/input/mouse_sensor.h +++ b/source/Concept/Framework/modules/input/mouse_sensor.h @@ -214,6 +214,16 @@ public: { return positionY; } + + void AdjustPositionX(float newPosX) + { + this->positionX = newPosX; + } + + void AdjustPositionY(float newPosY) + { + this->positionY = newPosY; + } }; #endif diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.c b/source/Concept/Framework/modules/interpreter/ball_tracker.c index 16fdfb4..a882abb 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.c +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.c @@ -3,6 +3,8 @@ //----------------------------------------------------------------------------- void Ball_Tracker::Update() { + Position_Tracker* ourPositionTracker = parent->GetModule(IO_POSITION_TRACKER_MAIN); + uint8 sensorCount = (IO_SENSOR_IR_330_DEG - IO_SENSOR_IR_0_DEG) + 1; uint16 intensity[sensorCount]; uint8 greatestIntensity = 0; @@ -26,15 +28,12 @@ void Ball_Tracker::Update() { if(intensity[i] > BALL_HELD_INTENSITY) // Ball derzeit sehr nah dran { - ballHeld = true; - ballHeldCounter = 100; + if(ballHeldCounter < 10) ballHeldCounter++; } - else if(ballHeldCounter > 0) // Oder vor kurzem erst sehr nah dran + else if(ballHeldCounter > 0) { - ballHeld = true; ballHeldCounter--; } - else ballHeld = false; // ansonsten hat er den Ball nicht } } @@ -132,13 +131,15 @@ void Ball_Tracker::Update() direction = (intensity[greatestIntensity] * mainDirection + intensity[secondIntensity] * secondDirection) / (intensity[greatestIntensity] + intensity[secondIntensity]); - - direction = easyAngle(direction); } else { direction = mainDirection; } + + direction += ourPositionTracker->GetOrientation(); + + direction = easyAngle(direction); } else { diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.h b/source/Concept/Framework/modules/interpreter/ball_tracker.h index cb90ff2..756a09f 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.h +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.h @@ -12,7 +12,6 @@ public: this->moduleId = 0; this->direction = EMPTY_FLOAT; this->ballHeldCounter = 0; - this->ballHeld = false; } Ball_Tracker(uint32 trackerId) @@ -21,13 +20,11 @@ public: this->moduleId = trackerId; this->direction = EMPTY_FLOAT; this->ballHeldCounter = 0; - this->ballHeld = false; } protected: float direction; uint8 ballHeldCounter; - bool ballHeld; public: void Update(); @@ -42,9 +39,9 @@ public: return (direction != EMPTY_FLOAT); } - bool RobotHasBall() + bool HasBall() { - return ballHeld; + return (ballHeldCounter >= 3); } }; diff --git a/source/Concept/Framework/modules/interpreter/command_handler.c b/source/Concept/Framework/modules/interpreter/command_handler.c index 7d8937b..e2a9ba2 100644 --- a/source/Concept/Framework/modules/interpreter/command_handler.c +++ b/source/Concept/Framework/modules/interpreter/command_handler.c @@ -3,6 +3,7 @@ //----------------------------------------------------------------------------- void Command_Handler::Update() { + Display* ourDisplay = parent->GetModule(IO_DISPLAY_MAIN); Keyboard* ourKeyboard = parent->GetModule(IO_KEYBOARD_MAIN); uint8 curInput = ourKeyboard->GetInput(); @@ -13,11 +14,20 @@ void Command_Handler::Update() if(curInput == 10) { ExecuteCommand(); + + for(uint8 i = 0; i < this->currentCommandLength; i++) + { + this->buffer[i] = NULL; + ourDisplay->Print(" ", i + 1, 1); + } + + this->currentCommandLength = 0; } else if(curInput == 11) { if(this->currentCommandLength > 0) { + ourDisplay->Print(" ", this->currentCommandLength, 1); this->currentCommandLength--; this->buffer[currentCommandLength] = NULL; } @@ -28,6 +38,7 @@ void Command_Handler::Update() { this->buffer[this->currentCommandLength] = curInput; this->currentCommandLength++; + ourDisplay->Print(curInput, this->currentCommandLength, 1); } } @@ -38,8 +49,163 @@ void Command_Handler::Update() //----------------------------------------------------------------------------- void Command_Handler::ExecuteCommand() { - if(this->buffer[0] == 5) + if(this->buffer[0] == 1) + { + if(this->buffer[1] == 1) + { + this->displayDistanceSensors = true; + this->displayPositionTracker = false; + this->displayBallTracker = false; + this->displayMouseSensors = false; + } + else if(this->buffer[1] == 2) + { + this->displayDistanceSensors = false; + this->displayPositionTracker = true; + this->displayBallTracker = false; + this->displayMouseSensors = false; + } + else if(this->buffer[1] == 3) + { + this->displayDistanceSensors = false; + this->displayPositionTracker = false; + this->displayBallTracker = true; + this->displayMouseSensors = false; + } + else if(this->buffer[1] == 4) + { + this->displayDistanceSensors = false; + this->displayPositionTracker = false; + this->displayBallTracker = false; + this->displayMouseSensors = true; + } + else if(this->buffer[1] == 0) + { + this->displayDistanceSensors = false; + this->displayPositionTracker = false; + this->displayBallTracker = false; + this->displayMouseSensors = false; + } + } + else if(this->buffer[0] == 2) + { + Logic* ourLogic = parent->GetModule(IO_LOGIC_MAIN); + if(this->buffer[1] == 1) + { + ourLogic->SetKeeper(true); + } + else if(this->buffer[1] == 2) + { + ourLogic->SetKeeper(false); + } + } + else if(this->buffer[0] == 3) + { + if(this->buffer[1] == 1) + { + Navigator* ourNavigator = parent->GetModule(IO_NAVIGATOR_MAIN); + ourNavigator->RotateTo(180, 200); + } + else if(this->buffer[1] == 2) + { + Aktuator* ourAktuator = parent->GetModule(IO_AKTUATOR_MAIN); + ourAktuator->Kick(); + } + else if(this->buffer[1] == 3) + { + Navigator* ourNavigator = parent->GetModule(IO_NAVIGATOR_MAIN); + ourNavigator->RotateTo(float(buffer[2]) * 100.0f + float(buffer[3]) * 10.0f + float(buffer[4]), 200); + } + else if(this->buffer[1] == 4) + { + Mouse_Sensor* ourLeftMouse = parent->GetModule(IO_SENSOR_MOUSE_LEFT); + Mouse_Sensor* ourRightMouse = parent->GetModule(IO_SENSOR_MOUSE_RIGHT); + + this->ticksPerCmOffset += 2.5f; + ourLeftMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset)); + ourLeftMouse->AdjustPositionY(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset)); + ourRightMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset)); + ourRightMouse->AdjustPositionY(3.88f * (TICKS_PER_CM + this->ticksPerCmOffset)); + } + else if(this->buffer[1] == 5) + { + Mouse_Sensor* ourLeftMouse = parent->GetModule(IO_SENSOR_MOUSE_LEFT); + Mouse_Sensor* ourRightMouse = parent->GetModule(IO_SENSOR_MOUSE_RIGHT); + + this->ticksPerCmOffset -= 2.5f; + ourLeftMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset)); + ourLeftMouse->AdjustPositionY(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset)); + ourRightMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset)); + ourRightMouse->AdjustPositionY(3.88f * (TICKS_PER_CM + this->ticksPerCmOffset)); + } + } + else if(this->buffer[0] == 4) + { + Navigator* ourNavigator = parent->GetModule(IO_NAVIGATOR_MAIN); + int16 speed = 200; + switch(this->buffer[1]) + { + case 1: + ourNavigator->Drive(225, 0, speed, 0); + break; + case 2: + ourNavigator->Drive(180, 0, speed, 0); + break; + case 3: + ourNavigator->Drive(135, 0, speed, 0); + break; + case 4: + ourNavigator->Drive(270, 0, speed, 0); + break; + case 5: + ourNavigator->Rotate(DEFAULT_ROTATION_SPEED); + break; + case 6: + ourNavigator->Drive(90, 0, speed, 0); + break; + case 7: + ourNavigator->Drive(315, 0, speed, 0); + break; + case 8: + ourNavigator->Drive(0, 0, speed, 0); + break; + case 9: + ourNavigator->Drive(45, 0, speed, 0); + break; + case 0: + ourNavigator->Stop(); + break; + } + } + else if(this->buffer[0] == 5) + { + Position_Tracker* ourPositionTracker = parent->GetModule(IO_POSITION_TRACKER_MAIN); + if(this->buffer[1] == 1) + { + ourPositionTracker->SetPosition(float(), ourPositionTracker->GetPositionY(), ourPositionTracker->GetOrientation()); + } + else if(this->buffer[1] == 0) + { + ourPositionTracker->SetPosition(0, 0, 0); + } + } + else if(this->buffer[0] == 6) + { + if(this->buffer[1] == 1) + { + Wireless* ourWireless = parent->GetModule(IO_WIRELESS_MAIN); + ourWireless->Send("Heyho!"); + } + } +} + +//----------------------------------------------------------------------------- +void Command_Handler::PrintCommand() +{ + Display* ourDisplay = parent->GetModule(IO_DISPLAY_MAIN); + + for(uint8 i = 0; i < currentCommandLength; i++) { - this->displayDistanceSensors = true; + ourDisplay->Print(this->buffer[i], i + 1, 1); } } diff --git a/source/Concept/Framework/modules/interpreter/command_handler.h b/source/Concept/Framework/modules/interpreter/command_handler.h index c3bc5bb..f376e96 100644 --- a/source/Concept/Framework/modules/interpreter/command_handler.h +++ b/source/Concept/Framework/modules/interpreter/command_handler.h @@ -12,6 +12,10 @@ public: this->moduleId = 0; this->currentCommandLength = 0; this->displayDistanceSensors = false; + this->displayPositionTracker = false; + this->displayBallTracker = false; + this->displayMouseSensors = false; + this->ticksPerCmOffset = 0; for(uint8 i = 0; i < COMMAND_BUFFER_SIZE; i++) { @@ -25,6 +29,10 @@ public: this->moduleId = commandHandlerId; this->currentCommandLength = 0; this->displayDistanceSensors = false; + this->displayPositionTracker = false; + this->displayBallTracker = false; + this->displayMouseSensors = false; + this->ticksPerCmOffset = 0; for(uint8 i = 0; i < COMMAND_BUFFER_SIZE; i++) { @@ -39,10 +47,17 @@ protected: void ExecuteCommand(); public: - void Update(); + void Update(); + + void PrintCommand(); //Command variables bool displayDistanceSensors; + bool displayPositionTracker; + bool displayBallTracker; + bool displayMouseSensors; + + float ticksPerCmOffset; }; #endif diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c index f6d67ac..fab8085 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.c +++ b/source/Concept/Framework/modules/interpreter/position_tracker.c @@ -3,6 +3,9 @@ //----------------------------------------------------------------------------- void Position_Tracker::Update() { + Command_Handler* ourCommandHandler = parent->GetModule(IO_COMMAND_HANDLER_MAIN); + Display* ourDisplay = parent->GetModule(IO_DISPLAY_MAIN); + // We want to use the mouse-sensors Mouse_Sensor* mouseLeft = parent->GetModule(IO_SENSOR_MOUSE_LEFT); Mouse_Sensor* mouseRight = parent->GetModule(IO_SENSOR_MOUSE_RIGHT); @@ -40,6 +43,14 @@ void Position_Tracker::Update() movementRightY = 0; } + if(ourCommandHandler->displayMouseSensors) + { + ourDisplay->Print(movementLeftX, 1, 2); + ourDisplay->Print(movementLeftY, 10, 2); + ourDisplay->Print(movementRightX, 1, 3); + ourDisplay->Print(movementRightY, 10, 3); + } + // Generate vector from P:left to P:right float movementDifferenceX = movementRightX - movementLeftX; float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY()); @@ -47,6 +58,11 @@ void Position_Tracker::Update() // Calculate the difference of orientation float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f); + if(ourCommandHandler->displayMouseSensors) + { + ourDisplay->Print(orientationDiff * 180.0f / PI, 1, 4); + } + float robotMovementX = movementDifferenceX / 2.0f; float robotMovementY = movementDifferenceY / 2.0f; robotMovementX += movementLeftX + mouseLeft->GetPositionX() + (-mouseLeft->GetPositionX() * cos(orientationDiff)); diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h index 56b16c4..7a5781f 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.h +++ b/source/Concept/Framework/modules/interpreter/position_tracker.h @@ -40,18 +40,20 @@ public: } // returns x-koordinate in mm - int GetPositionX() { - return (int)((positionX*10.0f)/TICKS_PER_CM); + int16 GetPositionX() + { + return (int16)((positionX*10.0f)/TICKS_PER_CM); } // returns y-koordinate in mm - int GetPositionY() { - return (int)((positionY*10.0f)/TICKS_PER_CM); + int16 GetPositionY() + { + return (int16)((positionY*10.0f)/TICKS_PER_CM); } // returns orientation - float GetOrientation() { - //return 0.0f; //tmp!!!!!!!!!! + float GetOrientation() + { return orientation; } }; diff --git a/source/Concept/Framework/modules/logic/logic.c b/source/Concept/Framework/modules/logic/logic.c index f698495..239e426 100755 --- a/source/Concept/Framework/modules/logic/logic.c +++ b/source/Concept/Framework/modules/logic/logic.c @@ -3,16 +3,51 @@ //----------------------------------------------------------------------------- void Logic::OnBallOwned() { + bool wasKeeper = this->IsKeeper(); + this->SetKeeper(false); + + if(!WIRELESS_ACTIVE) + { + Aktuator* ourAktuator = parent->GetModule(IO_AKTUATOR_MAIN); + + ourAktuator->Kick(); + this->SetKeeper(wasKeeper); + } } //----------------------------------------------------------------------------- void Logic::OnBallLost() { - Wireless* ourWireless = parent->GetModule(IO_WIRELESS_MAIN); - //ourWireless->Send(WIRELESS_CODE); - //ourWireless->Send(); +} + +//----------------------------------------------------------------------------- +void Logic::OnBecomeKeeper() +{ + Position_Tracker* ourPositionTracker = parent->GetModule(IO_POSITION_TRACKER_MAIN); + Navigator* ourNavigator = parent->GetModule(IO_NAVIGATOR_MAIN); + + if(distance2d(HOME_LOC_X, HOME_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) > DISTANCE_TOLERANCE) + { + status = STATUS_KEEPER_RETURN_HOME; + + ourNavigator->DriveTo(HOME_LOC_X, HOME_LOC_Y, ourPositionTracker->GetOrientation(), DEFAULT_SPEED, 0); + } + else + { + UpdateKeeperMovement(); + } +} + +//----------------------------------------------------------------------------- +void Logic::OnBecomeAttacker() +{ + Navigator* ourNavigator = parent->GetModule(IO_NAVIGATOR_MAIN); + + ourNavigator->Stop(); + + UpdateAttackerMovement(); } //----------------------------------------------------------------------------- @@ -20,21 +55,217 @@ void Logic::Update() { // We want to use a navigator Navigator* ourNavigator = parent->GetModule(IO_NAVIGATOR_MAIN); + Ball_Tracker* ourBallTracker = parent->GetModule(IO_BALL_TRACKER_MAIN); + Display* ourDisplay = parent->GetModule(IO_DISPLAY_MAIN); + Position_Tracker* ourPositionTracker = parent->GetModule(IO_POSITION_TRACKER_MAIN); + Obstacle_Tracker* ourObstacleTracker = parent->GetModule(IO_OBSTACLE_TRACKER_MAIN); + + if(!this->HasBall() && ourBallTracker->HasBall()) + { + this->hasBall = true; + this->OnBallOwned(); + } + else if(this->HasBall() && !ourBallTracker->HasBall()) + { + this->hasBall = false; + this->OnBallLost(); + } - // is Keeper? - if(isKeeper) { - // turn around al little bit... - if((status == STATUS_KEEPER_TURN_LEFT && !ourNavigator->IsMoving()) || - (status != STATUS_KEEPER_TURN_LEFT && status != STATUS_KEEPER_TURN_RIGHT)) { + switch(status) + { + case STATUS_ATTACKER_SEARCHING_AT_HOME: + case STATUS_ATTACKER_SEARCHING_AT_ENEMY: + if(ourBallTracker->KnowsBallDirection()) + { + ourNavigator->Stop(); + } + break; + case STATUS_KEEPER_HUNT_BALL: + if(ourBallTracker->KnowsBallDirection()) + { + ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0); + } + else + { + ourNavigator->Stop(); + } + break; + } + + if(!ourNavigator->IsMoving()) + { + if(this->IsKeeper()) + { + UpdateKeeperMovement(); + } + else + { + UpdateAttackerMovement(); + } + } + + if(ourNavigator->IsMoving()) + { + if(ourObstacleTracker->IsObstacleOnPath()) + { + avoidsObstacle = true; + for(uint8 i = 0; i < 5; i++) + { + if(ourObstacleTracker->IsObstacleOnPath(easyAngle(ourNavigator->GetDirection() - ((i - 2) * PI / 4.0f)))) + { + } + ourNavigator->GetDirection(); + } + } + } +} + +//----------------------------------------------------------------------------- +void Logic::UpdateKeeperMovement() +{ + Ball_Tracker* ourBallTracker = parent->GetModule(IO_BALL_TRACKER_MAIN); + Position_Tracker* ourPositionTracker = parent->GetModule(IO_POSITION_TRACKER_MAIN); + Navigator* ourNavigator = parent->GetModule(IO_NAVIGATOR_MAIN); + + if(ourBallTracker->KnowsBallDirection()) + { + if(ourBallTracker->GetBallDirection() > KEEPER_LEFT_ANGLE && + ourBallTracker->GetBallDirection() <= PI) + { + if(distance2d(DEFENCE_L_LOC_X, DEFENCE_L_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) < DISTANCE_TOLERANCE) + { + float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation()); + if(angleDifference > PI) + { + angleDifference = (2 * PI) - angleDifference; + } + + if(angleDifference > ORIENTATION_TOLERANCE) + { + status = STATUS_KEEPER_TURN_TO_BALL; + ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED); + } + else + { + status = STATUS_KEEPER_HUNT_BALL; + ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0); + } + } + else + { + status = STATUS_KEEPER_DRIVE_TO_DEFEND; + ourNavigator->DriveTo(DEFENCE_L_LOC_X, DEFENCE_L_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0); + } + } + else if(ourBallTracker->GetBallDirection() < KEEPER_RIGHT_ANGLE && + ourBallTracker->GetBallDirection() >= PI) + { + if(distance2d(DEFENCE_R_LOC_X, DEFENCE_R_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) < DISTANCE_TOLERANCE) + { + float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation()); + if(angleDifference > PI) + { + angleDifference = (2 * PI) - angleDifference; + } + + if(angleDifference > ORIENTATION_TOLERANCE) + { + status = STATUS_KEEPER_TURN_TO_BALL; + ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED); + } + else + { + status = STATUS_KEEPER_HUNT_BALL; + ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0); + } + } + else + { + status = STATUS_KEEPER_DRIVE_TO_DEFEND; + ourNavigator->DriveTo(DEFENCE_R_LOC_X, DEFENCE_R_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0); + } + } + else + { + if(distance2d(HOME_LOC_X, HOME_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) < DISTANCE_TOLERANCE) + { + float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation()); + if(angleDifference > PI) + { + angleDifference = (2 * PI) - angleDifference; + } + + if(angleDifference > ORIENTATION_TOLERANCE) + { + status = STATUS_KEEPER_TURN_TO_BALL; + ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED); + } + else + { + status = STATUS_KEEPER_HUNT_BALL; + ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0); + } + } + else + { + status = STATUS_KEEPER_DRIVE_TO_DEFEND; + ourNavigator->DriveTo(HOME_LOC_X, HOME_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0); + } + } + } + else + { + if(status == STATUS_KEEPER_TURN_LEFT) + { status = STATUS_KEEPER_TURN_RIGHT; - ourNavigator->RotateTo(315,200); + ourNavigator->RotateTo(315, DEFAULT_ROTATION_SPEED); } - else if(status == STATUS_KEEPER_TURN_RIGHT && !ourNavigator->IsMoving()) { + else + { status = STATUS_KEEPER_TURN_LEFT; - ourNavigator->RotateTo(45, 200); + ourNavigator->RotateTo(45, DEFAULT_ROTATION_SPEED); } } - else { // is Player? - +} + +//----------------------------------------------------------------------------- +void Logic::UpdateAttackerMovement() +{ + Ball_Tracker* ourBallTracker = parent->GetModule(IO_BALL_TRACKER_MAIN); + Position_Tracker* ourPositionTracker = parent->GetModule(IO_POSITION_TRACKER_MAIN); + Navigator* ourNavigator = parent->GetModule(IO_NAVIGATOR_MAIN); + + if(ourBallTracker->KnowsBallDirection()) + { + float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation()); + + if(angleDifference > PI) + { + angleDifference = (2 * PI) - angleDifference; + } + + if(angleDifference > ORIENTATION_TOLERANCE) + { + status = STATUS_ATTACKER_TURN_TO_BALL; + ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED); + } + else + { + status = STATUS_ATTACKER_DRIVE_TO_BALL; + ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0); + } + } + else + { + if(ourPositionTracker->GetPositionX() > 0) + { + status = STATUS_ATTACKER_SEARCHING_AT_HOME; + ourNavigator->DriveTo(HOME_LOC_X, HOME_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0); + } + else + { + status = STATUS_ATTACKER_SEARCHING_AT_ENEMY; + ourNavigator->DriveTo(ENEMY_LOC_X, ENEMY_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0); + } } } diff --git a/source/Concept/Framework/modules/logic/logic.h b/source/Concept/Framework/modules/logic/logic.h index ad8bad7..031e0b6 100755 --- a/source/Concept/Framework/modules/logic/logic.h +++ b/source/Concept/Framework/modules/logic/logic.h @@ -12,6 +12,8 @@ public: this->moduleId = 0; this->isKeeper = false; this->status = 0; + this->hasBall = false; + this->avoidsObstacle = false; } Logic(uint32 logicId) @@ -20,16 +22,29 @@ public: this->moduleId = logicId; this->isKeeper = false; this->status = 0; + this->hasBall = false; + this->avoidsObstacle = false; } protected: bool isKeeper; uint8 status; + bool hasBall; + bool avoidsObstacle; enum LogicalStatus { STATUS_KEEPER_TURN_RIGHT, STATUS_KEEPER_TURN_LEFT, + STATUS_KEEPER_RETURN_HOME, + STATUS_KEEPER_HUNT_BALL, + STATUS_KEEPER_TURN_TO_BALL, + STATUS_KEEPER_DRIVE_TO_DEFEND, + + STATUS_ATTACKER_TURN_TO_BALL, + STATUS_ATTACKER_DRIVE_TO_BALL, + STATUS_ATTACKER_SEARCHING_AT_HOME, + STATUS_ATTACKER_SEARCHING_AT_ENEMY, }; void OnBallOwned(); @@ -51,7 +66,8 @@ public: return !isKeeper; } - void SetKeeper(bool newStatus) { + void SetKeeper(bool newStatus) + { if(!this->isKeeper && newStatus) { this->OnBecomeKeeper(); @@ -65,8 +81,11 @@ public: bool HasBall() { - //fill me! + return this->hasBall; } + + void UpdateKeeperMovement(); + void UpdateAttackerMovement(); }; #endif diff --git a/source/Concept/Framework/robot.c b/source/Concept/Framework/robot.c index 7653c61..9dc564d 100755 --- a/source/Concept/Framework/robot.c +++ b/source/Concept/Framework/robot.c @@ -90,15 +90,15 @@ void Robot::Update() { GetModule(IO_COMMAND_HANDLER_MAIN)->Update(); + GetModule(IO_POSITION_TRACKER_MAIN)->Update(); + GetModule(IO_BALL_TRACKER_MAIN)->Update(); - GetModule(IO_POSITION_TRACKER_MAIN)->Update(); + GetModule(IO_OBSTACLE_TRACKER_MAIN)->Update(); - GetModule(IO_LOGIC_MAIN)->Update(); + //GetModule(IO_LOGIC_MAIN)->Update(); GetModule(IO_NAVIGATOR_MAIN)->Update(); - - //insert code here } //----------------------------------------------------------------------------- diff --git a/source/Concept/Framework/stdafx.h b/source/Concept/Framework/stdafx.h index 88d8c85..6e3f031 100644 --- a/source/Concept/Framework/stdafx.h +++ b/source/Concept/Framework/stdafx.h @@ -22,6 +22,7 @@ #include "mouse_sensor.h" #include "command_handler.h" #include "position_tracker.h" +#include "obstacle_tracker.h" #include "ball_tracker.h" #include "navigator.h" #include "logic.h" -- cgit v1.2.3