From 42a38959ff5c097463c2c95d993e5934f100d223 Mon Sep 17 00:00:00 2001 From: sicarius Date: Mon, 19 Feb 2007 20:57:03 +0000 Subject: Code-stuff --- source/Concept/Framework/RoboCode.aps | 4 + source/Concept/Framework/default/Makefile | 4 +- source/Concept/Framework/defines.h | 6 + source/Concept/Framework/main.c | 121 +++++++--- .../Concept/Framework/modules/executor/navigator.c | 264 +++++++++++++++++++++ .../Concept/Framework/modules/executor/navigator.h | 73 ++++++ .../Framework/modules/input/distance_sensor.c | 12 +- .../Concept/Framework/modules/input/mouse_sensor.h | 16 +- source/Concept/Framework/modules/output/display.h | 5 +- source/Concept/Framework/modules/output/engine.h | 11 +- source/Concept/Framework/robot.c | 8 +- source/Concept/Framework/tools.h | 3 +- 12 files changed, 473 insertions(+), 54 deletions(-) diff --git a/source/Concept/Framework/RoboCode.aps b/source/Concept/Framework/RoboCode.aps index 3bc029f..01a1bea 100644 --- a/source/Concept/Framework/RoboCode.aps +++ b/source/Concept/Framework/RoboCode.aps @@ -1 +1,5 @@ +<<<<<<< .mine +RoboCode16-Feb-2007 15:16:4619-Feb-2007 18:25:08241016-Feb-2007 15:16:4644, 12, 0, 462AVR GCCdefault\RoboCode.elfY:\Concept\Framework\falseR00R01R02R03R04R05R06R07R08R09R10R11R12R13R14R15R16R17R18R19R20R21R22R23R24R25R26R27R28R29R30R31000modules\io_module.catmega128io.cmain.crobot.ctools.cmodules\input\distance_sensor.cmodules\input\ir_sensor.cmodules\input\keyboard.cmodules\input\mouse_sensor.cmodules\input\sensor.cmodules\interpreter\position_tracker.cmodules\output\display.cmodules\output\dribbler.cmodules\output\engine.cmodules\output\kicker.cmodules\output\led.cmodules\interpreter\ball_tracker.cmodules\executor\navigator.cstdafx.htools.hatmega128io.hdefines.hrobot.hmodules\input\distance_sensor.hmodules\input\ir_sensor.hmodules\input\keyboard.hmodules\input\mouse_sensor.hmodules\input\sensor.hmodules\output\display.hmodules\output\dribbler.hmodules\output\engine.hmodules\output\kicker.hmodules\output\led.hmodules\interpreter\position_tracker.hmodules\io_module.hmodules\interpreter\ball_tracker.hmodules\executor\navigator.hdefaultNOatmega128100RoboCode.elfdefault\0modules\modules\executor\modules\input\modules\interpreter\modules\logic\modules\output\libm.a-Wall -gdwarf-2 -DF_CPU=16000000ULUL -O3 -fsigned-char-lmdefault0C:\WinAVR\bin\avr-g++.exeC:\WinAVR\utils\bin\make.exeY:\Concept\Framework\stdafx.hY:\Concept\Framework\tools.hY:\Concept\Framework\atmega128io.hY:\Concept\Framework\defines.hY:\Concept\Framework\robot.hY:\Concept\Framework\modules\input\distance_sensor.hY:\Concept\Framework\modules\input\ir_sensor.hY:\Concept\Framework\modules\input\keyboard.hY:\Concept\Framework\modules\input\mouse_sensor.hY:\Concept\Framework\modules\input\sensor.hY:\Concept\Framework\modules\output\display.hY:\Concept\Framework\modules\output\dribbler.hY:\Concept\Framework\modules\output\engine.hY:\Concept\Framework\modules\output\kicker.hY:\Concept\Framework\modules\output\led.hY:\Concept\Framework\modules\interpreter\position_tracker.hY:\Concept\Framework\modules\io_module.hY:\Concept\Framework\modules\interpreter\ball_tracker.hY:\Concept\Framework\modules\executor\navigator.hY:\Concept\Framework\modules\io_module.cY:\Concept\Framework\atmega128io.cY:\Concept\Framework\main.cY:\Concept\Framework\robot.cY:\Concept\Framework\tools.cY:\Concept\Framework\modules\input\distance_sensor.cY:\Concept\Framework\modules\input\ir_sensor.cY:\Concept\Framework\modules\input\keyboard.cY:\Concept\Framework\modules\input\mouse_sensor.cY:\Concept\Framework\modules\input\sensor.cY:\Concept\Framework\modules\interpreter\position_tracker.cY:\Concept\Framework\modules\output\display.cY:\Concept\Framework\modules\output\dribbler.cY:\Concept\Framework\modules\output\engine.cY:\Concept\Framework\modules\output\kicker.cY:\Concept\Framework\modules\output\led.cY:\Concept\Framework\modules\interpreter\ball_tracker.cY:\Concept\Framework\modules\executor\navigator.c00000stdafx.h100001defines.h100002robot.h100003atmega128io.h100004tools.h100005modules\interpreter\position_tracker.h100006modules\io_module.h100007modules\output\display.h100008modules\input\ir_sensor.h100009modules\output\engine.h100010modules\input\sensor.h100011modules\input\keyboard.h100012modules\io_module.c100013robot.c100014modules\input\ir_sensor.c100015modules\output\dribbler.h100016modules\output\kicker.h100017modules\output\led.h100018modules\input\distance_sensor.h100019modules\input\mouse_sensor.h100020modules\input\sensor.c100021main.c100022modules\interpreter\ball_tracker.h100023modules\interpreter\ball_tracker.c100024modules\interpreter\position_tracker.c100025modules\input\distance_sensor.c100026modules\output\engine.c100027modules\output\dribbler.c100028modules\executor\navigator.h100029modules\executor\navigator.c100030modules\output\display.c100031modules\input\mouse_sensor.c1299 75 1025 5566 033058 32861 33658 33107155 033058 32861 33658 3310745 033058 32861 33658 3310716 033058 32861 33658 3310711 033058 32861 33658 3310743 032287 32091 33013 325468 033058 32861 33784 3331673 033058 32861 33658 331072 032384 32233 33010 32534109 032296 32117 32926 3242227 032321 32148 32951 324536 032343 32177 32973 324821 032365 32206 32995 3251116 7132387 32235 33017 325409 032299 32119 32929 324243 032321 32148 32951 3245321 032343 32177 32973 324821 032365 32206 32995 3251171 0299 72 1025 52760 30Maximized32299 32119 32929 324241 0293 68 1019 523168 1932343 32177 32973 324823 0287 64 1013 51965 032387 32235 33017 325407 032299 32119 32929 324241 032321 32148 32951 324531 032343 32177 32973 324821 032365 32206 32995 3251139 0296 70 1022 525156 20300 100 930 4050 0322 129 952 4340 0 +======= RoboCode16-Feb-2007 15:16:4618-Feb-2007 22:21:14241016-Feb-2007 15:16:4644, 12, 0, 462AVR GCCdefault\RoboCode.elfI:\Projects\RoboCup WM 2006 07\trunk\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.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.exeI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\stdafx.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\tools.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\atmega128io.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\defines.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\robot.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\distance_sensor.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\ir_sensor.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\keyboard.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\mouse_sensor.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\sensor.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\display.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\dribbler.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\engine.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\kicker.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\led.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\interpreter\position_tracker.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\io_module.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\interpreter\ball_tracker.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\executor\navigator.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\io_module.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\atmega128io.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\main.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\robot.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\tools.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\distance_sensor.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\ir_sensor.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\keyboard.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\mouse_sensor.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\sensor.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\interpreter\position_tracker.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\display.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\dribbler.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\engine.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\kicker.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\led.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\interpreter\ball_tracker.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\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.c1232 74 958 5556 0267 72 1025 45960 34Maximized32994 32862 33594 3310845 032994 32862 33594 3310816 032994 32862 33594 3310811 032994 32862 33594 331085 032223 32093 32949 325488 032994 32862 33720 33317102 032994 32862 33594 331082 032320 32235 32946 3253655 032232 32119 32862 3242427 032257 32150 32887 324556 032279 32179 32909 324841 032301 32208 32931 3251317 032323 32237 32953 325429 032235 32121 32865 324263 032257 32150 32887 3245518 032279 32179 32909 324841 032301 32208 32931 3251371 032323 32237 32953 3254216 032235 32121 32865 324261 0229 70 955 525177 032279 32179 32909 324843 0223 66 949 52152 032323 32237 32953 325427 032235 32121 32865 324261 032257 32150 32887 324551 032279 32179 32909 324841 032301 32208 32931 325131 032323 32237 32953 32542214 5 +>>>>>>> .r199 diff --git a/source/Concept/Framework/default/Makefile b/source/Concept/Framework/default/Makefile index fc917c5..d401a7f 100644 --- a/source/Concept/Framework/default/Makefile +++ b/source/Concept/Framework/default/Makefile @@ -13,7 +13,7 @@ COMMON = -mmcu=$(MCU) ## Compile options common for all C compilation units. CFLAGS = $(COMMON) -CFLAGS += -Wall -gdwarf-2 -DF_CPU=16000000ULUL -O3 -fsigned-char +CFLAGS += -Wall -gdwarf-2 -DF_CPU=16000000ULUL -O3 -fsigned-char CFLAGS += -MD -MP -MT $(*F).o -MF dep/$(@F).d ## Assembly specific flags @@ -22,7 +22,7 @@ ASMFLAGS += -x assembler-with-cpp -Wa,-gdwarf2 ## Linker flags LDFLAGS = $(COMMON) -LDFLAGS += -lm +LDFLAGS += -lm ## Intel Hex file production flags diff --git a/source/Concept/Framework/defines.h b/source/Concept/Framework/defines.h index 1cec0f3..6a88917 100644 --- a/source/Concept/Framework/defines.h +++ b/source/Concept/Framework/defines.h @@ -58,8 +58,14 @@ //Constants #define SPEED_PER_PWM 1 #define DISTANCE_PER_VALUE 1 +<<<<<<< .mine #define PI 3.14159265358979323846f #define CYCLES_PER_CORRECTION 200 +#define EMPTY_FLOAT 81188.1484f +======= +#define PI 3.14159265358979323846f +#define CYCLES_PER_CORRECTION 200 +>>>>>>> .r199 #define EMPTY_FLOAT 81188.1484f diff --git a/source/Concept/Framework/main.c b/source/Concept/Framework/main.c index 7822933..306885b 100755 --- a/source/Concept/Framework/main.c +++ b/source/Concept/Framework/main.c @@ -5,6 +5,19 @@ int main() //Init our robot Robot* localRobot = new Robot(); + sleep(1); // Wait for LCD-Display + + //Init Displays + for(uint8 i = IO_DISPLAY_START; i < IO_DISPLAY_END; i++) + { + Display* newDisplay = new Display(i); + localRobot->AddModule(newDisplay); + newDisplay = NULL; + } + + Display* ourDisplay = localRobot->GetModule(IO_DISPLAY_MAIN); + // ourDisplay->Print("Starting..."); ourDisplay->NewLine(); // Debug output + //Init Engines for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) { @@ -12,6 +25,7 @@ int main() localRobot->AddModule(newEngine); newEngine = NULL; } + // ourDisplay->Print("Engines Ready"); ourDisplay->NewLine(); // Debug output //Init Dribbler for(uint8 i = IO_DRIBBLER_START; i < IO_DRIBBLER_END; i++) @@ -20,6 +34,7 @@ int main() localRobot->AddModule(newDribbler); newDribbler = NULL; } + // ourDisplay->Print("Dribbler Ready"); ourDisplay->NewLine(); // Debug output //Init Kicker for(uint8 i = IO_KICKER_START; i < IO_KICKER_END; i++) @@ -28,6 +43,7 @@ int main() localRobot->AddModule(newKicker); newKicker = NULL; } + // ourDisplay->Print("Kicker Ready"); ourDisplay->NewLine(); // Debug output //Init Sensors for(uint8 i = IO_SENSOR_START; i < IO_SENSOR_END; i++) @@ -47,6 +63,7 @@ int main() IR_Sensor* newSensor = new IR_Sensor(i); localRobot->AddModule(newSensor); newSensor = NULL; + // ourDisplay->Print("SensorIR Ready"); ourDisplay->NewLine(); // Debug output break; } case IO_SENSOR_DISTANCE_0_DEG: @@ -57,6 +74,8 @@ int main() Distance_Sensor* newSensor = new Distance_Sensor(i); localRobot->AddModule(newSensor); newSensor = NULL; + // ourDisplay->Print("DistanceSensor Ready"); // Debug output + break; } case IO_SENSOR_MOUSE_LEFT: @@ -65,6 +84,8 @@ int main() Mouse_Sensor* newSensor = new Mouse_Sensor(i); localRobot->AddModule(newSensor); newSensor = NULL; + // ourDisplay->Print("MouseSensor Ready"); ourDisplay->NewLine(); // Debug output + break; } //Other cases @@ -77,6 +98,7 @@ int main() } } } + // ourDisplay->Print("Sensors Ready"); ourDisplay->NewLine(); // Debug output //Init Leds for(uint8 i = IO_LED_START; i < IO_LED_END; i++) @@ -85,14 +107,7 @@ int main() localRobot->AddModule(newLed); newLed = NULL; } - - //Init Displays - for(uint8 i = IO_DISPLAY_START; i < IO_DISPLAY_END; i++) - { - Display* newDisplay = new Display(i); - localRobot->AddModule(newDisplay); - newDisplay = NULL; - } + // ourDisplay->Print("LEDs Ready"); ourDisplay->NewLine(); // Debug output //Init Keyboards for(uint8 i = IO_KEYBOARD_START; i < IO_KEYBOARD_END; i++) @@ -101,6 +116,7 @@ int main() localRobot->AddModule(newKeyboard); newKeyboard = NULL; } + // ourDisplay->Print("Keyboard Ready"); ourDisplay->NewLine(); // Debug output //Init Position Tracker for(uint8 i = IO_POSITION_TRACKER_START; i < IO_POSITION_TRACKER_END; i++) @@ -109,6 +125,7 @@ int main() localRobot->AddModule(newPositionTracker); newPositionTracker = NULL; } + // ourDisplay->Print("PositionTracker Ready"); ourDisplay->NewLine(); // Debug output //Init Ball Tracker for(uint8 i = IO_BALL_TRACKER_START; i < IO_BALL_TRACKER_END; i++) @@ -117,6 +134,7 @@ int main() localRobot->AddModule(newBallTracker); newBallTracker = NULL; } + // ourDisplay->Print("Balltracker Ready"); ourDisplay->NewLine(); // Debug output //Init Navigators for(uint8 i = IO_NAVIGATOR_START; i < IO_NAVIGATOR_END; i++) @@ -125,63 +143,110 @@ int main() localRobot->AddModule(newNavigator); newNavigator = NULL; } - + // ourDisplay->Print("Navigator Ready"); ourDisplay->NewLine(); // Debug output + // ourDisplay->Print("All Ready"); ourDisplay->NewLine(); // Debug output + sleep(1); + ourDisplay->Clear(); IR_Sensor* ourSensor = NULL; + Distance_Sensor* distanceSensor = NULL; uint16 value = 0; int8 value2 = 0; - Display* ourDisplay = localRobot->GetModule(IO_DISPLAY_MAIN); + Keyboard* ourKeyboard = localRobot->GetModule(IO_KEYBOARD_MAIN); uint32 i = 1; Navigator* ourNavigator = localRobot->GetModule(IO_NAVIGATOR_MAIN); float rotation = 0; - ourDisplay->Print("Starting...", 1, 1); + Mouse_Sensor* mouse_left = localRobot->GetModule(IO_SENSOR_MOUSE_LEFT); + Mouse_Sensor* mouse_right = localRobot->GetModule(IO_SENSOR_MOUSE_RIGHT); + //Run while(true) { + /*ourDisplay->Print(i++,1,1); + ourDisplay->Print(mouse_left->GetXMovement(),1,2); + ourDisplay->Print(" "); + ourDisplay->Print(mouse_left->GetYMovement(),10,2); + ourDisplay->Print(" "); + ourDisplay->Print(mouse_right->GetXMovement(),1,3); + ourDisplay->Print(" "); + ourDisplay->Print(mouse_right->GetYMovement(),10,3); + ourDisplay->Print(" "); + ourDisplay->Print(" ",1,4); // clear + + 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(";"); + + + msleep(500);*/ + uint8 someInput = ourKeyboard->GetInput(); + //ourDisplay->Clear(); + ourDisplay->Print(i++,1,1); + //ourDisplay->Print("Ready to accept...", 1, 2); + ourDisplay->Print(someInput, 1, 3); + ourDisplay->PrintFloat(rotation, 1, 4); switch(someInput) { + case 0: + ourNavigator->Stop(); case 1: - ourNavigator->Drive(225.0f * PI / 180.0f, rotation, 200, rotation); + ourNavigator->Drive(225, rotation, 200, rotation); break; case 2: - ourNavigator->Drive(180.0f * PI / 180.0f, rotation, 200, rotation); + ourNavigator->Drive(180, rotation, 200, rotation); break; case 3: - ourNavigator->Drive(135.0f * PI / 180.0f, rotation, 200, rotation); + ourNavigator->Drive(135, rotation, 200, rotation); break; case 4: - ourNavigator->Drive(270.0f * PI / 180.0f, rotation, 200, rotation); + ourNavigator->Drive(270, rotation, 200, rotation); break; case 5: - ourNavigator->Stop(); + ourNavigator->Drive(0, rotation, 0, rotation); break; case 6: - ourNavigator->Drive(90.0f * PI / 180.0f, rotation, 200, rotation); + ourNavigator->Drive(90, rotation, 200, rotation); break; case 7: - ourNavigator->Drive(315.0f * PI / 180.0f, rotation, 200, rotation); + ourNavigator->Drive(315, rotation, 200, rotation); break; case 8: - ourNavigator->Drive(0.0f * PI / 180.0f, rotation, 200, rotation); + ourNavigator->Drive(0, rotation, 200, rotation); break; case 9: - ourNavigator->Drive(45.0f * PI / 180.0f, rotation, 200, rotation); + ourNavigator->Drive(45, rotation, 200, rotation); break; case 10: - rotation -= 10; + rotation += 10; break; case 12: - rotation += 10; + rotation -= 10; break; } - msleep(50);/* - ourDisplay->Clear(); - ourDisplay->Print(i++); + + msleep(50); + + /* + //ourDisplay->Clear(); + ourDisplay->Print(i++,1,1); ourSensor = localRobot->GetModule(IO_SENSOR_IR_0_DEG); value = ourSensor->GetIRIntensity(); ourDisplay->Print(value, 1, 2); @@ -213,8 +278,10 @@ 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); diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index ceeda60..47b1c69 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -1,3 +1,138 @@ +<<<<<<< .mine +#include "navigator.h" + +//----------------------------------------------------------------------------- +void Navigator::Stop() +{ + this->direction = 0; + this->targetAngle = 0; + //this->targetX = EMPTY_FLOAT; + //this->targetY = EMPTY_FLOAT; + this->robotSpeed = 0; + this->rotationSpeed = 0; + + /*for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) + { + (parent->GetModule(i))->SetSpeed(0); + (parent->GetModule(i))->SetEnabled(true); + }*/ +} + +//----------------------------------------------------------------------------- +void Navigator::Rotate(float newTargetAngle,float newRotationSpeed) +{ + this->rotationSpeed = min(newRotationSpeed, 255.0f);; + this->direction = 0; + this->targetAngle = newTargetAngle; + //this->targetX = EMPTY_FLOAT; + //this->targetY = EMPTY_FLOAT; + this->robotSpeed = 0; + + /*for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) + { + (parent->GetModule(i))->SetSpeed(rotationSpeed); + (parent->GetModule(i))->SetEnabled(true); + }*/ +} + +//----------------------------------------------------------------------------- +void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed) +{ + this->rotationSpeed = min(rotationSpeed, 255.0f); + this->direction = newDirection*PI/180; + this->targetAngle = newAngle; + this->targetX = EMPTY_FLOAT; + this->targetY = EMPTY_FLOAT; + this->robotSpeed = newSpeed; + + if(targetAngle == EMPTY_FLOAT) + { + rotationSpeed = 0; + } + else + { + rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - (parent->GetModule(IO_POSITION_TRACKER_MAIN))->GetOrientation())); + } + + CalculateEngines(); +} + +//----------------------------------------------------------------------------- +void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed) +{ + if(newX < 0 || newY < 0) return; + + Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); + + this->rotationSpeed = min(rotationSpeed, 255.0f); + this->targetX = newX; + this->targetY = newY; + this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);; + this->targetAngle = newAngle; + this->robotSpeed = newSpeed; + + if(targetAngle - locationeer->GetOrientation() > PI) + { + if(rotationSpeed > 0) + { + rotationSpeed = -rotationSpeed; + } + } + else + { + if(rotationSpeed < 0) + { + rotationSpeed = -rotationSpeed; + } + } + + CalculateDirection(); +} + +//----------------------------------------------------------------------------- +void Navigator::Update() +{ + 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) + { + targetX = EMPTY_FLOAT; + targetY = EMPTY_FLOAT; + direction = EMPTY_FLOAT; + robotSpeed = 0; + + targetReached = true; + } + + if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f) + { + targetAngle = EMPTY_FLOAT; + rotationSpeed = 0; + + targetAngleReached = true; + } + + if(targetReached && targetAngleReached) + { + Stop(); + } + else if(targetReached || targetAngleReached) + { + CalculateDirection(); + } + + if(!(correctionCountdown--)) + { + CalculateDirection(); + } +} + +//----------------------------------------------------------------------------- +void Navigator::CalculateDirection() +======= #include "navigator.h" //----------------------------------------------------------------------------- @@ -19,7 +154,135 @@ void Navigator::Stop() //----------------------------------------------------------------------------- void Navigator::Rotate(float rotationSpeed) +>>>>>>> .r199 { +<<<<<<< .mine + Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); + correctionCountdown = CYCLES_PER_CORRECTION; + + if(targetAngle == EMPTY_FLOAT && !HasTarget()) return; + + if(HasTarget()) + { + direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY); + } + + if(targetAngle != EMPTY_FLOAT) + { + rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - locationeer->GetOrientation())); + } + + CalculateEngines(); +} + +//----------------------------------------------------------------------------- +void Navigator::CalculateEngines() +{ + 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; + + float maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight)); + + float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + + vBack * (vBack + vLeft + vRight)); + + if(calcSpeed != 1.0f) + { + speedCorrection = 1.0f / calcSpeed; + } + + float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection; + if(maxOverallSpeed > maxEngineSpeed) + { + robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection); + } + + + vLeft = vLeft * this->robotSpeed * speedCorrection; + vBack = vBack * this->robotSpeed * speedCorrection; + vRight = vRight * this->robotSpeed * speedCorrection; + + maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight)); + 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(); + } +} +======= this->rotationSpeed = min(rotationSpeed, 255.0f);; this->direction = EMPTY_FLOAT; this->targetAngle = EMPTY_FLOAT; @@ -257,3 +520,4 @@ void Navigator::CalculateEngines() Stop(); } } +>>>>>>> .r199 diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h index 56e7b8f..848c9fa 100755 --- a/source/Concept/Framework/modules/executor/navigator.h +++ b/source/Concept/Framework/modules/executor/navigator.h @@ -1,3 +1,75 @@ +<<<<<<< .mine +#ifndef _NAVIGATOR_H +#define _NAVIGATOR_H + +//#include +#include "../../stdafx.h" + +class Navigator : public IO_Module +{ +public: + Navigator() + { + this->parent = NULL; + this->moduleId = 0; + this->correctionCountdown = CYCLES_PER_CORRECTION; + this->direction = EMPTY_FLOAT; + this->targetAngle = EMPTY_FLOAT; + this->targetX = EMPTY_FLOAT; + this->targetY = EMPTY_FLOAT; + this->robotSpeed = 0; + this->rotationSpeed = 0; + } + + Navigator(uint32 navigatorId) + { + this->parent = NULL; + this->moduleId = navigatorId; + this->correctionCountdown = CYCLES_PER_CORRECTION; + this->direction = EMPTY_FLOAT; + this->targetAngle = EMPTY_FLOAT; + this->targetX = EMPTY_FLOAT; + this->targetY = EMPTY_FLOAT; + this->robotSpeed = 0; + this->rotationSpeed = 0; + } + +protected: + uint16 correctionCountdown; + float direction; + float targetAngle; + float targetX; + float targetY; + float robotSpeed; + float rotationSpeed; + +public: + void Update(); + + void Stop(); + + void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed); + + void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed); + + void Rotate(float rotationSpeed); + + void SetSpeed(float newSpeed) + { + this->robotSpeed = newSpeed; + } + + void CalculateDirection(); + + void CalculateEngines(); + + bool HasTarget() + { + return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT); + } +}; + +======= #ifndef _NAVIGATOR_H #define _NAVIGATOR_H @@ -68,4 +140,5 @@ public: } }; +>>>>>>> .r199 #endif diff --git a/source/Concept/Framework/modules/input/distance_sensor.c b/source/Concept/Framework/modules/input/distance_sensor.c index 977784f..508eeed 100755 --- a/source/Concept/Framework/modules/input/distance_sensor.c +++ b/source/Concept/Framework/modules/input/distance_sensor.c @@ -9,24 +9,24 @@ float Distance_Sensor::GetDistance() *hardwareDDR |= pin;//Set pin output *hardwarePort |= pin;//Activate port usleep(10);//Wait for 10µs - //*hardwarePort &= ~pin;//Deactivate port + *hardwarePort &= ~pin;//Deactivate port *hardwareDDR &= ~pin;//Set pin input - (parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1); //Wait for response - while(!(PINC & pin)){asm volatile("nop");} + for(int i=0;(!(PINC & pin))&&(i < 1000);i++) {asm volatile("nop");} - (parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1); //Calculate duration of response - while(*hardwarePin & pin) + while((*hardwarePin & pin)&&(result < 300000)) { result++; asm volatile("nop"); } - (parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1); return (float(result) * DISTANCE_PER_VALUE); } diff --git a/source/Concept/Framework/modules/input/mouse_sensor.h b/source/Concept/Framework/modules/input/mouse_sensor.h index 30f9d53..460e387 100755 --- a/source/Concept/Framework/modules/input/mouse_sensor.h +++ b/source/Concept/Framework/modules/input/mouse_sensor.h @@ -42,9 +42,9 @@ public: this->pinSCK = (1 << 6); this->registerConfig = 0x00; this->registerPixelData = 0x08; - this->registerSqual = 0x4; - this->registerDeltaX = 0x3; - this->registerDeltaY = 0x2; + this->registerSqual = 0x04; + this->registerDeltaX = 0x03; + this->registerDeltaY = 0x02; this->configReset = 0x80; this->configAwake = 0x01; break; @@ -52,13 +52,13 @@ public: this->hardwarePort = &PORTC; this->hardwareDDR = &DDRC; this->hardwarePin = &PINC; - this->pinSDA = (1 << 5); - this->pinSCK = (1 << 7); + this->pinSDA = (1 << 7); + this->pinSCK = (1 << 5); this->registerConfig = 0x00; this->registerPixelData = 0x08; - this->registerSqual = 0x4; - this->registerDeltaX = 0x3; - this->registerDeltaY = 0x2; + this->registerSqual = 0x04; + this->registerDeltaX = 0x03; + this->registerDeltaY = 0x02; this->configReset = 0x80; this->configAwake = 0x01; break; diff --git a/source/Concept/Framework/modules/output/display.h b/source/Concept/Framework/modules/output/display.h index e221120..bbfa292 100755 --- a/source/Concept/Framework/modules/output/display.h +++ b/source/Concept/Framework/modules/output/display.h @@ -39,7 +39,7 @@ public: this->settingIllumination = 76; this->settingCursorPosition = 79; msleep(100); - uart1_init(103);//9600 BAUD at 16MHz Atmel + uart1_init(51);//19200 BAUD at 16MHz Atmel msleep(100); break; default: @@ -52,6 +52,8 @@ public: this->settingCursorPosition = 0; break; } + + Clear(); } protected: @@ -137,6 +139,7 @@ public: void NewLine() { SendCommand(commandNewLine); + ReturnCursor(); } bool GetCursorVisible() diff --git a/source/Concept/Framework/modules/output/engine.h b/source/Concept/Framework/modules/output/engine.h index 27b9905..7b7a044 100755 --- a/source/Concept/Framework/modules/output/engine.h +++ b/source/Concept/Framework/modules/output/engine.h @@ -58,7 +58,7 @@ public: protected: bool enabled; - float curSpeed; + int curSpeed; //Hardware volatile uint8* hardwarePort; @@ -94,16 +94,19 @@ protected: } public: - float GetSpeed() + int GetSpeed() { return curSpeed; } - void SetSpeed(float newSpeed) + void SetSpeed(int newSpeed) { curSpeed = newSpeed; - *pwmSpeed = (abs((int16)(newSpeed / SPEED_PER_PWM))); + int pwm = abs(newSpeed); + if(pwm > 255) pwm = 255; + + *pwmSpeed = pwm; UpdateDirection(); } diff --git a/source/Concept/Framework/robot.c b/source/Concept/Framework/robot.c index d8ea1c2..0e4e9ac 100755 --- a/source/Concept/Framework/robot.c +++ b/source/Concept/Framework/robot.c @@ -13,8 +13,8 @@ Robot::Robot() DDRB = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7); PORTB = (1 << 1); - //All output except PC4/PC5 (mousesensor SDA) - DDRC = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 6) | (1 << 7); + //All output except PC4/PC7 (mousesensor SDA) + DDRC = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 5) | (1 << 6); PORTC = 0; //All output except PD0+1(I2C) + 2+3(RS232) @@ -101,8 +101,8 @@ uint16 Robot::GetADCValue(uint8 channel) uint32 result = 0; - //Activate ADC and set division factor to 8 - ADCSRA = (1 << ADEN) | (1 << ADPS1) | (1 << ADPS0); + //Activate ADC and set division factor to 128 + ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); //Set multiplexer channel ADMUX = channel; diff --git a/source/Concept/Framework/tools.h b/source/Concept/Framework/tools.h index ca19993..25a63f2 100644 --- a/source/Concept/Framework/tools.h +++ b/source/Concept/Framework/tools.h @@ -33,9 +33,8 @@ inline void msleep(int msec) inline void usleep(int usec) { - usec *= 100; for (int s=0; s