From 655dd7522fb60e2ef0e68437337178184109f347 Mon Sep 17 00:00:00 2001 From: sicarius Date: Thu, 22 Feb 2007 13:12:03 +0000 Subject: Codework --- .../Concept/Framework/modules/executor/navigator.c | 149 +++++---------------- .../Framework/modules/input/distance_sensor.c | 19 ++- source/Concept/Framework/modules/input/ir_sensor.c | 2 +- source/Concept/Framework/modules/input/ir_sensor.h | 7 + .../Framework/modules/interpreter/ball_tracker.c | 7 + .../Framework/modules/interpreter/ball_tracker.h | 10 ++ .../modules/interpreter/position_tracker.c | 13 +- .../modules/interpreter/position_tracker.h | 26 ++-- source/Concept/Framework/modules/output/dribbler.h | 7 +- 9 files changed, 103 insertions(+), 137 deletions(-) (limited to 'source/Concept/Framework/modules') 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(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;*/ } //----------------------------------------------------------------------------- @@ -178,108 +154,43 @@ void Navigator::CalculateDirection() //----------------------------------------------------------------------------- void Navigator::CalculateEngines() { + // Use the position_tracker 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; + // 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(IO_DISPLAY_MAIN))->Print(vLeft,10,2); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print(vBack,10,3); + //(parent->GetModule(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(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(); - } + // Transfer the values to the engines + 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; } 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(IO_DISPLAY_MAIN))->Print("Gen Pulse; Pin:",1,4); + //(parent->GetModule(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(IO_DISPLAY_MAIN))->Print("Wait response; Pin:",1,4); + //(parent->GetModule(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(IO_SENSOR_MOUSE_LEFT); Mouse_Sensor* mouseRight = parent->GetModule(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(IO_DISPLAY_MAIN))->Print(" ", 5, 1); - //(parent->GetModule(IO_DISPLAY_MAIN))->Print(absoluteDiffX, 5, 1); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print(robotMovementY + movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff)), 5, 1); //(parent->GetModule(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 { -- cgit v1.2.3