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

Code-stuff

Browse source
This commit is contained in:
sicarius 2007-02-19 20:57:03 +00:00
parent b3196682b1
commit 42a38959ff
12 changed files with 474 additions and 55 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -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

73
source/Concept/Framework/modules/executor/navigator.h
View file

@ -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

12
source/Concept/Framework/modules/input/distance_sensor.c
View file

@ -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);
}

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

@ -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;

5
source/Concept/Framework/modules/output/display.h
View file

@ -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()

11
source/Concept/Framework/modules/output/engine.h
View file

@ -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();
}