neocturne/rc2007-soccer
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

+++ Codework, Navigator&Positiontracker done (buggy?)

Browse source
This commit is contained in:
sicarius 2007-02-20 21:01:02 +00:00
parent ec2a18e931
commit 22433e52a7
9 changed files with 168 additions and 46 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

BIN
Information/Hardware/Portbelegung.ods
View file

Binary file not shown.

2
source/Concept/Framework/RoboCode.aps
View file

File diff suppressed because one or more lines are too long

3
source/Concept/Framework/defines.h
View file

@ -58,9 +58,10 @@
//Constants
#define SPEED_PER_PWM 1
#define DISTANCE_PER_VALUE 1
#define TICKS_PER_CM 205.0f
#define PI 3.14159265358979323846f
#define CYCLES_PER_CORRECTION 200
#define CYCLES_PER_CORRECTION 20
#define EMPTY_FLOAT 81188.1484f
//IO Module Names

58
source/Concept/Framework/main.c
View file

@ -145,27 +145,29 @@ int main()
Keyboard* ourKeyboard = localRobot->GetModule<Keyboard>(IO_KEYBOARD_MAIN);
uint32 i = 1;
Navigator* ourNavigator = localRobot->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
Position_Tracker* ourPosition_Tracker = localRobot->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
float rotation = 0;
Mouse_Sensor* mouse_left = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
Mouse_Sensor* mouse_right = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
//Mouse_Sensor* mouse_left = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
//Mouse_Sensor* mouse_right = localRobot->GetModule<Mouse_Sensor>(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
ourDisplay->Print(i++,1,1);
if(!(i % 200))
{
ourDisplay->Clear();
ourDisplay->PrintFloat(ourPosition_Tracker->GetPositionX(),1,2);
ourDisplay->PrintFloat(ourPosition_Tracker->GetPositionY(),1,3);
ourDisplay->PrintFloat(ourPosition_Tracker->GetOrientation() * 180.0f / PI,1,4);
}
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
/*distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
value = distanceSensor->GetDistance();
ourDisplay->Print(value, 1, 4);
ourDisplay->Print(";");
@ -180,47 +182,41 @@ int main()
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_270_DEG);
value = distanceSensor->GetDistance();
ourDisplay->Print(value);
ourDisplay->Print(";");
msleep(500);*/
ourDisplay->Print(";"); */
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();
break;
case 1:
ourNavigator->Drive(225, rotation, 200, rotation);
ourNavigator->Drive(225, rotation, 100, rotation);
break;
case 2:
ourNavigator->Drive(180, rotation, 200, rotation);
ourNavigator->Drive(180, rotation, 100, rotation);
break;
case 3:
ourNavigator->Drive(135, rotation, 200, rotation);
ourNavigator->Drive(135, rotation, 100, rotation);
break;
case 4:
ourNavigator->Drive(270, rotation, 200, rotation);
ourNavigator->Drive(270, rotation, 100, rotation);
break;
case 5:
ourNavigator->Drive(0, rotation, 0, rotation);
break;
case 6:
ourNavigator->Drive(90, rotation, 200, rotation);
ourNavigator->Drive(90, rotation, 100, rotation);
break;
case 7:
ourNavigator->Drive(315, rotation, 200, rotation);
ourNavigator->Drive(315, rotation, 100, rotation);
break;
case 8:
ourNavigator->Drive(0, rotation, 200, rotation);
ourNavigator->Drive(0, rotation, 100, rotation);
break;
case 9:
ourNavigator->Drive(45, rotation, 200, rotation);
ourNavigator->Drive(45, rotation, 100, rotation);
break;
case 10:
rotation += 10;
@ -230,8 +226,6 @@ int main()
break;
}
msleep(50);
/*
//ourDisplay->Clear();
ourDisplay->Print(i++,1,1);

44
source/Concept/Framework/modules/executor/navigator.c
View file

@ -54,6 +54,30 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float
}
CalculateEngines();
//dings,,,
/*float xDrive = cos(direction + PI / 6.0f);
float yDrive = sin(direction + PI / 6.0f);
float vLeft = xDrive;
float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;
vLeft = vLeft * this->robotSpeed + rotationSpeed;
vBack = vBack * this->robotSpeed + rotationSpeed;
vRight = vRight * this->robotSpeed + rotationSpeed;
Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
curEngine->SetSpeed(vLeft);
curEngine->SetEnabled(true);
curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
curEngine->SetSpeed(vBack);
curEngine->SetEnabled(true);
curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
curEngine->SetSpeed(vRight);
curEngine->SetEnabled(true);
curEngine = NULL;*/
}
//-----------------------------------------------------------------------------
@ -154,6 +178,8 @@ void Navigator::CalculateDirection()
//-----------------------------------------------------------------------------
void Navigator::CalculateEngines()
{
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
if(direction != EMPTY_FLOAT)
{
float relativeDirection = this->direction - locationeer->GetOrientation();
@ -165,15 +191,15 @@ void Navigator::CalculateEngines()
float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;
float speedCorrection = 1.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));*/
float calcSpeed = 1.0f;//sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight +
// vBack * (vBack + vLeft + vRight));
if(calcSpeed != 1.0f)
{
@ -184,14 +210,14 @@ void Navigator::CalculateEngines()
if(maxOverallSpeed > maxEngineSpeed)
{
robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection);
}
}*/
vLeft = vLeft * this->robotSpeed * speedCorrection;
vBack = vBack * this->robotSpeed * speedCorrection;
vRight = vRight * this->robotSpeed * 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));
/*maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));
float minSingleSpeed = min(min(vLeft, vBack), vRight);
if(rotationSpeed)
@ -226,7 +252,7 @@ void Navigator::CalculateEngines()
vRight -= this->rotationSpeed;
}
}
}
}*/
Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
curEngine->SetSpeed(vLeft);

