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

+++ corrected svn cripplement

Browse source
This commit is contained in:
masterm 2007-02-19 22:02:01 +00:00
parent 42a38959ff
commit ec2a18e931
8 changed files with 40 additions and 405 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
source/Concept/Framework/RoboCode.aps
View file

File diff suppressed because one or more lines are too long

10
source/Concept/Framework/defines.h
View file

@ -58,15 +58,9 @@
//Constants
#define SPEED_PER_PWM 1
#define DISTANCE_PER_VALUE 1
<<<<<<< .mine
#define PI 3.14159265358979323846f
#define CYCLES_PER_CORRECTION 200
#define EMPTY_FLOAT 81188.1484f
=======
#define PI 3.14159265358979323846f
#define CYCLES_PER_CORRECTION 200
>>>>>>> .r199
#define PI 3.14159265358979323846f
#define CYCLES_PER_CORRECTION 200
#define EMPTY_FLOAT 81188.1484f
//IO Module Names

22
source/Concept/Framework/main.c
View file

@ -5,7 +5,7 @@ int main()
//Init our robot
Robot* localRobot = new Robot();
sleep(1); // Wait for LCD-Display
sleep(1);//Wait for Hardware
//Init Displays
for(uint8 i = IO_DISPLAY_START; i < IO_DISPLAY_END; i++)
@ -15,9 +15,6 @@ int main()
newDisplay = NULL;
}
Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN);
// ourDisplay->Print("Starting..."); ourDisplay->NewLine(); // Debug output
//Init Engines
for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
{
@ -25,7 +22,6 @@ int main()
localRobot->AddModule(newEngine);
newEngine = NULL;
}
// ourDisplay->Print("Engines Ready"); ourDisplay->NewLine(); // Debug output
//Init Dribbler
for(uint8 i = IO_DRIBBLER_START; i < IO_DRIBBLER_END; i++)
@ -34,7 +30,6 @@ int main()
localRobot->AddModule(newDribbler);
newDribbler = NULL;
}
// ourDisplay->Print("Dribbler Ready"); ourDisplay->NewLine(); // Debug output
//Init Kicker
for(uint8 i = IO_KICKER_START; i < IO_KICKER_END; i++)
@ -43,7 +38,6 @@ int main()
localRobot->AddModule(newKicker);
newKicker = NULL;
}
// ourDisplay->Print("Kicker Ready"); ourDisplay->NewLine(); // Debug output
//Init Sensors
for(uint8 i = IO_SENSOR_START; i < IO_SENSOR_END; i++)
@ -63,7 +57,6 @@ int main()
IR_Sensor* newSensor = new IR_Sensor(i);
localRobot->AddModule(newSensor);
newSensor = NULL;
// ourDisplay->Print("SensorIR Ready"); ourDisplay->NewLine(); // Debug output
break;
}
case IO_SENSOR_DISTANCE_0_DEG:
@ -74,7 +67,6 @@ int main()
Distance_Sensor* newSensor = new Distance_Sensor(i);
localRobot->AddModule(newSensor);
newSensor = NULL;
// ourDisplay->Print("DistanceSensor Ready"); // Debug output
break;
}
@ -84,7 +76,6 @@ int main()
Mouse_Sensor* newSensor = new Mouse_Sensor(i);
localRobot->AddModule(newSensor);
newSensor = NULL;
// ourDisplay->Print("MouseSensor Ready"); ourDisplay->NewLine(); // Debug output
break;
}
@ -98,7 +89,6 @@ int main()
}
}
}
// ourDisplay->Print("Sensors Ready"); ourDisplay->NewLine(); // Debug output
//Init Leds
for(uint8 i = IO_LED_START; i < IO_LED_END; i++)
@ -107,7 +97,6 @@ int main()
localRobot->AddModule(newLed);
newLed = NULL;
}
// ourDisplay->Print("LEDs Ready"); ourDisplay->NewLine(); // Debug output
//Init Keyboards
for(uint8 i = IO_KEYBOARD_START; i < IO_KEYBOARD_END; i++)
@ -116,7 +105,6 @@ int main()
localRobot->AddModule(newKeyboard);
newKeyboard = NULL;
}
// ourDisplay->Print("Keyboard Ready"); ourDisplay->NewLine(); // Debug output
//Init Position Tracker
for(uint8 i = IO_POSITION_TRACKER_START; i < IO_POSITION_TRACKER_END; i++)
@ -125,7 +113,6 @@ int main()
localRobot->AddModule(newPositionTracker);
newPositionTracker = NULL;
}
// ourDisplay->Print("PositionTracker Ready"); ourDisplay->NewLine(); // Debug output
//Init Ball Tracker
for(uint8 i = IO_BALL_TRACKER_START; i < IO_BALL_TRACKER_END; i++)
@ -134,7 +121,6 @@ int main()
localRobot->AddModule(newBallTracker);
newBallTracker = NULL;
}
// ourDisplay->Print("Balltracker Ready"); ourDisplay->NewLine(); // Debug output
//Init Navigators
for(uint8 i = IO_NAVIGATOR_START; i < IO_NAVIGATOR_END; i++)
@ -143,9 +129,11 @@ int main()
localRobot->AddModule(newNavigator);
newNavigator = NULL;
}
// ourDisplay->Print("Navigator Ready"); ourDisplay->NewLine(); // Debug output
// ourDisplay->Print("All Ready"); ourDisplay->NewLine(); // Debug output
sleep(1);
//Debug code
Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN);
ourDisplay->Clear();

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

