diff options
Diffstat (limited to 'source/Concept/Framework/modules')
9 files changed, 103 insertions, 137 deletions
diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index 5dbf8bf..928a5ff 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -54,30 +54,6 @@ 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;*/
} //-----------------------------------------------------------------------------
@@ -178,108 +154,43 @@ void Navigator::CalculateDirection() //-----------------------------------------------------------------------------
void Navigator::CalculateEngines()
{
+ // Use the position_tracker
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(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;
+ // get the relative position of the robot
+ float relativeDirection = this->direction - locationeer->GetOrientation();
- float maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));
+ // calculate x and y-koordinates
+ float xDrive = cos(relativeDirection + PI / 6.0f);
+ float yDrive = sin(relativeDirection + PI / 6.0f);
- float calcSpeed = 1.0f;//sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight +
- // vBack * (vBack + vLeft + vRight));
+ // calculate relative speed of engines
+ float vLeft = xDrive;
+ float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
+ float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;
- if(calcSpeed != 1.0f)
- {
- speedCorrection = 1.0f / calcSpeed;
- }
+ // Get the maximal value
+ float speedCorrection = (float)max(max(fabs(vLeft),fabs(vBack)),fabs(vRight));
- float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection;
- if(maxOverallSpeed > maxEngineSpeed)
- {
- robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection);
- }*/
-
+ // calculate the correct speeds of the engines
+ vLeft = ((float)vLeft * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
+ vBack = ((float)vBack * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
+ vRight = ((float)vRight * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
- vLeft = vLeft * this->robotSpeed;// * speedCorrection;
- vBack = vBack * this->robotSpeed;// * speedCorrection;
- vRight = vRight * this->robotSpeed;// * speedCorrection;
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vLeft,10,2);
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vBack,10,3);
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vRight,10,4);
- /*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<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;
- }
- else if(rotationSpeed)
- {
- Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
- curEngine->SetSpeed(this->rotationSpeed);
- curEngine->SetEnabled(true);
- curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
- curEngine->SetSpeed(this->rotationSpeed);
- curEngine->SetEnabled(true);
- curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
- curEngine->SetSpeed(this->rotationSpeed);
- curEngine->SetEnabled(true);
- curEngine = NULL;
- }
- else
- {
- Stop();
- }
+ // Transfer the values to the engines
+ 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;
} diff --git a/source/Concept/Framework/modules/input/distance_sensor.c b/source/Concept/Framework/modules/input/distance_sensor.c index 262d35f..e6fd3f8 100755 --- a/source/Concept/Framework/modules/input/distance_sensor.c +++ b/source/Concept/Framework/modules/input/distance_sensor.c @@ -5,21 +5,36 @@ float Distance_Sensor::GetDistance() {
uint32 result = 0;
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("Gen Pulse; Pin:",1,4);
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print((int)(*hardwarePin & pin));
+
+ msleep(500);
+
//Generate pulse
*hardwareDDR |= pin;//Set pin output
*hardwarePort |= pin;//Activate port
usleep(10);//Wait for 10µs
- *hardwarePort &= ~pin;//Deactivate port
*hardwareDDR &= ~pin;//Set pin input
+ *hardwarePort &= ~pin;//Deactivate port
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("Wait response; Pin:",1,4);
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print((int)(*hardwarePin & pin));
+
+ uint16 i;
//Wait for response
- for(uint16 i = 0; (!(*hardwarePin & pin)) && (i < 1000); i++) { asm volatile("nop"); }
+ for(i = 0; (!(*hardwarePin & pin)) && (i < 1000); i++) { asm volatile("nop"); }
//Calculate duration of response
while((*hardwarePin & pin)&&(result < 300000))
{
result++;
asm volatile("nop");
+ asm volatile("nop");
+ asm volatile("nop");
+ asm volatile("nop");
+ asm volatile("nop");
+ asm volatile("nop");
+ asm volatile("nop");
}
return (float(result) * DISTANCE_PER_VALUE);
diff --git a/source/Concept/Framework/modules/input/ir_sensor.c b/source/Concept/Framework/modules/input/ir_sensor.c index c34feed..12d74e0 100755 --- a/source/Concept/Framework/modules/input/ir_sensor.c +++ b/source/Concept/Framework/modules/input/ir_sensor.c @@ -5,5 +5,5 @@ uint16 IR_Sensor::GetIRIntensity() { if(!parent) return 0;
- return parent->GetADCValue(channel);
+ return min(parent->GetADCValue(channel) + this->intensityCorrection, 1023);
}
diff --git a/source/Concept/Framework/modules/input/ir_sensor.h b/source/Concept/Framework/modules/input/ir_sensor.h index 30e6ea4..74396ec 100755 --- a/source/Concept/Framework/modules/input/ir_sensor.h +++ b/source/Concept/Framework/modules/input/ir_sensor.h @@ -12,12 +12,14 @@ public: {
this->parent = NULL;
this->moduleId = 0;
+ this->intensityCorrection = 0;
}
IR_Sensor(uint32 sensorId)
{
this->parent = NULL;
this->moduleId = sensorId;
+ this->intensityCorrection = 0;
switch(sensorId)
{
@@ -32,12 +34,15 @@ public: break;
case IO_SENSOR_IR_100_DEG:
this->channel = 3;
+ this->intensityCorrection = 40;
break;
case IO_SENSOR_IR_180_DEG:
this->channel = 4;
+ this->intensityCorrection = 50;
break;
case IO_SENSOR_IR_260_DEG:
this->channel = 5;
+ this->intensityCorrection = 70;
break;
case IO_SENSOR_IR_300_DEG:
this->channel = 6;
@@ -55,6 +60,8 @@ protected: //Hardware
uint8 channel;
+ uint8 intensityCorrection;
+
public:
uint16 GetIRIntensity();
};
diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.c b/source/Concept/Framework/modules/interpreter/ball_tracker.c index 1701121..2d85b96 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.c +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.c @@ -109,9 +109,16 @@ void Ball_Tracker::Update() break;
}
+ if(fabs(mainDirection - secondDirection) > PI)
+ {
+ min(mainDirection, secondDirection) += 2.0f * PI;
+ }
+
direction = (intensity[greatestIntensity] * mainDirection +
intensity[secondIntensity] * secondDirection) /
(intensity[greatestIntensity] + intensity[secondIntensity]);
+
+ direction = easyAngle(direction);
}
else
{
diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.h b/source/Concept/Framework/modules/interpreter/ball_tracker.h index bea4a19..c62f05e 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.h +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.h @@ -30,6 +30,16 @@ public: {
return direction;
}
+
+ bool KnowsBallDirection()
+ {
+ return direction != EMPTY_FLOAT;
+ }
+
+ bool RobotHasBall()
+ {
+ //fill me!
+ }
};
#endif diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c index 63a7f5f..a64ab60 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.c +++ b/source/Concept/Framework/modules/interpreter/position_tracker.c @@ -3,9 +3,11 @@ //-----------------------------------------------------------------------------
void Position_Tracker::Update()
{
+ // We want to use the mouse-sensors
Mouse_Sensor* mouseLeft = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
Mouse_Sensor* mouseRight = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
+ // Generate a vector for the left mouse
int8 leftX = mouseLeft->GetXMovement();
int8 leftY = mouseLeft->GetYMovement();
float distanceLeft = sqrt(leftX * leftX + leftY * leftY);
@@ -14,12 +16,14 @@ void Position_Tracker::Update() float movementLeftX = cos(angleLeft) * distanceLeft;
float movementLeftY = sin(angleLeft) * distanceLeft;
+ // AVR calculates a little bit strange ;)
if(!leftX && !leftY)
{
movementLeftX = 0;
movementLeftY = 0;
}
+ // Generate a vector for the right mouse
int8 rightX = mouseRight->GetXMovement();
int8 rightY = mouseRight->GetYMovement();
float distanceRight = sqrt(rightX * rightX + rightY * rightY);
@@ -28,21 +32,24 @@ void Position_Tracker::Update() float movementRightX = cos(angleRight) * distanceRight;
float movementRightY = sin(angleRight) * distanceRight;
+ // AVR calculates a little bit strange ;)
if(!rightX && !rightY)
{
movementRightX = 0;
movementRightY = 0;
}
- float movementDifferenceX = (movementRightX + mouseRight->GetPositionX()) - (movementLeftX + mouseLeft->GetPositionX());
+ // Generate vector from P:left to P:right
+ float movementDifferenceX = movementRightX - movementLeftX;
float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY());
+ // Calculate the difference of orientation
float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f);
float robotMovementX = movementDifferenceX / 2.0f;
float robotMovementY = movementDifferenceY / 2.0f;
robotMovementX += movementLeftX + mouseLeft->GetPositionX() + (-mouseLeft->GetPositionX() * cos(orientationDiff));
- robotMovementY += movementLeftY + mouseLeft->GetPositionY() + (-mouseLeft->GetPositionX() * sin(orientationDiff));
+ robotMovementY += movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff));
//float robotDistance = sqrt(robotMovementX * robotMovementX + robotMovementY * robotMovementY);
float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotMovementX - sin(this->orientation + (orientationDiff / 2.0f)) * robotMovementY;
@@ -58,7 +65,7 @@ void Position_Tracker::Update() }
//(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(robotMovementY + movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff)), 5, 1);
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffY, 12, 1);
this->positionX += absoluteDiffX;
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h index e93ef0c..47d0740 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.h +++ b/source/Concept/Framework/modules/interpreter/position_tracker.h @@ -32,19 +32,27 @@ protected: public:
void Update();
- float GetPositionX()
- {
- return positionX;
+ // Sets the current position; x and y in mm
+ void SetPosition(int newPositionX, int newPositionY, float newOrientation) {
+ positionX = newPositionX*(TICKS_PER_CM/10.0f);
+ positionY = newPositionY*(TICKS_PER_CM/10.0f);
+ orientation = easyAngle(newOrientation);
}
- float GetPositionY()
- {
- return positionY;
+ // returns x-koordinate in mm
+ int GetPositionX() {
+ return (int)((positionX*10.0f)/TICKS_PER_CM);
}
- float GetOrientation()
- {
- return orientation;
+ // returns y-koordinate in mm
+ int GetPositionY() {
+ return (int)((positionY*10.0f)/TICKS_PER_CM);
+ }
+
+ // returns orientation
+ float GetOrientation() {
+ return 0.0f; //tmp!!!!!!!!!!
+ //return orientation;
}
};
diff --git a/source/Concept/Framework/modules/output/dribbler.h b/source/Concept/Framework/modules/output/dribbler.h index b11af69..8544698 100755 --- a/source/Concept/Framework/modules/output/dribbler.h +++ b/source/Concept/Framework/modules/output/dribbler.h @@ -66,19 +66,20 @@ protected: {
*hardwarePort |= pinForward;
*hardwarePort &= ~pinReverse;
+ *portPower |= pinPower;
}
else if(curSpeed < 0)
{
*hardwarePort |= pinReverse;
*hardwarePort &= ~pinForward;
+ *portPower |= pinPower;
}
else
{
*hardwarePort |= pinForward;
*hardwarePort |= pinReverse;
- }
-
- *portPower |= pinPower;
+ *portPower &= ~pinPower;
+ }
}
else
{
|