22
source/Concept/Framework/modules/input/mouse_sensor.h
View file

@ -24,6 +24,8 @@ public:
this->configReset = 0;
this->configAwake = 0;
this->newImage = false;
this->positionX = 0;
this->positionY = 0;
}
Mouse_Sensor(uint32 sensorId)
@ -47,6 +49,8 @@ public:
this->registerDeltaY = 0x02;
this->configReset = 0x80;
this->configAwake = 0x01;
this->positionX = -3.88f * TICKS_PER_CM;
this->positionY = -3.88f * TICKS_PER_CM;
break;
case IO_SENSOR_MOUSE_RIGHT:
this->hardwarePort = &PORTC;
@ -61,6 +65,8 @@ public:
this->registerDeltaY = 0x02;
this->configReset = 0x80;
this->configAwake = 0x01;
this->positionX = -3.88f * TICKS_PER_CM;
this->positionY = 3.88f * TICKS_PER_CM;
break;
default:
this->hardwarePort = NULL;
@ -101,8 +107,10 @@ protected:
uint8 registerDeltaY;
uint8 configReset;
uint8 configAwake;
//Information
float positionX;
float positionY;
public:
void WriteByte(uint8 newByte)
{
*hardwareDDR |= pinSDA;//Set SDA output
@ -180,6 +188,8 @@ public:
return pixel;
}
public:
uint8 GetSqual()
{
return Read(registerSqual);
@ -194,6 +204,16 @@ public:
{
return (int8)(Read(registerDeltaY));
}
float GetPositionX()
{
return positionX;
}
float GetPositionY()
{
return positionY;
}
};
#endif

62
source/Concept/Framework/modules/interpreter/position_tracker.c
View file

@ -3,5 +3,65 @@
//-----------------------------------------------------------------------------
void Position_Tracker::Update()
{
//insert code here
Mouse_Sensor* mouseLeft = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
Mouse_Sensor* mouseRight = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
int8 leftX = mouseLeft->GetXMovement();
int8 leftY = mouseLeft->GetYMovement();
float distanceLeft = sqrt(leftX * leftX + leftY * leftY);
float angleLeft = easyAngle(atan2(leftY, leftX) + (225.0f * PI / 180.0f));
float movementLeftX = cos(angleLeft) * distanceLeft;
float movementLeftY = sin(angleLeft) * distanceLeft;
if(!leftX && !leftY)
{
movementLeftX = 0;
movementLeftY = 0;
}
int8 rightX = mouseRight->GetXMovement();
int8 rightY = mouseRight->GetYMovement();
float distanceRight = sqrt(rightX * rightX + rightY * rightY);
float angleRight = easyAngle(atan2(rightY, rightX) - (45.0f * PI / 180.0f));
float movementRightX = cos(angleRight) * distanceRight;
float movementRightY = sin(angleRight) * distanceRight;
if(!rightX && !rightY)
{
movementRightX = 0;
movementRightY = 0;
}
float movementDifferenceX = movementRightX - movementLeftX;
float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY());
float robotMovementX = movementDifferenceX / 2.0f;
float robotMovementY = movementDifferenceY / 2.0f;
robotMovementX += movementLeftX;
robotMovementY += movementLeftY + mouseLeft->GetPositionY();
float robotDistance = sqrt(robotMovementX * robotMovementX + robotMovementY * robotMovementY);
float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f);
float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementX);
float absoluteDiffY = sin(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementY);
if(!robotMovementX && !robotMovementY)
{
absoluteDiffX = 0;
absoluteDiffY = 0;
}
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(" ", 5, 1);
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffX, 5, 1);
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffY, 12, 1);
this->positionX += absoluteDiffX;
this->positionY += absoluteDiffY;
this->orientation += orientationDiff;
this->orientation = easyAngle(this->orientation);
}

6
source/Concept/Framework/modules/interpreter/position_tracker.h
View file

@ -10,12 +10,18 @@ public:
{
this->parent = NULL;
this->moduleId = 0;
this->positionX = 0;
this->positionY = 0;
this->orientation = 0;
}
Position_Tracker(uint32 trackerId)
{
this->parent = NULL;
this->moduleId = trackerId;
this->positionX = 0;
this->positionY = 0;
this->orientation = 0;
}
protected:

17
source/Concept/Framework/tools.h
View file

@ -14,6 +14,21 @@
#define min(a, b) (((a) < (b)) ? (a) : (b))
#define sign(a) (((a) < 0) ? (-1) : (1))
inline float easyAngle(float angle)
{
while(angle < 0)
{
angle += 2.0f * PI;
}
while(angle >= 2.0f * PI)
{
angle -= 2.0f * PI;
}
return angle;
}
inline void sleep(uint16 sec)
{
for (uint16 s = 0; s < sec; s++) {
@ -48,7 +63,7 @@ inline float distance2d(float x1, float y1, float x2, float y2)
inline float direction2d(float x1, float y1, float x2, float y2)
{
return atan((y2 - y1) / (x2 - x1));
return atan2((y2 - y1), (x2 - x1));
}
#endif