summaryrefslogtreecommitdiffstats
path: root/source/Concept/Framework/modules
diff options
context:
space:
mode:
Diffstat (limited to 'source/Concept/Framework/modules')
-rwxr-xr-xsource/Concept/Framework/modules/executor/navigator.c44
-rwxr-xr-xsource/Concept/Framework/modules/input/mouse_sensor.h22
-rwxr-xr-xsource/Concept/Framework/modules/interpreter/position_tracker.c62
-rwxr-xr-xsource/Concept/Framework/modules/interpreter/position_tracker.h6
4 files changed, 123 insertions, 11 deletions
diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c
index a69c533..5dbf8bf 100755
--- a/source/Concept/Framework/modules/executor/navigator.c
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -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);
diff --git a/source/Concept/Framework/modules/input/mouse_sensor.h b/source/Concept/Framework/modules/input/mouse_sensor.h
index 460e387..b3d1b1a 100755
--- a/source/Concept/Framework/modules/input/mouse_sensor.h
+++ b/source/Concept/Framework/modules/input/mouse_sensor.h
@@ -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
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c
index 262069a..c8cd52d 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.c
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.c
@@ -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);
}
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h
index b31dd8f..e93ef0c 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.h
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.h
@@ -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:
generated by cgit v1.2.3 (git 2.25.1) at 2024-06-29 04:28:48 +0200