@ -1,138 +1,3 @@
<<<<<<< .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"
//-----------------------------------------------------------------------------
@ -154,136 +19,8 @@ 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->rotationSpeed = min(fabs(rotationSpeed), 255.0f) * sign(rotationSpeed);
this->direction = EMPTY_FLOAT;
this->targetAngle = EMPTY_FLOAT;
this->targetX = EMPTY_FLOAT;
@ -301,8 +38,8 @@ void Navigator::CalculateEngines()
void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed)
{
this->rotationSpeed = min(rotationSpeed, 255.0f);
this->direction = newDirection;
this->targetAngle = newAngle;
this->direction = newDirection * PI / 180.0f;
this->targetAngle = newAngle * PI / 180.0f;
this->targetX = EMPTY_FLOAT;
this->targetY = EMPTY_FLOAT;
this->robotSpeed = newSpeed;
@ -330,7 +67,7 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
this->targetX = newX;
this->targetY = newY;
this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);;
this->targetAngle = newAngle;
this->targetAngle = newAngle * PI / 180.0f;
this->robotSpeed = newSpeed;
if(targetAngle - locationeer->GetOrientation() > PI)
@ -520,4 +257,3 @@ void Navigator::CalculateEngines()
Stop();
}
}
>>>>>>> .r199

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

@ -1,8 +1,6 @@
<<<<<<< .mine
#ifndef _NAVIGATOR_H
#define _NAVIGATOR_H
//#include <math.h>
#include "../../stdafx.h"
class Navigator : public IO_Module
@ -69,76 +67,4 @@ public:
}
};
=======
#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);
}
};
>>>>>>> .r199
#endif

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

@ -12,12 +12,8 @@ float Distance_Sensor::GetDistance()
*hardwarePort &= ~pin;//Deactivate port
*hardwareDDR &= ~pin;//Set pin input
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1);
//Wait for response
for(int i=0;(!(PINC & pin))&&(i < 1000);i++) {asm volatile("nop");}
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1);
for(uint16 i = 0; (!(*hardwarePin & pin)) && (i < 1000); i++) { asm volatile("nop"); }
//Calculate duration of response
while((*hardwarePin & pin)&&(result < 300000))
@ -26,7 +22,5 @@ float Distance_Sensor::GetDistance()
asm volatile("nop");
}
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1);
return (float(result) * DISTANCE_PER_VALUE);
}

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

@ -58,7 +58,7 @@ public:
protected:
bool enabled;
int curSpeed;
int16 curSpeed;
//Hardware
volatile uint8* hardwarePort;
@ -94,16 +94,16 @@ protected:
}
public:
int GetSpeed()
int16 GetSpeed()
{
return curSpeed;
}
void SetSpeed(int newSpeed)
void SetSpeed(int16 newSpeed)
{
curSpeed = newSpeed;
int pwm = abs(newSpeed);
uint16 pwm = abs(newSpeed);
if(pwm > 255) pwm = 255;
*pwmSpeed = pwm;

19
source/Concept/Framework/tools.h
View file

@ -3,6 +3,7 @@
#include <stdlib.h>
#include <math.h>
#include "defines.h"
#ifndef new
void* operator new(size_t sz);
@ -13,28 +14,28 @@
#define min(a, b) (((a) < (b)) ? (a) : (b))
#define sign(a) (((a) < 0) ? (-1) : (1))
inline void sleep(int sec)
inline void sleep(uint16 sec)
{
for (int s=0; s<sec; s++) {
for (long int i=0; i<1405678; i++) {
for (uint16 s = 0; s < sec; s++) {
for (uint32 i = 0; i < 1405678; i++) {
asm volatile("nop");
}
}
};
inline void msleep(int msec)
inline void msleep(uint16 msec)
{
for (int s=0; s<msec; s++) {
for (long int i=0; i<1405; i++) {
for (uint16 s = 0; s < msec; s++) {
for (uint16 i = 0; i < 1405; i++) {
asm volatile("nop");
}
}
};
inline void usleep(int usec)
inline void usleep(uint16 usec)
{
for (int s=0; s<usec; s++) {
for (long int i=0; i<3; i++) {
for (uint16 s = 0; s < usec; s++) {
for (uint8 i = 0; i < 3; i++) {
asm volatile("nop");
}
}