From 655dd7522fb60e2ef0e68437337178184109f347 Mon Sep 17 00:00:00 2001 From: sicarius Date: Thu, 22 Feb 2007 13:12:03 +0000 Subject: Codework --- source/Concept/Framework/RoboCode.aps | 2 +- source/Concept/Framework/default/Makefile | 11 +- source/Concept/Framework/defines.h | 34 ++++- source/Concept/Framework/main.c | 86 ++++++++---- .../Concept/Framework/modules/executor/navigator.c | 149 +++++---------------- .../Framework/modules/input/distance_sensor.c | 19 ++- source/Concept/Framework/modules/input/ir_sensor.c | 2 +- source/Concept/Framework/modules/input/ir_sensor.h | 7 + .../Framework/modules/interpreter/ball_tracker.c | 7 + .../Framework/modules/interpreter/ball_tracker.h | 10 ++ .../modules/interpreter/position_tracker.c | 13 +- .../modules/interpreter/position_tracker.h | 26 ++-- source/Concept/Framework/modules/output/dribbler.h | 7 +- source/Concept/Framework/stdafx.h | 5 +- 14 files changed, 212 insertions(+), 166 deletions(-) diff --git a/source/Concept/Framework/RoboCode.aps b/source/Concept/Framework/RoboCode.aps index 83aaae6..609c3cf 100644 --- a/source/Concept/Framework/RoboCode.aps +++ b/source/Concept/Framework/RoboCode.aps @@ -1 +1 @@ -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 +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 diff --git a/source/Concept/Framework/default/Makefile b/source/Concept/Framework/default/Makefile index d401a7f..0411c61 100644 --- a/source/Concept/Framework/default/Makefile +++ b/source/Concept/Framework/default/Makefile @@ -40,7 +40,7 @@ INCLUDES = -I"Y:\Concept\Framework\modules" -I"Y:\Concept\Framework\modules\exec 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 +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 explicitly added by the user LINKONLYOBJECTS = @@ -103,6 +103,15 @@ ball_tracker.o: ../modules/interpreter/ball_tracker.c navigator.o: ../modules/executor/navigator.c $(CC) $(INCLUDES) $(CFLAGS) -c $< +aktuator.o: ../modules/executor/aktuator.c + $(CC) $(INCLUDES) $(CFLAGS) -c $< + +wireless.o: ../modules/wireless.c + $(CC) $(INCLUDES) $(CFLAGS) -c $< + +logic.o: ../modules/logic/logic.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 586a3d8..6e62143 100644 --- a/source/Concept/Framework/defines.h +++ b/source/Concept/Framework/defines.h @@ -59,11 +59,19 @@ #define SPEED_PER_PWM 1 #define DISTANCE_PER_VALUE 1 #define TICKS_PER_CM 205.0f +//#define TICKS_PER_CM 90.0f #define PI 3.14159265358979323846f #define CYCLES_PER_CORRECTION 20 #define EMPTY_FLOAT 81188.1484f +#define WIRELESS_CODE "SPASS!" +enum WirelessCommands +{ + WLESS_CMD_CONFIRM, + WLESS_CMD_RECIEVED_BALL, +}; + //IO Module Names enum IOModuleNames { @@ -94,9 +102,25 @@ enum IOModuleNames IO_KICKER_MAIN = IO_KICKER_START, IO_KICKER_END, + + // Aktuator + + IO_AKTUATOR_START = IO_KICKER_END, + + IO_AKTUATOR_MAIN = IO_AKTUATOR_START, + + IO_AKTUATOR_END, + + // Wireless + + IO_WIRELESS_START = IO_AKTUATOR_END, + + IO_WIRELESS_MAIN = IO_WIRELESS_START, + + IO_WIRELESS_END, //Sensors - IO_SENSOR_START = IO_KICKER_END, + IO_SENSOR_START = IO_WIRELESS_END, IO_SENSOR_IR_0_DEG = IO_SENSOR_START, IO_SENSOR_IR_30_DEG, @@ -163,6 +187,14 @@ enum IOModuleNames IO_NAVIGATOR_END, + //Logics + + IO_LOGIC_START = IO_NAVIGATOR_END, + + IO_LOGIC_MAIN, + + IO_LOGIC_END, + //General IO_END = IO_NAVIGATOR_END, }; diff --git a/source/Concept/Framework/main.c b/source/Concept/Framework/main.c index b0fc96b..1875847 100755 --- a/source/Concept/Framework/main.c +++ b/source/Concept/Framework/main.c @@ -39,6 +39,22 @@ int main() newKicker = NULL; } + //Init Aktuator + for(uint8 i = IO_AKTUATOR_START; i < IO_AKTUATOR_END; i++) + { + Aktuator* newAktuator = new Aktuator(i); + localRobot->AddModule(newAktuator); + newAktuator = NULL; + } + + //Init Wireless + for(uint8 i = IO_WIRELESS_START; i < IO_WIRELESS_END; i++) + { + Wireless* newWireless = new Wireless(i); + localRobot->AddModule(newWireless); + newWireless = NULL; + } + //Init Sensors for(uint8 i = IO_SENSOR_START; i < IO_SENSOR_END; i++) { @@ -130,6 +146,14 @@ int main() newNavigator = NULL; } + //Init Logics + for(uint8 i = IO_LOGIC_START; i < IO_LOGIC_END; i++) + { + Logic* newLogic = new Logic(i); + localRobot->AddModule(newLogic); + newLogic = NULL; + } + sleep(1); //Debug code @@ -146,8 +170,14 @@ int main() uint32 i = 1; Navigator* ourNavigator = localRobot->GetModule(IO_NAVIGATOR_MAIN); Position_Tracker* ourPosition_Tracker = 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); + Aktuator* ourAktuator = localRobot->GetModule(IO_AKTUATOR_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); @@ -157,19 +187,26 @@ int main() while(true) { ourDisplay->Print(i++,1,1); + + msleep(100); - if(!(i % 200)) + if(!(i % 1)) { 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); + //ourDisplay->Print(ourPosition_Tracker->GetPositionX(),1,2); + //ourDisplay->Print(ourPosition_Tracker->GetPositionY(),1,3); + //ourDisplay->Print(ourPosition_Tracker->GetOrientation() * 180.0f / PI,1,4); + } + + if(!(i % 50)) + { + ourAktuator->Kick(); } - /*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(value, 1, 2); ourDisplay->Print(";"); distanceSensor = localRobot->GetModule(IO_SENSOR_DISTANCE_90_DEG); value = distanceSensor->GetDistance(); @@ -177,12 +214,12 @@ int main() ourDisplay->Print(";"); distanceSensor = localRobot->GetModule(IO_SENSOR_DISTANCE_180_DEG); value = distanceSensor->GetDistance(); - ourDisplay->Print(value); + ourDisplay->Print(value, 1, 3); ourDisplay->Print(";"); distanceSensor = localRobot->GetModule(IO_SENSOR_DISTANCE_270_DEG); value = distanceSensor->GetDistance(); ourDisplay->Print(value); - ourDisplay->Print(";"); */ + ourDisplay->Print(";"); uint8 someInput = ourKeyboard->GetInput(); //ourDisplay->Print("Ready to accept...", 1, 2); @@ -190,46 +227,45 @@ int main() { case 0: ourNavigator->Stop(); + return 0; break; case 1: - ourNavigator->Drive(225, rotation, 100, rotation); + ourNavigator->Drive(225, rotation, speed, rotation); break; case 2: - ourNavigator->Drive(180, rotation, 100, rotation); + ourNavigator->Drive(180, rotation, speed, rotation); break; case 3: - ourNavigator->Drive(135, rotation, 100, rotation); + ourNavigator->Drive(135, rotation, speed, rotation); break; case 4: - ourNavigator->Drive(270, rotation, 100, rotation); + ourNavigator->Drive(270, rotation, speed, rotation); break; case 5: ourNavigator->Drive(0, rotation, 0, rotation); break; case 6: - ourNavigator->Drive(90, rotation, 100, rotation); + ourNavigator->Drive(90, rotation, speed, rotation); break; case 7: - ourNavigator->Drive(315, rotation, 100, rotation); + ourNavigator->Drive(315, rotation, speed, rotation); break; case 8: - ourNavigator->Drive(0, rotation, 100, rotation); + ourNavigator->Drive(0, rotation, speed, rotation); break; case 9: - ourNavigator->Drive(45, rotation, 100, rotation); + ourNavigator->Drive(45, rotation, speed, rotation); break; case 10: - rotation += 10; + ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker break; case 12: - rotation -= 10; + ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker break; } - /* //ourDisplay->Clear(); - ourDisplay->Print(i++,1,1); - ourSensor = localRobot->GetModule(IO_SENSOR_IR_0_DEG); + /*ourSensor = localRobot->GetModule(IO_SENSOR_IR_0_DEG); value = ourSensor->GetIRIntensity(); ourDisplay->Print(value, 1, 2); ourDisplay->Print(";"); @@ -260,13 +296,13 @@ int main() ourSensor = localRobot->GetModule(IO_SENSOR_IR_330_DEG); value = ourSensor->GetIRIntensity(); ourDisplay->Print(value); - ourDisplay->Print(";"); + ourDisplay->Print(";");*/ - msleep(500); - */ localRobot->Update(); - //ourDisplay->PrintFloat(ourBallTracker->GetBallDirection(), 1, 4); + //ourDisplay->Print(ourBallTracker->GetBallDirection() * 180.0f / PI, 1, 4); + + //ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0); } //Cleanup diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index 5dbf8bf..928a5ff 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -54,30 +54,6 @@ 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;*/ } //----------------------------------------------------------------------------- @@ -178,108 +154,43 @@ void Navigator::CalculateDirection() //----------------------------------------------------------------------------- void Navigator::CalculateEngines() { + // Use the position_tracker Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); - - if(direction != EMPTY_FLOAT) - { - float relativeDirection = this->direction - locationeer->GetOrientation(); - - float xDrive = cos(relativeDirection + PI / 6.0f); - float yDrive = sin(relativeDirection + PI / 6.0f); - - float vLeft = xDrive; - float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f; - float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f; - - /*float speedCorrection = 1.0f; - - float maxEngineSpeed = 255.0f; - float minEngineSpeed = -255.0f; + // get the relative position of the robot + float relativeDirection = this->direction - locationeer->GetOrientation(); - float maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight)); + // calculate x and y-koordinates + float xDrive = cos(relativeDirection + PI / 6.0f); + float yDrive = sin(relativeDirection + PI / 6.0f); - float calcSpeed = 1.0f;//sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + - // vBack * (vBack + vLeft + vRight)); + // calculate relative speed of engines + float vLeft = xDrive; + float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f; + float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f; - if(calcSpeed != 1.0f) - { - speedCorrection = 1.0f / calcSpeed; - } + // Get the maximal value + float speedCorrection = (float)max(max(fabs(vLeft),fabs(vBack)),fabs(vRight)); - float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection; - if(maxOverallSpeed > maxEngineSpeed) - { - robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection); - }*/ - + // calculate the correct speeds of the engines + vLeft = ((float)vLeft * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed; + vBack = ((float)vBack * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed; + vRight = ((float)vRight * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed; - vLeft = vLeft * this->robotSpeed;// * speedCorrection; - vBack = vBack * this->robotSpeed;// * speedCorrection; - vRight = vRight * this->robotSpeed;// * speedCorrection; + //(parent->GetModule(IO_DISPLAY_MAIN))->Print(vLeft,10,2); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print(vBack,10,3); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print(vRight,10,4); - /*maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight)); - float minSingleSpeed = min(min(vLeft, vBack), vRight); - if(rotationSpeed) - { - if(this->rotationSpeed > 0) - { - if(maxEngineSpeed - maxSingleSpeed < this->rotationSpeed) - { - vLeft += maxEngineSpeed - maxSingleSpeed; - vBack += maxEngineSpeed - maxSingleSpeed; - vRight += maxEngineSpeed - maxSingleSpeed; - } - else - { - vLeft += this->rotationSpeed; - vBack += this->rotationSpeed; - vRight += this->rotationSpeed; - } - } - else - { - if((minEngineSpeed - minSingleSpeed) < this->rotationSpeed) - { - vLeft -= minEngineSpeed - minSingleSpeed; - vBack -= minEngineSpeed - minSingleSpeed; - vRight -= minEngineSpeed - minSingleSpeed; - } - else - { - vLeft -= this->rotationSpeed; - vBack -= this->rotationSpeed; - vRight -= this->rotationSpeed; - } - } - }*/ - Engine* curEngine = parent->GetModule(IO_ENGINE_DRIVE_LEFT); - curEngine->SetSpeed(vLeft); - curEngine->SetEnabled(true); - curEngine = parent->GetModule(IO_ENGINE_DRIVE_BACK); - curEngine->SetSpeed(vBack); - curEngine->SetEnabled(true); - curEngine = parent->GetModule(IO_ENGINE_DRIVE_RIGHT); - curEngine->SetSpeed(vRight); - curEngine->SetEnabled(true); - curEngine = NULL; - } - else if(rotationSpeed) - { - Engine* curEngine = parent->GetModule(IO_ENGINE_DRIVE_LEFT); - curEngine->SetSpeed(this->rotationSpeed); - curEngine->SetEnabled(true); - curEngine = parent->GetModule(IO_ENGINE_DRIVE_BACK); - curEngine->SetSpeed(this->rotationSpeed); - curEngine->SetEnabled(true); - curEngine = parent->GetModule(IO_ENGINE_DRIVE_RIGHT); - curEngine->SetSpeed(this->rotationSpeed); - curEngine->SetEnabled(true); - curEngine = NULL; - } - else - { - Stop(); - } + // Transfer the values to the engines + 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; } diff --git a/source/Concept/Framework/modules/input/distance_sensor.c b/source/Concept/Framework/modules/input/distance_sensor.c index 262d35f..e6fd3f8 100755 --- a/source/Concept/Framework/modules/input/distance_sensor.c +++ b/source/Concept/Framework/modules/input/distance_sensor.c @@ -5,21 +5,36 @@ float Distance_Sensor::GetDistance() { uint32 result = 0; + //(parent->GetModule(IO_DISPLAY_MAIN))->Print("Gen Pulse; Pin:",1,4); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print((int)(*hardwarePin & pin)); + + msleep(500); + //Generate pulse *hardwareDDR |= pin;//Set pin output *hardwarePort |= pin;//Activate port usleep(10);//Wait for 10µs - *hardwarePort &= ~pin;//Deactivate port *hardwareDDR &= ~pin;//Set pin input + *hardwarePort &= ~pin;//Deactivate port + //(parent->GetModule(IO_DISPLAY_MAIN))->Print("Wait response; Pin:",1,4); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print((int)(*hardwarePin & pin)); + + uint16 i; //Wait for response - for(uint16 i = 0; (!(*hardwarePin & pin)) && (i < 1000); i++) { asm volatile("nop"); } + for(i = 0; (!(*hardwarePin & pin)) && (i < 1000); i++) { asm volatile("nop"); } //Calculate duration of response while((*hardwarePin & pin)&&(result < 300000)) { result++; asm volatile("nop"); + asm volatile("nop"); + asm volatile("nop"); + asm volatile("nop"); + asm volatile("nop"); + asm volatile("nop"); + asm volatile("nop"); } return (float(result) * DISTANCE_PER_VALUE); diff --git a/source/Concept/Framework/modules/input/ir_sensor.c b/source/Concept/Framework/modules/input/ir_sensor.c index c34feed..12d74e0 100755 --- a/source/Concept/Framework/modules/input/ir_sensor.c +++ b/source/Concept/Framework/modules/input/ir_sensor.c @@ -5,5 +5,5 @@ uint16 IR_Sensor::GetIRIntensity() { if(!parent) return 0; - return parent->GetADCValue(channel); + return min(parent->GetADCValue(channel) + this->intensityCorrection, 1023); } diff --git a/source/Concept/Framework/modules/input/ir_sensor.h b/source/Concept/Framework/modules/input/ir_sensor.h index 30e6ea4..74396ec 100755 --- a/source/Concept/Framework/modules/input/ir_sensor.h +++ b/source/Concept/Framework/modules/input/ir_sensor.h @@ -12,12 +12,14 @@ public: { this->parent = NULL; this->moduleId = 0; + this->intensityCorrection = 0; } IR_Sensor(uint32 sensorId) { this->parent = NULL; this->moduleId = sensorId; + this->intensityCorrection = 0; switch(sensorId) { @@ -32,12 +34,15 @@ public: break; case IO_SENSOR_IR_100_DEG: this->channel = 3; + this->intensityCorrection = 40; break; case IO_SENSOR_IR_180_DEG: this->channel = 4; + this->intensityCorrection = 50; break; case IO_SENSOR_IR_260_DEG: this->channel = 5; + this->intensityCorrection = 70; break; case IO_SENSOR_IR_300_DEG: this->channel = 6; @@ -55,6 +60,8 @@ protected: //Hardware uint8 channel; + uint8 intensityCorrection; + public: uint16 GetIRIntensity(); }; diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.c b/source/Concept/Framework/modules/interpreter/ball_tracker.c index 1701121..2d85b96 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.c +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.c @@ -109,9 +109,16 @@ void Ball_Tracker::Update() break; } + if(fabs(mainDirection - secondDirection) > PI) + { + min(mainDirection, secondDirection) += 2.0f * PI; + } + direction = (intensity[greatestIntensity] * mainDirection + intensity[secondIntensity] * secondDirection) / (intensity[greatestIntensity] + intensity[secondIntensity]); + + 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 bea4a19..c62f05e 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.h +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.h @@ -30,6 +30,16 @@ public: { return direction; } + + bool KnowsBallDirection() + { + return direction != EMPTY_FLOAT; + } + + bool RobotHasBall() + { + //fill me! + } }; #endif diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c index 63a7f5f..a64ab60 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.c +++ b/source/Concept/Framework/modules/interpreter/position_tracker.c @@ -3,9 +3,11 @@ //----------------------------------------------------------------------------- void Position_Tracker::Update() { + // 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); + // Generate a vector for the left mouse int8 leftX = mouseLeft->GetXMovement(); int8 leftY = mouseLeft->GetYMovement(); float distanceLeft = sqrt(leftX * leftX + leftY * leftY); @@ -14,12 +16,14 @@ void Position_Tracker::Update() float movementLeftX = cos(angleLeft) * distanceLeft; float movementLeftY = sin(angleLeft) * distanceLeft; + // AVR calculates a little bit strange ;) if(!leftX && !leftY) { movementLeftX = 0; movementLeftY = 0; } + // Generate a vector for the right mouse int8 rightX = mouseRight->GetXMovement(); int8 rightY = mouseRight->GetYMovement(); float distanceRight = sqrt(rightX * rightX + rightY * rightY); @@ -28,21 +32,24 @@ void Position_Tracker::Update() float movementRightX = cos(angleRight) * distanceRight; float movementRightY = sin(angleRight) * distanceRight; + // AVR calculates a little bit strange ;) if(!rightX && !rightY) { movementRightX = 0; movementRightY = 0; } - float movementDifferenceX = (movementRightX + mouseRight->GetPositionX()) - (movementLeftX + mouseLeft->GetPositionX()); + // Generate vector from P:left to P:right + float movementDifferenceX = movementRightX - movementLeftX; float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY()); + // Calculate the difference of orientation float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f); float robotMovementX = movementDifferenceX / 2.0f; float robotMovementY = movementDifferenceY / 2.0f; robotMovementX += movementLeftX + mouseLeft->GetPositionX() + (-mouseLeft->GetPositionX() * cos(orientationDiff)); - robotMovementY += movementLeftY + mouseLeft->GetPositionY() + (-mouseLeft->GetPositionX() * sin(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; @@ -58,7 +65,7 @@ void Position_Tracker::Update() } //(parent->GetModule(IO_DISPLAY_MAIN))->Print(" ", 5, 1); - //(parent->GetModule(IO_DISPLAY_MAIN))->Print(absoluteDiffX, 5, 1); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print(robotMovementY + movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff)), 5, 1); //(parent->GetModule(IO_DISPLAY_MAIN))->Print(absoluteDiffY, 12, 1); this->positionX += absoluteDiffX; diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h index e93ef0c..47d0740 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.h +++ b/source/Concept/Framework/modules/interpreter/position_tracker.h @@ -32,19 +32,27 @@ protected: public: void Update(); - float GetPositionX() - { - return positionX; + // Sets the current position; x and y in mm + void SetPosition(int newPositionX, int newPositionY, float newOrientation) { + positionX = newPositionX*(TICKS_PER_CM/10.0f); + positionY = newPositionY*(TICKS_PER_CM/10.0f); + orientation = easyAngle(newOrientation); } - float GetPositionY() - { - return positionY; + // returns x-koordinate in mm + int GetPositionX() { + return (int)((positionX*10.0f)/TICKS_PER_CM); } - float GetOrientation() - { - return orientation; + // returns y-koordinate in mm + int GetPositionY() { + return (int)((positionY*10.0f)/TICKS_PER_CM); + } + + // returns orientation + float GetOrientation() { + return 0.0f; //tmp!!!!!!!!!! + //return orientation; } }; diff --git a/source/Concept/Framework/modules/output/dribbler.h b/source/Concept/Framework/modules/output/dribbler.h index b11af69..8544698 100755 --- a/source/Concept/Framework/modules/output/dribbler.h +++ b/source/Concept/Framework/modules/output/dribbler.h @@ -66,19 +66,20 @@ protected: { *hardwarePort |= pinForward; *hardwarePort &= ~pinReverse; + *portPower |= pinPower; } else if(curSpeed < 0) { *hardwarePort |= pinReverse; *hardwarePort &= ~pinForward; + *portPower |= pinPower; } else { *hardwarePort |= pinForward; *hardwarePort |= pinReverse; - } - - *portPower |= pinPower; + *portPower &= ~pinPower; + } } else { diff --git a/source/Concept/Framework/stdafx.h b/source/Concept/Framework/stdafx.h index af60c6e..a4054e3 100644 --- a/source/Concept/Framework/stdafx.h +++ b/source/Concept/Framework/stdafx.h @@ -14,6 +14,8 @@ #include "engine.h" #include "dribbler.h" #include "kicker.h" +#include "aktuator.h" +#include "wireless.h" #include "led.h" #include "distance_sensor.h" #include "ir_sensor.h" @@ -21,4 +23,5 @@ #include "position_tracker.h" #include "ball_tracker.h" #include "navigator.h" -#include "robot.h" +#include "logic.h" +#include "robot.h" -- cgit v1.2.3