diff options
Diffstat (limited to 'source/Concept/Framework/modules')
6 files changed, 362 insertions, 19 deletions
diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index ceeda60..47b1c69 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -1,3 +1,138 @@ +<<<<<<< .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<Engine>(i))->SetSpeed(0); + (parent->GetModule<Engine>(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<Engine>(i))->SetSpeed(rotationSpeed); + (parent->GetModule<Engine>(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<Position_Tracker>(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<Position_Tracker>(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<Position_Tracker>(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"
//-----------------------------------------------------------------------------
@@ -19,7 +154,135 @@ void Navigator::Stop() //-----------------------------------------------------------------------------
void Navigator::Rotate(float rotationSpeed)
+>>>>>>> .r199 {
+<<<<<<< .mine + Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(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() +{ + 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; + + 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<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(); + } +} +======= this->rotationSpeed = min(rotationSpeed, 255.0f);;
this->direction = EMPTY_FLOAT;
this->targetAngle = EMPTY_FLOAT;
@@ -257,3 +520,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 56e7b8f..848c9fa 100755 --- a/source/Concept/Framework/modules/executor/navigator.h +++ b/source/Concept/Framework/modules/executor/navigator.h @@ -1,3 +1,75 @@ +<<<<<<< .mine +#ifndef _NAVIGATOR_H +#define _NAVIGATOR_H + +//#include <math.h> +#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
@@ -68,4 +140,5 @@ public: }
};
+>>>>>>> .r199 #endif diff --git a/source/Concept/Framework/modules/input/distance_sensor.c b/source/Concept/Framework/modules/input/distance_sensor.c index 977784f..508eeed 100755 --- a/source/Concept/Framework/modules/input/distance_sensor.c +++ b/source/Concept/Framework/modules/input/distance_sensor.c @@ -9,24 +9,24 @@ float Distance_Sensor::GetDistance() *hardwareDDR |= pin;//Set pin output
*hardwarePort |= pin;//Activate port
usleep(10);//Wait for 10µs
- //*hardwarePort &= ~pin;//Deactivate port
+ *hardwarePort &= ~pin;//Deactivate port
*hardwareDDR &= ~pin;//Set pin input
- (parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1);
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1);
//Wait for response
- while(!(PINC & pin)){asm volatile("nop");}
+ for(int i=0;(!(PINC & pin))&&(i < 1000);i++) {asm volatile("nop");}
- (parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1);
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1);
//Calculate duration of response
- while(*hardwarePin & pin)
+ while((*hardwarePin & pin)&&(result < 300000))
{
result++;
asm volatile("nop");
}
- (parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1);
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1);
return (float(result) * DISTANCE_PER_VALUE);
}
diff --git a/source/Concept/Framework/modules/input/mouse_sensor.h b/source/Concept/Framework/modules/input/mouse_sensor.h index 30f9d53..460e387 100755 --- a/source/Concept/Framework/modules/input/mouse_sensor.h +++ b/source/Concept/Framework/modules/input/mouse_sensor.h @@ -42,9 +42,9 @@ public: this->pinSCK = (1 << 6);
this->registerConfig = 0x00;
this->registerPixelData = 0x08;
- this->registerSqual = 0x4;
- this->registerDeltaX = 0x3;
- this->registerDeltaY = 0x2;
+ this->registerSqual = 0x04;
+ this->registerDeltaX = 0x03;
+ this->registerDeltaY = 0x02;
this->configReset = 0x80;
this->configAwake = 0x01;
break;
@@ -52,13 +52,13 @@ public: this->hardwarePort = &PORTC;
this->hardwareDDR = &DDRC;
this->hardwarePin = &PINC;
- this->pinSDA = (1 << 5);
- this->pinSCK = (1 << 7);
+ this->pinSDA = (1 << 7);
+ this->pinSCK = (1 << 5);
this->registerConfig = 0x00;
this->registerPixelData = 0x08;
- this->registerSqual = 0x4;
- this->registerDeltaX = 0x3;
- this->registerDeltaY = 0x2;
+ this->registerSqual = 0x04;
+ this->registerDeltaX = 0x03;
+ this->registerDeltaY = 0x02;
this->configReset = 0x80;
this->configAwake = 0x01;
break;
diff --git a/source/Concept/Framework/modules/output/display.h b/source/Concept/Framework/modules/output/display.h index e221120..bbfa292 100755 --- a/source/Concept/Framework/modules/output/display.h +++ b/source/Concept/Framework/modules/output/display.h @@ -39,7 +39,7 @@ public: this->settingIllumination = 76;
this->settingCursorPosition = 79;
msleep(100);
- uart1_init(103);//9600 BAUD at 16MHz Atmel
+ uart1_init(51);//19200 BAUD at 16MHz Atmel
msleep(100);
break;
default:
@@ -52,6 +52,8 @@ public: this->settingCursorPosition = 0;
break;
}
+
+ Clear();
}
protected:
@@ -137,6 +139,7 @@ public: void NewLine()
{
SendCommand(commandNewLine);
+ ReturnCursor();
}
bool GetCursorVisible()
diff --git a/source/Concept/Framework/modules/output/engine.h b/source/Concept/Framework/modules/output/engine.h index 27b9905..7b7a044 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;
- float curSpeed;
+ int curSpeed;
//Hardware
volatile uint8* hardwarePort;
@@ -94,16 +94,19 @@ protected: }
public:
- float GetSpeed()
+ int GetSpeed()
{
return curSpeed;
}
- void SetSpeed(float newSpeed)
+ void SetSpeed(int newSpeed)
{
curSpeed = newSpeed;
- *pwmSpeed = (abs((int16)(newSpeed / SPEED_PER_PWM)));
+ int pwm = abs(newSpeed);
+ if(pwm > 255) pwm = 255;
+
+ *pwmSpeed = pwm;
UpdateDirection();
}
|