diff --git a/source/Concept/Framework/RoboCode.aps b/source/Concept/Framework/RoboCode.aps index dc61dc5..4822e36 100644 --- a/source/Concept/Framework/RoboCode.aps +++ b/source/Concept/Framework/RoboCode.aps @@ -1 +1 @@ -RoboCode16-Feb-2007 15:16:4617-Feb-2007 14:21:10241016-Feb-2007 15:16:4644, 12, 0, 462AVR GCCdefault\RoboCode.elfY:\Concept\Framework\falseR00R01R02R03R04R05R06R07R08R09R10R11R12R13R14R15R16R17R18R19R20R21R22R23R24R25R26R27R28R29R30R31000main.cppsensor.cpptools.cppatmega128io.cppdistance_sensor.cppengine.cppio_module.cppir_sensor.cppkicker.cppled.cpprobot.cppdribbler.cppdisplay.cppmouse_sensor.cppkeyboard.cppposition_tracker.cppsensor.hstdafx.htools.hatmega128io.hdefines.hdistance_sensor.hengine.hio_module.hir_sensor.hkicker.hled.hrobot.hdribbler.hdisplay.hmouse_sensor.hkeyboard.hposition_tracker.hdefault\MakefiledefaultYESdefault\Makefileatmega128100RoboCode.elfdefault\1-Wall -gdwarf-2 -O0 -fsigned-chardefault1C:\WinAVR\bin\avr-gcc.exeC:\WinAVR\utils\bin\make.exeY:\Concept\Framework\sensor.hY:\Concept\Framework\stdafx.hY:\Concept\Framework\tools.hY:\Concept\Framework\atmega128io.hY:\Concept\Framework\defines.hY:\Concept\Framework\distance_sensor.hY:\Concept\Framework\engine.hY:\Concept\Framework\io_module.hY:\Concept\Framework\ir_sensor.hY:\Concept\Framework\kicker.hY:\Concept\Framework\led.hY:\Concept\Framework\robot.hY:\Concept\Framework\dribbler.hY:\Concept\Framework\display.hY:\Concept\Framework\mouse_sensor.hY:\Concept\Framework\keyboard.hY:\Concept\Framework\main.cppY:\Concept\Framework\sensor.cppY:\Concept\Framework\tools.cppY:\Concept\Framework\atmega128io.cppY:\Concept\Framework\distance_sensor.cppY:\Concept\Framework\engine.cppY:\Concept\Framework\io_module.cppY:\Concept\Framework\ir_sensor.cppY:\Concept\Framework\kicker.cppY:\Concept\Framework\led.cppY:\Concept\Framework\robot.cppY:\Concept\Framework\dribbler.cppY:\Concept\Framework\display.cppY:\Concept\Framework\mouse_sensor.cppY:\Concept\Framework\keyboard.cpp00000stdafx.h100001engine.h100002defines.h100003io_module.h100004robot.h100005robot.cpp100006atmega128io.h100007main.cpp100008tools.cpp100009tools.h100010distance_sensor.h100011ir_sensor.h100012kicker.h100013led.h100014sensor.h100015dribbler.h100016kicker.cpp100017dribbler.cpp100018ir_sensor.cpp100019io_module.cpp100020engine.cpp100021distance_sensor.cpp100022atmega128io.cpp100023led.cpp100024sensor.cpp100025display.h100026display.cpp100027mouse_sensor.h100028mouse_sensor.cpp100029keyboard.h100030keyboard.cpp100031position_tracker.h100032position_tracker.cpp1299 74 1025 52920 29344 135 944 381111 2366 157 966 403148 6388 179 988 42516 0410 201 1010 44719 0300 91 900 33797 44344 135 944 38118 0296 70 1022 525149 41388 179 988 4252 0410 201 1010 44732 17297 98 927 40319 33319 127 949 43257 7341 156 971 46129 0363 185 993 49058 0385 214 1015 51917 22297 98 927 40358 16341 156 971 4611 0363 185 993 4900 0385 214 1015 5199 0297 98 927 4031 0319 127 949 4321 0341 156 971 4610 0363 185 993 490511 0385 214 1015 5191 0297 98 927 4030 0300 100 930 405170 2344 158 974 4630 0347 160 977 465198 38391 218 1021 5231 0303 102 933 40734 10347 160 977 4651 0299 72 1025 52717 28Maximized303 102 933 4071 0 +RoboCode16-Feb-2007 15:16:4617-Feb-2007 21:48:40241016-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\1modules\modules\executor\modules\input\modules\interpreter\modules\logic\modules\output\-Wall -gdwarf-2 -O3 -fsigned-char default0C:\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.h100003robot.cpp100004atmega128io.h100005main.cpp100006tools.cpp100007tools.h100008atmega128io.cpp100009modules\interpreter\position_tracker.cpp100010modules\interpreter\position_tracker.h100011modules\io_module.h100012modules\output\display.h100013modules\input\ir_sensor.h100014modules\output\engine.h100015modules\input\sensor.h100016modules\input\keyboard.h100017modules\io_module.c100018robot.c100019modules\input\ir_sensor.c100020modules\output\dribbler.h100021modules\output\kicker.h100022modules\output\led.h100023modules\input\distance_sensor.h100024modules\input\mouse_sensor.h100025modules\input\sensor.c100026main.c100027modules\interpreter\ball_tracker.h100028modules\interpreter\ball_tracker.c100029modules\interpreter\position_tracker.c100030modules\input\distance_sensor.c100031modules\output\engine.c100032modules\output\dribbler.c100033modules\executor\navigator.h100034modules\executor\navigator.c1310 73 1036 52822 2232650 32233 33250 32479155 1932672 32255 33272 3250145 032694 32277 33294 3252316 032716 32299 33316 3254519 032606 32189 33206 3243597 032650 32233 33250 3247917 032602 32168 33328 326234 1732694 32277 33294 325232 0399 216 1025 5174 0311 100 941 40531 2336 131 966 4366 0358 160 988 46542 16380 189 1010 49417 2402 218 1032 52322 12314 102 944 4073 16336 131 966 43618 9358 160 988 4651 0380 189 1010 49491 34402 218 1032 5230 0314 102 944 40724 13336 131 966 4363 16358 160 988 4653 16380 189 1010 49447 30402 218 1032 523187 20314 102 944 4071 0336 131 966 436125 14358 160 988 46519 25380 189 1010 49467 38402 218 1032 5237 0314 102 944 40727 0336 131 966 4360 0358 160 988 4650 0402 218 1021 52335 15310 72 1025 52778 34Maximized diff --git a/source/Concept/Framework/atmega128io.cpp b/source/Concept/Framework/atmega128io.c old mode 100644 new mode 100755 similarity index 100% rename from source/Concept/Framework/atmega128io.cpp rename to source/Concept/Framework/atmega128io.c diff --git a/source/Concept/Framework/default/Makefile b/source/Concept/Framework/default/Makefile index e8aa72d..d0fbfb0 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 -O3 -fsigned-char CFLAGS += -MD -MP -MT $(*F).o -MF dep/$(@F).d ## Assembly specific flags @@ -33,8 +33,11 @@ HEX_EEPROM_FLAGS += --set-section-flags=.eeprom="alloc,load" 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" + ## Objects that must be built in order to link -OBJECTS = main.o sensor.o tools.o atmega128io.o display.o keyboard.o distance_sensor.o mouse_sensor.o engine.o dribbler.o io_module.o ir_sensor.o kicker.o led.o robot.o position_tracker.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 ## Objects explicitly added by the user LINKONLYOBJECTS = @@ -43,49 +46,58 @@ LINKONLYOBJECTS = all: $(TARGET) RoboCode.hex RoboCode.eep size ## Compile -main.o: ../main.cpp +io_module.o: ../modules/io_module.c $(CC) $(INCLUDES) $(CFLAGS) -c $< -tools.o: ../tools.cpp +atmega128io.o: ../atmega128io.c $(CC) $(INCLUDES) $(CFLAGS) -c $< -io_module.o: ../io_module.cpp +main.o: ../main.c $(CC) $(INCLUDES) $(CFLAGS) -c $< -robot.o: ../robot.cpp +robot.o: ../robot.c $(CC) $(INCLUDES) $(CFLAGS) -c $< -sensor.o: ../sensor.cpp +tools.o: ../tools.c $(CC) $(INCLUDES) $(CFLAGS) -c $< -engine.o: ../engine.cpp +distance_sensor.o: ../modules/input/distance_sensor.c $(CC) $(INCLUDES) $(CFLAGS) -c $< -led.o: ../led.cpp +ir_sensor.o: ../modules/input/ir_sensor.c $(CC) $(INCLUDES) $(CFLAGS) -c $< -display.o: ../display.cpp +keyboard.o: ../modules/input/keyboard.c $(CC) $(INCLUDES) $(CFLAGS) -c $< -keyboard.o: ../keyboard.cpp +mouse_sensor.o: ../modules/input/mouse_sensor.c $(CC) $(INCLUDES) $(CFLAGS) -c $< -ir_sensor.o: ../ir_sensor.cpp +sensor.o: ../modules/input/sensor.c $(CC) $(INCLUDES) $(CFLAGS) -c $< -distance_sensor.o: ../distance_sensor.cpp +position_tracker.o: ../modules/interpreter/position_tracker.c $(CC) $(INCLUDES) $(CFLAGS) -c $< -mouse_sensor.o: ../mouse_sensor.cpp +display.o: ../modules/output/display.c $(CC) $(INCLUDES) $(CFLAGS) -c $< -kicker.o: ../kicker.cpp +dribbler.o: ../modules/output/dribbler.c $(CC) $(INCLUDES) $(CFLAGS) -c $< -dribbler.o: ../dribbler.cpp +engine.o: ../modules/output/engine.c $(CC) $(INCLUDES) $(CFLAGS) -c $< -atmega128io.o: ../atmega128io.cpp +kicker.o: ../modules/output/kicker.c + $(CC) $(INCLUDES) $(CFLAGS) -c $< + +led.o: ../modules/output/led.c + $(CC) $(INCLUDES) $(CFLAGS) -c $< + +ball_tracker.o: ../modules/interpreter/ball_tracker.c + $(CC) $(INCLUDES) $(CFLAGS) -c $< + +navigator.o: ../modules/executor/navigator.c $(CC) $(INCLUDES) $(CFLAGS) -c $< ##Link diff --git a/source/Concept/Framework/defines.h b/source/Concept/Framework/defines.h index 2bc59d9..73505e2 100644 --- a/source/Concept/Framework/defines.h +++ b/source/Concept/Framework/defines.h @@ -58,6 +58,7 @@ //Constants #define SPEED_PER_PWM 1 #define DISTANCE_PER_VALUE 1 +#define PI 3.14159265f //IO Module Names enum IOModuleNames @@ -142,8 +143,24 @@ enum IOModuleNames IO_POSITION_TRACKER_END, + //Position Tracker + + IO_BALL_TRACKER_START = IO_POSITION_TRACKER_END, + + IO_BALL_TRACKER_MAIN, + + IO_BALL_TRACKER_END, + + //Navigators + + IO_NAVIGATOR_START = IO_BALL_TRACKER_END, + + IO_NAVIGATOR_MAIN, + + IO_NAVIGATOR_END, + //General - IO_END = IO_POSITION_TRACKER_END, + IO_END = IO_NAVIGATOR_END, }; #endif diff --git a/source/Concept/Framework/main.cpp b/source/Concept/Framework/main.c old mode 100644 new mode 100755 similarity index 75% rename from source/Concept/Framework/main.cpp rename to source/Concept/Framework/main.c index f689d88..fa6d718 --- a/source/Concept/Framework/main.cpp +++ b/source/Concept/Framework/main.c @@ -102,11 +102,38 @@ int main() newKeyboard = NULL; } - Keyboard* ourKeyboard = localRobot->GetModule(IO_KEYBOARD_MAIN); + //Init Position Tracker + for(uint8 i = IO_POSITION_TRACKER_START; i < IO_POSITION_TRACKER_END; i++) + { + Position_Tracker* newPositionTracker = new Position_Tracker(i); + localRobot->AddModule(newPositionTracker); + newPositionTracker = NULL; + } + + //Init Ball Tracker + for(uint8 i = IO_BALL_TRACKER_START; i < IO_BALL_TRACKER_END; i++) + { + Ball_Tracker* newBallTracker = new Ball_Tracker(i); + localRobot->AddModule(newBallTracker); + newBallTracker = NULL; + } + + //Init Navigators + for(uint8 i = IO_NAVIGATOR_START; i < IO_NAVIGATOR_END; i++) + { + Navigator* newNavigator = new Navigator(i); + localRobot->AddModule(newNavigator); + newNavigator = NULL; + } + + + IR_Sensor* ourSensor = NULL; uint16 value = 0; + int8 value2 = 0; Display* ourDisplay = localRobot->GetModule(IO_DISPLAY_MAIN); uint32 i = 1; + Ball_Tracker* ourBallTracker = localRobot->GetModule(IO_BALL_TRACKER_MAIN); //Run while(true) @@ -114,7 +141,6 @@ int main() msleep(500); ourDisplay->Clear(); ourDisplay->Print(i++); - ourDisplay->NewLine(); ourSensor = localRobot->GetModule(IO_SENSOR_IR_0_DEG); value = ourSensor->GetIRIntensity(); ourDisplay->Print(value, 1, 2); @@ -147,9 +173,14 @@ int main() value = ourSensor->GetIRIntensity(); ourDisplay->Print(value); ourDisplay->Print(";"); - ourDisplay->Print(ourKeyboard->GetInput(), 1, 4); + value2 = (localRobot->GetModule(IO_SENSOR_MOUSE_LEFT))->GetXMovement(); + ourDisplay->Print(value2, 10, 4); + value2 = (localRobot->GetModule(IO_SENSOR_MOUSE_LEFT))->GetYMovement(); + ourDisplay->Print(value2, 15, 4); localRobot->Update(); + + ourDisplay->PrintFloat(ourBallTracker->GetBallDirection(), 1, 4); } //Cleanup diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c new file mode 100755 index 0000000..459d4c1 --- /dev/null +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -0,0 +1,199 @@ +#include "navigator.h" + +//----------------------------------------------------------------------------- +void Navigator::Stop() +{ + this->direction = -1.0f; + this->targetAngle = -1.0f; + this->targetX = -1.0f; + this->targetY = -1.0f; + this->speed = 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 rotationSpeed) +{ + this->rotationSpeed = rotationSpeed; + this->direction = -1.0f; + this->targetAngle = -1.0f; + this->targetX = -1.0f; + this->targetY = -1.0f; + this->speed = 0; + + for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) + { + (parent->GetModule(i))->SetSpeed(rotationSpeed); + (parent->GetModule(i))->SetEnabled(true); + } +} + +//----------------------------------------------------------------------------- +void Navigator::Update() +{ + Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); + + bool hasDistanceToDrive = true; + bool hasOrientationToAdjust = true; + + //Check for distance to drive + if((targetX >= 0) != (targetY >= 0)) + { + targetX = -1.0f; + targetY = -1.0f; + + hasDistanceToDrive = false; + } + else if(targetX >= 0 && targetY >= 0) + { + if(distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f) + { + targetX = -1.0f; + targetY = -1.0f; + + hasDistanceToDrive = false; + } + else + { + hasDistanceToDrive = true; + } + } + else + { + if(direction >= 0) + { + hasDistanceToDrive = true; + } + else + { + hasDistanceToDrive = false; + } + } + + //Check for orientation to adjust + if(targetAngle >= 0) + { + if(fabs(targetAngle - locationeer->GetOrientation()) < 0.1f) + { + hasOrientationToAdjust = false; + } + else + { + hasOrientationToAdjust = true; + } + } + else + { + if(rotationSpeed != 0) + { + hasOrientationToAdjust = true; + } + else + { + hasOrientationToAdjust = false; + } + } + + //Calculate directional/rotational engine speed + if(hasDistanceToDrive) + { + + + float maxRobotSpeed = 255.0f * sqrt(2) / 2.0f; + + if(speed > maxRobotSpeed) + { + speed = maxRobotSpeed; + } + + maxMotorSpeed = speed / (sqrt(2) / 2); + + + + relAngel = direction - orientation + + robotSpeed = sin(45) * maxMotorSpeed + maxMotorSpeed = robotSpeed / sin(45) + + if(relAngel > 45) + { + sin(relAngel) * (speed / sin(45)) * sin(relAngel + 45); + + back = speed / sin(relAngel); + } + else + { + + } + + left = + back = sin(relAngel) * speed + + + direction = 0: + orientation = 0: + left = speed + right = -speed + back = 0 + + direction = 0: + orientation = 90: + left = speed + right = speed + back = (sinVcos)(45) * speed + + direction = 0: + orientation = 45: + left = speed + right = 0 + back = -speed + + direction = 0: + orientation = 180: + left = -speed + right = speed + back = 0 + + } + else if(!hasOrientationToAdjust) + { + Stop(); + } + else + { + } +} + +// Aktualieren ohne Parameter +/*void Navigator::Update() { + // Richtung in x und y-Kompontente zerlegen + double y = cos((double)direction*0.01745); // richtung ist winkel + double x = sin((double)direction*0.01745); + + // Abweichung der Ausrichtung ermitteln(als winkel) + int w = sensor.GetAusrichtung() - ausrichtung; + + // Stärke der einzelnen Motoren berechnen + double v0 = (-x+sqrt(3)*y)/2; + double v1 = x; + double v2 = (-x-sqrt(3)*y)/2; + + // Ausgerechnete Stärke an die Motoren übergeben + board.motor(0,(int)((double)v0*speed +w)); + board.motor(1,(int)((double)v1*speed +w)); + board.motor(2,(int)((double)v2*speed +w)); +} + +// Aktualieren mit allen Parametern +void Navigator::Drive(float newDirection, float newAngle, float newSpeed) { + SetDirection(newDirection); + SetAngle(newAngle); + SetSpeed(newSpeed); + Update(); // Und anwenden +}*/ diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h new file mode 100755 index 0000000..ed8b3c8 --- /dev/null +++ b/source/Concept/Framework/modules/executor/navigator.h @@ -0,0 +1,53 @@ +#ifndef _NAVIGATOR_H +#define _NAVIGATOR_H + +#include "../../stdafx.h" + +class Navigator : public IO_Module +{ +public: + Navigator() + { + this->parent = NULL; + this->moduleId = 0; + this->direction = -1.0f; + this->targetAngle = -1.0f; + this->targetX = -1.0f; + this->targetY = -1.0f; + this->speed = 0; + this->rotationSpeed = 0; + } + + Navigator(uint32 navigatorId) + { + this->parent = NULL; + this->moduleId = navigatorId; + this->direction = -1.0f; + this->targetAngle = -1.0f; + this->targetX = -1.0f; + this->targetY = -1.0f; + this->speed = 0; + this->rotationSpeed = 0; + } + +protected: + float direction; + float targetAngle; + float targetX; + float targetY; + float speed; + 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); + + void Rotate(float rotationSpeed); +}; + +#endif diff --git a/source/Concept/Framework/distance_sensor.cpp b/source/Concept/Framework/modules/input/distance_sensor.c old mode 100644 new mode 100755 similarity index 60% rename from source/Concept/Framework/distance_sensor.cpp rename to source/Concept/Framework/modules/input/distance_sensor.c index c6c0582..977784f --- a/source/Concept/Framework/distance_sensor.cpp +++ b/source/Concept/Framework/modules/input/distance_sensor.c @@ -9,11 +9,15 @@ 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); + //Wait for response - while(!(*hardwarePin & pin)); + while(!(PINC & pin)){asm volatile("nop");} + + (parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1); //Calculate duration of response while(*hardwarePin & pin) @@ -22,5 +26,7 @@ float Distance_Sensor::GetDistance() asm volatile("nop"); } + (parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1); + return (float(result) * DISTANCE_PER_VALUE); } diff --git a/source/Concept/Framework/modules/input/distance_sensor.h b/source/Concept/Framework/modules/input/distance_sensor.h new file mode 100755 index 0000000..8e6aa21 --- /dev/null +++ b/source/Concept/Framework/modules/input/distance_sensor.h @@ -0,0 +1,71 @@ +#ifndef _DISTANCE_SENSOR_H +#define _DISTANCE_SENSOR_H + +#include "../../stdafx.h" +#include "sensor.h" + +class Distance_Sensor : public Sensor +{ +public: + Distance_Sensor() + { + this->parent = NULL; + this->moduleId = 0; + this->hardwarePort = NULL; + this->hardwareDDR = NULL; + this->hardwarePin = NULL; + this->pin = 0; + } + + Distance_Sensor(uint32 sensorId) + { + this->parent = NULL; + this->moduleId = sensorId; + + switch(sensorId) + { + case IO_SENSOR_DISTANCE_0_DEG: + this->hardwarePort = &PORTC; + this->hardwareDDR = &DDRC; + this->hardwarePin = &PINC; + this->pin = (1 << 0); + break; + case IO_SENSOR_DISTANCE_90_DEG: + this->hardwarePort = &PORTC; + this->hardwareDDR = &DDRC; + this->hardwarePin = &PINC; + this->pin = (1 << 1); + break; + case IO_SENSOR_DISTANCE_180_DEG: + this->hardwarePort = &PORTC; + this->hardwareDDR = &DDRC; + this->hardwarePin = &PINC; + this->pin = (1 << 2); + break; + case IO_SENSOR_DISTANCE_270_DEG: + this->hardwarePort = &PORTC; + this->hardwareDDR = &DDRC; + this->hardwarePin = &PINC; + this->pin = (1 << 3); + break; + default: + this->hardwarePort = NULL; + this->hardwareDDR = NULL; + this->hardwarePin = NULL; + this->pin = 0; + break; + } + } + +protected: + //Hardware + volatile uint8* hardwarePort; + volatile uint8* hardwareDDR; + volatile uint8* hardwarePin; + uint8 pin; + +public: + float GetDistance(); +}; + +#endif diff --git a/source/Concept/Framework/ir_sensor.cpp b/source/Concept/Framework/modules/input/ir_sensor.c old mode 100644 new mode 100755 similarity index 100% rename from source/Concept/Framework/ir_sensor.cpp rename to source/Concept/Framework/modules/input/ir_sensor.c diff --git a/source/Concept/Framework/modules/input/ir_sensor.h b/source/Concept/Framework/modules/input/ir_sensor.h new file mode 100755 index 0000000..30e6ea4 --- /dev/null +++ b/source/Concept/Framework/modules/input/ir_sensor.h @@ -0,0 +1,62 @@ +#ifndef _IR_SENSOR_H +#define _IR_SENSOR_H + +#include "../../defines.h" +#include "../../robot.h" +#include "sensor.h" + +class IR_Sensor : public Sensor +{ +public: + IR_Sensor() + { + this->parent = NULL; + this->moduleId = 0; + } + + IR_Sensor(uint32 sensorId) + { + this->parent = NULL; + this->moduleId = sensorId; + + switch(sensorId) + { + case IO_SENSOR_IR_0_DEG: + this->channel = 0; + break; + case IO_SENSOR_IR_30_DEG: + this->channel = 1; + break; + case IO_SENSOR_IR_60_DEG: + this->channel = 2; + break; + case IO_SENSOR_IR_100_DEG: + this->channel = 3; + break; + case IO_SENSOR_IR_180_DEG: + this->channel = 4; + break; + case IO_SENSOR_IR_260_DEG: + this->channel = 5; + break; + case IO_SENSOR_IR_300_DEG: + this->channel = 6; + break; + case IO_SENSOR_IR_330_DEG: + this->channel = 7; + break; + default: + this->channel = 8; + break; + } + } + +protected: + //Hardware + uint8 channel; + +public: + uint16 GetIRIntensity(); +}; + +#endif diff --git a/source/Concept/Framework/keyboard.cpp b/source/Concept/Framework/modules/input/keyboard.c old mode 100644 new mode 100755 similarity index 100% rename from source/Concept/Framework/keyboard.cpp rename to source/Concept/Framework/modules/input/keyboard.c diff --git a/source/Concept/Framework/modules/input/keyboard.h b/source/Concept/Framework/modules/input/keyboard.h new file mode 100755 index 0000000..243bb6c --- /dev/null +++ b/source/Concept/Framework/modules/input/keyboard.h @@ -0,0 +1,87 @@ +#ifndef _KEYBOARD_H +#define _KEYBOARD_H + +#include "../../stdafx.h" + +class Keyboard : public IO_Module +{ +public: + Keyboard() + { + this->parent = NULL; + this->moduleId = 0; + this->commandSetting = 0; + this->settingClearBuffer = 0; + } + + Keyboard(uint32 keyboardId) + { + this->parent = NULL; + this->moduleId = keyboardId; + + switch(keyboardId) + { + case IO_KEYBOARD_MAIN: + this->commandSetting = 27; + this->settingClearBuffer = 123; + break; + default: + this->commandSetting = 0; + this->settingClearBuffer = 0; + break; + } + } + +protected: + //Commands + uint8 commandSetting; + //Settings + uint8 settingClearBuffer; + + void SendCommand(uint8 newCommand) + { + switch(moduleId) + { + case IO_KEYBOARD_MAIN: + uart1_putc(newCommand); + break; + default: + break; + } + } + +public: + uint8 GetInput() + { + uint16 input = uart1_getc(); + + if(input == 0x100)//no data + { + return 0xEE;//empty + } + else if(input >= '0' && input <= '9') + { + return (uint8)(input - '0'); + } + else if(input == '*') + { + return 10; + } + else if(input == '#') + { + return 11; + } + else + { + return 0xFF;//unknown + } + } + + void ClearKeyBuffer() + { + SendCommand(commandSetting); + SendCommand(settingClearBuffer); + } +}; + +#endif diff --git a/source/Concept/Framework/mouse_sensor.cpp b/source/Concept/Framework/modules/input/mouse_sensor.c old mode 100644 new mode 100755 similarity index 100% rename from source/Concept/Framework/mouse_sensor.cpp rename to source/Concept/Framework/modules/input/mouse_sensor.c diff --git a/source/Concept/Framework/modules/input/mouse_sensor.h b/source/Concept/Framework/modules/input/mouse_sensor.h new file mode 100755 index 0000000..30f9d53 --- /dev/null +++ b/source/Concept/Framework/modules/input/mouse_sensor.h @@ -0,0 +1,199 @@ +#ifndef _MOUSE_SENSOR_H +#define _MOUSE_SENSOR_H + +#include "../../stdafx.h" +#include "sensor.h" + +class Mouse_Sensor : public Sensor +{ +public: + Mouse_Sensor() + { + this->parent = NULL; + this->moduleId = 0; + this->hardwarePort = NULL; + this->hardwareDDR = NULL; + this->hardwarePin = NULL; + this->pinSDA = 0; + this->pinSCK = 0; + this->registerConfig = 0; + this->registerPixelData = 0; + this->registerSqual = 0; + this->registerDeltaX = 0; + this->registerDeltaY = 0; + this->configReset = 0; + this->configAwake = 0; + this->newImage = false; + } + + Mouse_Sensor(uint32 sensorId) + { + this->parent = NULL; + this->moduleId = sensorId; + this->newImage = false; + + switch(sensorId) + { + case IO_SENSOR_MOUSE_LEFT: + this->hardwarePort = &PORTC; + this->hardwareDDR = &DDRC; + this->hardwarePin = &PINC; + this->pinSDA = (1 << 4); + this->pinSCK = (1 << 6); + this->registerConfig = 0x00; + this->registerPixelData = 0x08; + this->registerSqual = 0x4; + this->registerDeltaX = 0x3; + this->registerDeltaY = 0x2; + this->configReset = 0x80; + this->configAwake = 0x01; + break; + case IO_SENSOR_MOUSE_RIGHT: + this->hardwarePort = &PORTC; + this->hardwareDDR = &DDRC; + this->hardwarePin = &PINC; + this->pinSDA = (1 << 5); + this->pinSCK = (1 << 7); + this->registerConfig = 0x00; + this->registerPixelData = 0x08; + this->registerSqual = 0x4; + this->registerDeltaX = 0x3; + this->registerDeltaY = 0x2; + this->configReset = 0x80; + this->configAwake = 0x01; + break; + default: + this->hardwarePort = NULL; + this->hardwareDDR = NULL; + this->hardwarePin = NULL; + this->pinSDA = 0; + this->pinSCK = 0; + this->registerConfig = 0; + this->registerPixelData = 0; + this->registerSqual = 0; + this->registerDeltaX = 0; + this->registerDeltaY = 0; + this->configReset = 0; + this->configAwake = 0; + break; + } + + *hardwareDDR |= pinSCK; + *hardwarePort &= ~pinSCK; + + Write(registerConfig, configReset); + Write(registerConfig, configAwake); + } + +protected: + //Hardware + volatile uint8* hardwarePort; + volatile uint8* hardwareDDR; + volatile uint8* hardwarePin; + uint8 pinSDA; + uint8 pinSCK; + bool newImage; + //Registers and Settings + uint8 registerConfig; + uint8 registerPixelData; + uint8 registerSqual; + uint8 registerDeltaX; + uint8 registerDeltaY; + uint8 configReset; + uint8 configAwake; + +public: + void WriteByte(uint8 newByte) + { + *hardwareDDR |= pinSDA;//Set SDA output + + for(uint8 i = 0; i < 8; i++) + { + *hardwarePort &= ~pinSCK;//prepare SCK + + //write data + *hardwarePort = (*hardwarePort & (~(*hardwarePin))) | + ((newByte >> 7) * pinSDA); + + newByte = newByte << 1;//prepare next byte + asm volatile("nop"); + + *hardwarePort |= pinSCK; + } + } + + void Write(int8 adr, uint8 data) + { + WriteByte(adr | 0x80); + WriteByte(data); + usleep(100); + } + + uint8 ReadByte() + { + uint8 data=0; + + *hardwareDDR &= ~pinSDA;//Set SDA input + + for(uint8 i = 0; i < 8; i++) + { + *hardwarePort &= ~pinSCK;//Prepare data + data = data << 1; + + asm volatile("nop"); + *hardwarePort |= pinSCK;//Prepare for reading + + data |= (*hardwarePin & pinSDA) / pinSDA; + } + + return data; + } + + uint8 Read(uint8 adr) + { + WriteByte(adr); + usleep(100); + + return ReadByte(); + } + + void ImagePrepare() + { + Write(registerConfig, configAwake); + Write(registerPixelData, 0x00); + + newImage = true; + } + + uint8 ImageRead() + { + uint8 pixel = Read(registerPixelData); + if(newImage) + { + while (!(pixel & 0x80))//0x80 indicates first pixel + { + pixel=Read(registerPixelData); + } + newImage = false; + } + + return pixel; + } + + uint8 GetSqual() + { + return Read(registerSqual); + } + + int8 GetXMovement() + { + return (int8)(Read(registerDeltaX)); + } + + int8 GetYMovement() + { + return (int8)(Read(registerDeltaY)); + } +}; + +#endif diff --git a/source/Concept/Framework/modules/input/sensor.c b/source/Concept/Framework/modules/input/sensor.c new file mode 100755 index 0000000..3036159 --- /dev/null +++ b/source/Concept/Framework/modules/input/sensor.c @@ -0,0 +1 @@ +#include "sensor.h" diff --git a/source/Concept/Framework/modules/input/sensor.h b/source/Concept/Framework/modules/input/sensor.h new file mode 100755 index 0000000..e44c739 --- /dev/null +++ b/source/Concept/Framework/modules/input/sensor.h @@ -0,0 +1,27 @@ +#ifndef _SENSOR_H +#define _SENSOR_H + +#include "../../defines.h" +#include "../io_module.h" + +class Sensor : public IO_Module +{ +public: + Sensor() + { + this->parent = NULL; + this->moduleId = 0; + } + + Sensor(uint32 sensorId) + { + this->parent = NULL; + this->moduleId = sensorId; + } + +protected: + +public: +}; + +#endif diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.c b/source/Concept/Framework/modules/interpreter/ball_tracker.c new file mode 100755 index 0000000..6679d3e --- /dev/null +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.c @@ -0,0 +1,125 @@ +#include "ball_tracker.h" + +//----------------------------------------------------------------------------- +void Ball_Tracker::Update() +{ + uint8 sensorCount = (IO_SENSOR_IR_330_DEG - IO_SENSOR_IR_0_DEG) + 1; + uint16 intensity[sensorCount]; + uint8 greatestIntensity = 0; + for(uint8 i = 0; i < sensorCount; i++) + { + IR_Sensor* currentSensor = parent->GetModule(i + IO_SENSOR_IR_0_DEG); + + intensity[i] = 1023 - currentSensor->GetIRIntensity(); + + if(intensity[i] < 24) + { + intensity[i] = 0; + } + + if(intensity[i] > intensity[greatestIntensity]) + { + greatestIntensity = i; + } + } + + if(intensity[greatestIntensity]) + { + uint8 secondIntensity = 0xFF; + uint8 leftSensor = (greatestIntensity + 1) % sensorCount; + uint8 rightSensor = (greatestIntensity + sensorCount - 1) % sensorCount; + + if(intensity[leftSensor]) + { + secondIntensity = leftSensor; + } + + if(intensity[rightSensor] > intensity[leftSensor]) + { + secondIntensity = rightSensor; + } + + float mainDirection; + + switch(greatestIntensity + IO_SENSOR_IR_0_DEG) + { + case IO_SENSOR_IR_0_DEG: + mainDirection = 0; + break; + case IO_SENSOR_IR_30_DEG: + mainDirection = 1.0f * PI / 6.0f; + break; + case IO_SENSOR_IR_60_DEG: + mainDirection = 1.0f * PI / 3.0f; + break; + case IO_SENSOR_IR_100_DEG: + mainDirection = 5.0f * PI / 9.0f; + break; + case IO_SENSOR_IR_180_DEG: + mainDirection = PI; + break; + case IO_SENSOR_IR_260_DEG: + mainDirection = 13.0f * PI / 9.0f; + break; + case IO_SENSOR_IR_300_DEG: + mainDirection = 15.0f * PI / 9.0f; + break; + case IO_SENSOR_IR_330_DEG: + mainDirection = 33.0f * PI / 18.0f; + break; + default: + mainDirection = -1.0f; + return; + break; + } + + if(secondIntensity != 0xFF) + { + float secondDirection; + + switch(secondIntensity + IO_SENSOR_IR_0_DEG) + { + case IO_SENSOR_IR_0_DEG: + secondDirection = 0; + break; + case IO_SENSOR_IR_30_DEG: + secondDirection = 1.0f * PI / 6.0f; + break; + case IO_SENSOR_IR_60_DEG: + secondDirection = 1.0f * PI / 3.0f; + break; + case IO_SENSOR_IR_100_DEG: + secondDirection = 5.0f * PI / 9.0f; + break; + case IO_SENSOR_IR_180_DEG: + secondDirection = PI; + break; + case IO_SENSOR_IR_260_DEG: + secondDirection = 13.0f * PI / 9.0f; + break; + case IO_SENSOR_IR_300_DEG: + secondDirection = 15.0f * PI / 9.0f; + break; + case IO_SENSOR_IR_330_DEG: + secondDirection = 33.0f * PI / 18.0f; + break; + default: + secondDirection = -1.0f; + return; + break; + } + + direction = (intensity[greatestIntensity] * mainDirection + + intensity[secondIntensity] * secondDirection) / + (intensity[greatestIntensity] + intensity[secondIntensity]); + } + else + { + direction = mainDirection; + } + } + else + { + direction = -1.0f; + } +} diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.h b/source/Concept/Framework/modules/interpreter/ball_tracker.h new file mode 100755 index 0000000..ed8801f --- /dev/null +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.h @@ -0,0 +1,35 @@ +#ifndef _BALL_TRACKER_H +#define _BALL_TRACKER_H + +#include "../../stdafx.h" + +class Ball_Tracker : public IO_Module +{ +public: + Ball_Tracker() + { + this->parent = NULL; + this->moduleId = 0; + this->direction = -1.0f; + } + + Ball_Tracker(uint32 trackerId) + { + this->parent = NULL; + this->moduleId = trackerId; + this->direction = -1.0f; + } + +protected: + float direction; + +public: + void Update(); + + float GetBallDirection() + { + return direction; + } +}; + +#endif diff --git a/source/Concept/Framework/position_tracker.cpp b/source/Concept/Framework/modules/interpreter/position_tracker.c old mode 100644 new mode 100755 similarity index 100% rename from source/Concept/Framework/position_tracker.cpp rename to source/Concept/Framework/modules/interpreter/position_tracker.c diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h new file mode 100755 index 0000000..b31dd8f --- /dev/null +++ b/source/Concept/Framework/modules/interpreter/position_tracker.h @@ -0,0 +1,45 @@ +#ifndef _POSITION_TRACKER_H +#define _POSITION_TRACKER_H + +#include "../../stdafx.h" + +class Position_Tracker : public IO_Module +{ +public: + Position_Tracker() + { + this->parent = NULL; + this->moduleId = 0; + } + + Position_Tracker(uint32 trackerId) + { + this->parent = NULL; + this->moduleId = trackerId; + } + +protected: + float positionX; + float positionY; + float orientation; + +public: + void Update(); + + float GetPositionX() + { + return positionX; + } + + float GetPositionY() + { + return positionY; + } + + float GetOrientation() + { + return orientation; + } +}; + +#endif diff --git a/source/Concept/Framework/io_module.cpp b/source/Concept/Framework/modules/io_module.c old mode 100644 new mode 100755 similarity index 100% rename from source/Concept/Framework/io_module.cpp rename to source/Concept/Framework/modules/io_module.c diff --git a/source/Concept/Framework/modules/io_module.h b/source/Concept/Framework/modules/io_module.h new file mode 100755 index 0000000..383eefb --- /dev/null +++ b/source/Concept/Framework/modules/io_module.h @@ -0,0 +1,45 @@ +#ifndef _MODULE_H +#define _MODULE_H + +#include "../defines.h" +#include "../tools.h" + +class Robot; + +class IO_Module +{ +public: + IO_Module() + { + this->parent = NULL; + this->moduleId = 0; + } + + IO_Module(uint32 moduleId) + { + this->parent = NULL; + this->moduleId = moduleId; + } + +protected: + Robot* parent; + uint32 moduleId; + +public: + Robot* GetParent() + { + return parent; + } + + void SetParent(Robot* newParent) + { + parent = newParent; + } + + uint32 GetId() + { + return moduleId; + } +}; + +#endif diff --git a/source/Concept/Framework/display.cpp b/source/Concept/Framework/modules/output/display.c old mode 100644 new mode 100755 similarity index 100% rename from source/Concept/Framework/display.cpp rename to source/Concept/Framework/modules/output/display.c diff --git a/source/Concept/Framework/modules/output/display.h b/source/Concept/Framework/modules/output/display.h new file mode 100755 index 0000000..e221120 --- /dev/null +++ b/source/Concept/Framework/modules/output/display.h @@ -0,0 +1,198 @@ +#ifndef _DISPLAY_H +#define _DISPLAY_H + +#include "../../stdafx.h" + +class Display : public IO_Module +{ +public: + Display() + { + this->parent = NULL; + this->moduleId = 0; + this->cursorVisible = false; + this->illuminationEnabled = true; + this->commandClear = 0; + this->commandReturnCursor = 0; + this->commandNewLine = 0; + this->commandSetting = 0; + this->settingCursorVisible = 0; + this->settingIllumination = 0; + this->settingCursorPosition = 0; + } + + Display(uint32 displayId) + { + this->parent = NULL; + this->moduleId = displayId; + this->cursorVisible = false; + this->illuminationEnabled = true; + + switch(displayId) + { + case IO_DISPLAY_MAIN: + this->commandClear = 12; + this->commandReturnCursor = 13; + this->commandNewLine = 10; + this->commandSetting = 27; + this->settingCursorVisible = 67; + this->settingIllumination = 76; + this->settingCursorPosition = 79; + msleep(100); + uart1_init(103);//9600 BAUD at 16MHz Atmel + msleep(100); + break; + default: + this->commandClear = 0; + this->commandReturnCursor = 0; + this->commandNewLine = 0; + this->commandSetting = 0; + this->settingCursorVisible = 0; + this->settingIllumination = 0; + this->settingCursorPosition = 0; + break; + } + } + +protected: + bool cursorVisible; + bool illuminationEnabled; + //Commands + uint8 commandClear; + uint8 commandReturnCursor; + uint8 commandNewLine; + uint8 commandSetting; + //Settings + uint8 settingCursorVisible; + uint8 settingIllumination; + uint8 settingCursorPosition; + + void SendCommand(uint8 newCommand) + { + switch(moduleId) + { + case IO_DISPLAY_MAIN: + uart1_putc(newCommand); + break; + default: + break; + } + } + +public: + void Print(char* newString) + { + switch(moduleId) + { + case IO_DISPLAY_MAIN: + uart1_puts(newString); + break; + default: + break; + } + } + + void Print(int32 newInteger) + { + char buffer[12]; + ltoa(newInteger, buffer, 10); + Print(buffer); + } + + void PrintFloat(float newFloat) + { + Print((int32)(newFloat)); + Print("."); + Print(abs((uint32)(newFloat - float((int32)(newFloat)) * 100000))); + } + + void Print(char* newString, uint8 xPos, uint8 yPos) + { + SetCursorPosition(xPos, yPos); + Print(newString); + } + + void Print(int32 newInteger, uint8 xPos, uint8 yPos) + { + SetCursorPosition(xPos, yPos); + Print(newInteger); + } + + void PrintFloat(float newFloat, uint8 xPos, uint8 yPos) + { + SetCursorPosition(xPos, yPos); + PrintFloat(newFloat); + } + + void Clear() + { + SendCommand(commandClear); + } + + void ReturnCursor() + { + SendCommand(commandReturnCursor); + } + + void NewLine() + { + SendCommand(commandNewLine); + } + + bool GetCursorVisible() + { + return cursorVisible; + } + + void SetCursorVisible(bool newStatus) + { + cursorVisible = newStatus; + + SendCommand(commandSetting); + SendCommand(settingCursorVisible); + + if(cursorVisible) + { + SendCommand(1); + } + else + { + SendCommand(0); + } + } + + bool GetLightingEnabled() + { + return illuminationEnabled; + } + + void SetLightingEnabled(bool newStatus) + { + illuminationEnabled = newStatus; + + SendCommand(commandSetting); + SendCommand(settingIllumination); + + if(illuminationEnabled) + { + SendCommand(1); + } + else + { + SendCommand(0); + } + } + + void SetCursorPosition(uint8 newX, uint8 newY) + { + if(!newX || newX > 20) return; + if(!newY || newY > 4) return; + + SendCommand(commandSetting); + SendCommand(settingCursorPosition); + SendCommand(newX); + SendCommand(newY); + } +}; + +#endif diff --git a/source/Concept/Framework/dribbler.cpp b/source/Concept/Framework/modules/output/dribbler.c old mode 100644 new mode 100755 similarity index 100% rename from source/Concept/Framework/dribbler.cpp rename to source/Concept/Framework/modules/output/dribbler.c diff --git a/source/Concept/Framework/modules/output/dribbler.h b/source/Concept/Framework/modules/output/dribbler.h new file mode 100755 index 0000000..b11af69 --- /dev/null +++ b/source/Concept/Framework/modules/output/dribbler.h @@ -0,0 +1,118 @@ +#ifndef _DRIBBLER_H +#define _DRIBBLER_H + +#include "../../stdafx.h" + +class Dribbler : public IO_Module +{ +public: + Dribbler() + { + this->enabled = false; + this->curSpeed = 0; + this->parent = NULL; + this->moduleId = 0; + this->hardwarePort = NULL; + this->portPower = NULL; + this->pinForward = 0; + this->pinReverse = 0; + this->pinPower = 0; + } + + Dribbler(uint32 dribblerId) + { + this->enabled = false; + this->curSpeed = 1.0f; + this->parent = NULL; + this->moduleId = dribblerId; + + switch(dribblerId) + { + case IO_DRIBBLER_MAIN: + this->hardwarePort = &PORTD; + this->portPower = &PORTA; + this->pinForward = (1 << 6); + this->pinReverse = (1 << 7); + this->pinPower = (1 << 5); + break; + default: + this->hardwarePort = NULL; + this->portPower = NULL; + this->pinForward = 0; + this->pinReverse = 0; + this->pinPower = 0; + break; + } + + UpdateDirection(); + } + +protected: + bool enabled; + float curSpeed; + + //Hardware + volatile uint8* hardwarePort; + volatile uint8* portPower; + uint8 pinForward; + uint8 pinReverse; + uint8 pinPower; + + void UpdateDirection() + { + if(enabled) + { + if(curSpeed > 0) + { + *hardwarePort |= pinForward; + *hardwarePort &= ~pinReverse; + } + else if(curSpeed < 0) + { + *hardwarePort |= pinReverse; + *hardwarePort &= ~pinForward; + } + else + { + *hardwarePort |= pinForward; + *hardwarePort |= pinReverse; + } + + *portPower |= pinPower; + } + else + { + *hardwarePort &= ~pinForward; + *hardwarePort &= ~pinReverse; + + *portPower &= ~pinPower; + } + } + +public: + float GetSpeed() + { + return curSpeed; + } + + void SetSpeed(float newSpeed) + { + curSpeed = newSpeed; + + UpdateDirection(); + } + + bool GetEnabled() + { + return enabled; + } + + void SetEnabled(bool newStatus) + { + enabled = newStatus; + + UpdateDirection(); + } +}; + +#endif diff --git a/source/Concept/Framework/engine.cpp b/source/Concept/Framework/modules/output/engine.c old mode 100644 new mode 100755 similarity index 100% rename from source/Concept/Framework/engine.cpp rename to source/Concept/Framework/modules/output/engine.c diff --git a/source/Concept/Framework/modules/output/engine.h b/source/Concept/Framework/modules/output/engine.h new file mode 100755 index 0000000..27b9905 --- /dev/null +++ b/source/Concept/Framework/modules/output/engine.h @@ -0,0 +1,124 @@ +#ifndef _ENGINE_H +#define _ENGINE_H + +#include "../../stdafx.h" + +class Engine : public IO_Module +{ +public: + Engine() + { + this->enabled = false; + this->curSpeed = 0; + this->parent = NULL; + this->moduleId = 0; + this->hardwarePort = NULL; + this->pwmSpeed = NULL; + this->pinForward = 0; + this->pinReverse = 0; + } + + Engine(uint32 engineId) + { + this->enabled = false; + this->curSpeed = 0; + this->parent = NULL; + this->moduleId = engineId; + + switch(engineId) + { + case IO_ENGINE_DRIVE_LEFT: + this->hardwarePort = &PORTB; + this->pwmSpeed = &OCR1A; + this->pinForward = (1 << 0); + this->pinReverse = (1 << 1); + break; + case IO_ENGINE_DRIVE_BACK: + this->hardwarePort = &PORTB; + this->pwmSpeed = &OCR1B; + this->pinForward = (1 << 2); + this->pinReverse = (1 << 3); + break; + case IO_ENGINE_DRIVE_RIGHT: + this->hardwarePort = &PORTD; + this->pwmSpeed = &OCR3A; + this->pinForward = (1 << 5); + this->pinReverse = (1 << 4); + break; + default: + this->hardwarePort = NULL; + this->pwmSpeed = NULL; + this->pinForward = 0; + this->pinReverse = 0; + break; + } + + *this->pwmSpeed = 0; + } + +protected: + bool enabled; + float curSpeed; + + //Hardware + volatile uint8* hardwarePort; + volatile uint16* pwmSpeed; + uint8 pinForward; + uint8 pinReverse; + + void UpdateDirection() + { + if(enabled) + { + if(curSpeed > 0) + { + *hardwarePort |= pinForward; + *hardwarePort &= ~pinReverse; + } + else if(curSpeed < 0) + { + *hardwarePort |= pinReverse; + *hardwarePort &= ~pinForward; + } + else + { + *hardwarePort |= pinForward; + *hardwarePort |= pinReverse; + } + } + else + { + *hardwarePort &= ~pinForward; + *hardwarePort &= ~pinReverse; + } + } + +public: + float GetSpeed() + { + return curSpeed; + } + + void SetSpeed(float newSpeed) + { + curSpeed = newSpeed; + + *pwmSpeed = (abs((int16)(newSpeed / SPEED_PER_PWM))); + + UpdateDirection(); + } + + bool GetEnabled() + { + return enabled; + } + + void SetEnabled(bool newStatus) + { + enabled = newStatus; + + UpdateDirection(); + } +}; + +#endif diff --git a/source/Concept/Framework/kicker.cpp b/source/Concept/Framework/modules/output/kicker.c old mode 100644 new mode 100755 similarity index 100% rename from source/Concept/Framework/kicker.cpp rename to source/Concept/Framework/modules/output/kicker.c diff --git a/source/Concept/Framework/modules/output/kicker.h b/source/Concept/Framework/modules/output/kicker.h new file mode 100755 index 0000000..080666a --- /dev/null +++ b/source/Concept/Framework/modules/output/kicker.h @@ -0,0 +1,84 @@ +#ifndef _KICKER_H +#define _KICKER_H + +#include "../../stdafx.h" + +class Kicker : public IO_Module +{ +public: + Kicker() + { + this->enabled = false; + this->parent = NULL; + this->moduleId = 0; + this->portPower = NULL; + this->portForward = NULL; + this->portReverse = NULL; + this->pinPower = 0; + this->pinForward = 0; + this->pinReverse = 0; + } + + Kicker(uint32 kickerId) + { + this->enabled = false; + this->parent = NULL; + this->moduleId = kickerId; + + switch(kickerId) + { + case IO_KICKER_MAIN: + this->portPower = &PORTG; + this->portForward = &PORTA; + this->portReverse = &PORTE; + this->pinPower = (1 << 3); + this->pinForward = (1 << 2); + this->pinReverse = (1 << 6); + break; + default: + this->portPower = NULL; + this->portForward = NULL; + this->portReverse = NULL; + this->pinPower = 0; + this->pinForward = 0; + this->pinReverse = 0; + break; + } + + *this->portForward |= this->pinForward; + *this->portReverse &= ~this->pinReverse; + } + +protected: + bool enabled; + + //Hardware + volatile uint8* portPower; + volatile uint8* portForward; + volatile uint8* portReverse; + uint8 pinPower; + uint8 pinForward; + uint8 pinReverse; + +public: + bool GetEnabled() + { + return enabled; + } + + void SetEnabled(bool newStatus) + { + enabled = newStatus; + + if(enabled) + { + *portPower |= pinPower; + } + else + { + *portPower &= ~pinPower; + } + } +}; + +#endif diff --git a/source/Concept/Framework/led.cpp b/source/Concept/Framework/modules/output/led.c old mode 100644 new mode 100755 similarity index 100% rename from source/Concept/Framework/led.cpp rename to source/Concept/Framework/modules/output/led.c diff --git a/source/Concept/Framework/modules/output/led.h b/source/Concept/Framework/modules/output/led.h new file mode 100755 index 0000000..08e7466 --- /dev/null +++ b/source/Concept/Framework/modules/output/led.h @@ -0,0 +1,65 @@ +#ifndef _LED_H +#define _LED_H + +#include "../../stdafx.h" + +class Led : public IO_Module +{ +public: + Led() + { + this->enabled = false; + this->parent = NULL; + this->moduleId = 0; + this->hardwarePort = NULL; + this->pinPower = 0; + } + + Led(uint32 ledId) + { + this->enabled = false; + this->parent = NULL; + this->moduleId = ledId; + + switch(ledId) + { + case IO_LED_MAIN: + this->hardwarePort = &PORTB; + this->pinPower = (1 << 1); + break; + default: + this->hardwarePort = NULL; + this->pinPower = 0; + break; + } + } + +protected: + bool enabled; + + //Hardware + volatile uint8* hardwarePort; + uint8 pinPower; + +public: + bool GetEnabled() + { + return enabled; + } + + void SetEnabled(bool newStatus) + { + enabled = newStatus; + + if(enabled) + { + *hardwarePort &= ~pinPower; + } + else + { + *hardwarePort |= pinPower; + } + } +}; + +#endif diff --git a/source/Concept/Framework/robot.cpp b/source/Concept/Framework/robot.c old mode 100644 new mode 100755 similarity index 89% rename from source/Concept/Framework/robot.cpp rename to source/Concept/Framework/robot.c index 5c673cb..d8ea1c2 --- a/source/Concept/Framework/robot.cpp +++ b/source/Concept/Framework/robot.c @@ -45,7 +45,7 @@ Robot::Robot() sei(); //Interface - for(uint8 i = 0; i < IO_END; i++) + for(uint8 i = IO_START; i < IO_END; i++) { modules[i] = NULL; } @@ -85,6 +85,12 @@ bool Robot::RemoveModule(IO_Module* oldModule) //----------------------------------------------------------------------------- void Robot::Update() { + GetModule(IO_BALL_TRACKER_MAIN)->Update(); + + GetModule(IO_POSITION_TRACKER_MAIN)->Update(); + + GetModule(IO_NAVIGATOR_MAIN)->Update(); + //insert code here } diff --git a/source/Concept/Framework/sensor.cpp b/source/Concept/Framework/sensor.cpp deleted file mode 100644 index b9b4936..0000000 --- a/source/Concept/Framework/sensor.cpp +++ /dev/null @@ -1 +0,0 @@ -#include "sensor.h" \ No newline at end of file diff --git a/source/Concept/Framework/stdafx.h b/source/Concept/Framework/stdafx.h index 151d749..af60c6e 100644 --- a/source/Concept/Framework/stdafx.h +++ b/source/Concept/Framework/stdafx.h @@ -19,4 +19,6 @@ #include "ir_sensor.h" #include "mouse_sensor.h" #include "position_tracker.h" +#include "ball_tracker.h" +#include "navigator.h" #include "robot.h" diff --git a/source/Concept/Framework/tools.cpp b/source/Concept/Framework/tools.c old mode 100644 new mode 100755 similarity index 100% rename from source/Concept/Framework/tools.cpp rename to source/Concept/Framework/tools.c diff --git a/source/Concept/Framework/tools.h b/source/Concept/Framework/tools.h index 6519a1b..0ca4dda 100644 --- a/source/Concept/Framework/tools.h +++ b/source/Concept/Framework/tools.h @@ -1,7 +1,8 @@ #ifndef _TOOLS_H #define _TOOLS_H -#include +#include +#include #ifndef new void* operator new(size_t sz); @@ -34,6 +35,11 @@ inline void usleep(int usec) asm volatile("nop"); } } -}; +}; + +inline float distance2d(float x1, float y1, float x2, float y2) +{ + return sqrt(((x1 - x2) * (x1 - x2)) + ((y1 - y2) * (y1 - y2))); +} #endif