From ec2a18e931cdaa6c62a8843c29dc93c4a823a2cd Mon Sep 17 00:00:00 2001 From: masterm Date: Mon, 19 Feb 2007 22:02:01 +0000 Subject: +++ corrected svn cripplement --- source/Concept/Framework/RoboCode.aps | 4 - source/Concept/Framework/defines.h | 8 +- source/Concept/Framework/main.c | 22 +- .../Concept/Framework/modules/executor/navigator.c | 298 ++------------------- .../Concept/Framework/modules/executor/navigator.h | 76 +----- .../Framework/modules/input/distance_sensor.c | 8 +- source/Concept/Framework/modules/output/engine.h | 8 +- source/Concept/Framework/tools.h | 19 +- 8 files changed, 39 insertions(+), 404 deletions(-) (limited to 'source/Concept/Framework') diff --git a/source/Concept/Framework/RoboCode.aps b/source/Concept/Framework/RoboCode.aps index 01a1bea..0cae013 100644 --- a/source/Concept/Framework/RoboCode.aps +++ b/source/Concept/Framework/RoboCode.aps @@ -1,5 +1 @@ -<<<<<<< .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/defines.h b/source/Concept/Framework/defines.h index 6a88917..8cf2d7a 100644 --- a/source/Concept/Framework/defines.h +++ b/source/Concept/Framework/defines.h @@ -58,15 +58,9 @@ //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 //IO Module Names diff --git a/source/Concept/Framework/main.c b/source/Concept/Framework/main.c index 306885b..8a7d90f 100755 --- a/source/Concept/Framework/main.c +++ b/source/Concept/Framework/main.c @@ -5,7 +5,7 @@ int main() //Init our robot Robot* localRobot = new Robot(); - sleep(1); // Wait for LCD-Display + sleep(1);//Wait for Hardware //Init Displays for(uint8 i = IO_DISPLAY_START; i < IO_DISPLAY_END; i++) @@ -15,9 +15,6 @@ int main() 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++) { @@ -25,7 +22,6 @@ 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++) @@ -34,7 +30,6 @@ 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++) @@ -43,7 +38,6 @@ 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++) @@ -63,7 +57,6 @@ 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: @@ -74,7 +67,6 @@ int main() Distance_Sensor* newSensor = new Distance_Sensor(i); localRobot->AddModule(newSensor); newSensor = NULL; - // ourDisplay->Print("DistanceSensor Ready"); // Debug output break; } @@ -84,7 +76,6 @@ int main() Mouse_Sensor* newSensor = new Mouse_Sensor(i); localRobot->AddModule(newSensor); newSensor = NULL; - // ourDisplay->Print("MouseSensor Ready"); ourDisplay->NewLine(); // Debug output break; } @@ -98,7 +89,6 @@ int main() } } } - // ourDisplay->Print("Sensors Ready"); ourDisplay->NewLine(); // Debug output //Init Leds for(uint8 i = IO_LED_START; i < IO_LED_END; i++) @@ -107,7 +97,6 @@ int main() localRobot->AddModule(newLed); newLed = NULL; } - // ourDisplay->Print("LEDs Ready"); ourDisplay->NewLine(); // Debug output //Init Keyboards for(uint8 i = IO_KEYBOARD_START; i < IO_KEYBOARD_END; i++) @@ -116,7 +105,6 @@ 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++) @@ -125,7 +113,6 @@ 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++) @@ -134,7 +121,6 @@ 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++) @@ -143,9 +129,11 @@ 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); + + //Debug code + Display* ourDisplay = localRobot->GetModule(IO_DISPLAY_MAIN); ourDisplay->Clear(); diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index 47b1c69..a69c533 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -1,138 +1,3 @@ -<<<<<<< .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" //----------------------------------------------------------------------------- @@ -150,140 +15,12 @@ void Navigator::Stop() (parent->GetModule(i))->SetSpeed(0); (parent->GetModule(i))->SetEnabled(true); } -} - -//----------------------------------------------------------------------------- -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() +//----------------------------------------------------------------------------- +void Navigator::Rotate(float rotationSpeed) { - 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->rotationSpeed = min(fabs(rotationSpeed), 255.0f) * sign(rotationSpeed); this->direction = EMPTY_FLOAT; this->targetAngle = EMPTY_FLOAT; this->targetX = EMPTY_FLOAT; @@ -295,14 +32,14 @@ void Navigator::CalculateEngines() (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; - this->targetAngle = newAngle; + this->direction = newDirection * PI / 180.0f; + this->targetAngle = newAngle * PI / 180.0f; this->targetX = EMPTY_FLOAT; this->targetY = EMPTY_FLOAT; this->robotSpeed = newSpeed; @@ -317,8 +54,8 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float } CalculateEngines(); -} - +} + //----------------------------------------------------------------------------- void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed) { @@ -330,7 +67,7 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, this->targetX = newX; this->targetY = newY; this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);; - this->targetAngle = newAngle; + this->targetAngle = newAngle * PI / 180.0f; this->robotSpeed = newSpeed; if(targetAngle - locationeer->GetOrientation() > PI) @@ -349,8 +86,8 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, } CalculateDirection(); -} - +} + //----------------------------------------------------------------------------- void Navigator::Update() { @@ -390,8 +127,8 @@ void Navigator::Update() { CalculateDirection(); } -} - +} + //----------------------------------------------------------------------------- void Navigator::CalculateDirection() { @@ -412,8 +149,8 @@ void Navigator::CalculateDirection() } CalculateEngines(); -} - +} + //----------------------------------------------------------------------------- void Navigator::CalculateEngines() { @@ -519,5 +256,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 848c9fa..81919f9 100755 --- a/source/Concept/Framework/modules/executor/navigator.h +++ b/source/Concept/Framework/modules/executor/navigator.h @@ -1,79 +1,6 @@ -<<<<<<< .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 -//#include #include "../../stdafx.h" class Navigator : public IO_Module @@ -139,6 +66,5 @@ public: return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT); } }; - ->>>>>>> .r199 + #endif diff --git a/source/Concept/Framework/modules/input/distance_sensor.c b/source/Concept/Framework/modules/input/distance_sensor.c index 508eeed..262d35f 100755 --- a/source/Concept/Framework/modules/input/distance_sensor.c +++ b/source/Concept/Framework/modules/input/distance_sensor.c @@ -12,12 +12,8 @@ float Distance_Sensor::GetDistance() *hardwarePort &= ~pin;//Deactivate port *hardwareDDR &= ~pin;//Set pin input - //(parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1); - //Wait for response - for(int i=0;(!(PINC & pin))&&(i < 1000);i++) {asm volatile("nop");} - - //(parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1); + for(uint16 i = 0; (!(*hardwarePin & pin)) && (i < 1000); i++) { asm volatile("nop"); } //Calculate duration of response while((*hardwarePin & pin)&&(result < 300000)) @@ -26,7 +22,5 @@ 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/output/engine.h b/source/Concept/Framework/modules/output/engine.h index 7b7a044..4c022bb 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; - int curSpeed; + int16 curSpeed; //Hardware volatile uint8* hardwarePort; @@ -94,16 +94,16 @@ protected: } public: - int GetSpeed() + int16 GetSpeed() { return curSpeed; } - void SetSpeed(int newSpeed) + void SetSpeed(int16 newSpeed) { curSpeed = newSpeed; - int pwm = abs(newSpeed); + uint16 pwm = abs(newSpeed); if(pwm > 255) pwm = 255; *pwmSpeed = pwm; diff --git a/source/Concept/Framework/tools.h b/source/Concept/Framework/tools.h index 25a63f2..735e773 100644 --- a/source/Concept/Framework/tools.h +++ b/source/Concept/Framework/tools.h @@ -3,6 +3,7 @@ #include #include +#include "defines.h" #ifndef new void* operator new(size_t sz); @@ -13,28 +14,28 @@ #define min(a, b) (((a) < (b)) ? (a) : (b)) #define sign(a) (((a) < 0) ? (-1) : (1)) -inline void sleep(int sec) +inline void sleep(uint16 sec) { - for (int s=0; s