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

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

@ -58,8 +58,14 @@
//Constants //Constants
#define SPEED_PER_PWM 1 #define SPEED_PER_PWM 1
#define DISTANCE_PER_VALUE 1 #define DISTANCE_PER_VALUE 1
<<<<<<< .mine
#define PI 3.14159265358979323846f #define PI 3.14159265358979323846f
#define CYCLES_PER_CORRECTION 200 #define CYCLES_PER_CORRECTION 200
#define EMPTY_FLOAT 81188.1484f
=======
#define PI 3.14159265358979323846f
#define CYCLES_PER_CORRECTION 200
>>>>>>> .r199
#define EMPTY_FLOAT 81188.1484f #define EMPTY_FLOAT 81188.1484f

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

@ -5,6 +5,19 @@ int main()
//Init our robot //Init our robot
Robot* localRobot = new Robot(); Robot* localRobot = new Robot();
sleep(1); // Wait for LCD-Display
//Init Displays
for(uint8 i = IO_DISPLAY_START; i < IO_DISPLAY_END; i++)
{
Display* newDisplay = new Display(i);
localRobot->AddModule(newDisplay);
newDisplay = NULL;
}
Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN);
// ourDisplay->Print("Starting..."); ourDisplay->NewLine(); // Debug output
//Init Engines //Init Engines
for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
{ {
@ -12,6 +25,7 @@ int main()
localRobot->AddModule(newEngine); localRobot->AddModule(newEngine);
newEngine = NULL; newEngine = NULL;
} }
// ourDisplay->Print("Engines Ready"); ourDisplay->NewLine(); // Debug output
//Init Dribbler //Init Dribbler
for(uint8 i = IO_DRIBBLER_START; i < IO_DRIBBLER_END; i++) for(uint8 i = IO_DRIBBLER_START; i < IO_DRIBBLER_END; i++)
@ -20,6 +34,7 @@ int main()
localRobot->AddModule(newDribbler); localRobot->AddModule(newDribbler);
newDribbler = NULL; newDribbler = NULL;
} }
// ourDisplay->Print("Dribbler Ready"); ourDisplay->NewLine(); // Debug output
//Init Kicker //Init Kicker
for(uint8 i = IO_KICKER_START; i < IO_KICKER_END; i++) for(uint8 i = IO_KICKER_START; i < IO_KICKER_END; i++)
@ -28,6 +43,7 @@ int main()
localRobot->AddModule(newKicker); localRobot->AddModule(newKicker);
newKicker = NULL; newKicker = NULL;
} }
// ourDisplay->Print("Kicker Ready"); ourDisplay->NewLine(); // Debug output
//Init Sensors //Init Sensors
for(uint8 i = IO_SENSOR_START; i < IO_SENSOR_END; i++) for(uint8 i = IO_SENSOR_START; i < IO_SENSOR_END; i++)
@ -47,6 +63,7 @@ int main()
IR_Sensor* newSensor = new IR_Sensor(i); IR_Sensor* newSensor = new IR_Sensor(i);
localRobot->AddModule(newSensor); localRobot->AddModule(newSensor);
newSensor = NULL; newSensor = NULL;
// ourDisplay->Print("SensorIR Ready"); ourDisplay->NewLine(); // Debug output
break; break;
} }
case IO_SENSOR_DISTANCE_0_DEG: case IO_SENSOR_DISTANCE_0_DEG:
@ -57,6 +74,8 @@ int main()
Distance_Sensor* newSensor = new Distance_Sensor(i); Distance_Sensor* newSensor = new Distance_Sensor(i);
localRobot->AddModule(newSensor); localRobot->AddModule(newSensor);
newSensor = NULL; newSensor = NULL;
// ourDisplay->Print("DistanceSensor Ready"); // Debug output
break; break;
} }
case IO_SENSOR_MOUSE_LEFT: case IO_SENSOR_MOUSE_LEFT:
@ -65,6 +84,8 @@ int main()
Mouse_Sensor* newSensor = new Mouse_Sensor(i); Mouse_Sensor* newSensor = new Mouse_Sensor(i);
localRobot->AddModule(newSensor); localRobot->AddModule(newSensor);
newSensor = NULL; newSensor = NULL;
// ourDisplay->Print("MouseSensor Ready"); ourDisplay->NewLine(); // Debug output
break; break;
} }
//Other cases //Other cases
@ -77,6 +98,7 @@ int main()
} }
} }
} }
// ourDisplay->Print("Sensors Ready"); ourDisplay->NewLine(); // Debug output
//Init Leds //Init Leds
for(uint8 i = IO_LED_START; i < IO_LED_END; i++) for(uint8 i = IO_LED_START; i < IO_LED_END; i++)
@ -85,14 +107,7 @@ int main()
localRobot->AddModule(newLed); localRobot->AddModule(newLed);
newLed = NULL; newLed = NULL;
} }
// ourDisplay->Print("LEDs Ready"); ourDisplay->NewLine(); // Debug output
//Init Displays
for(uint8 i = IO_DISPLAY_START; i < IO_DISPLAY_END; i++)
{
Display* newDisplay = new Display(i);
localRobot->AddModule(newDisplay);
newDisplay = NULL;
}
//Init Keyboards //Init Keyboards
for(uint8 i = IO_KEYBOARD_START; i < IO_KEYBOARD_END; i++) for(uint8 i = IO_KEYBOARD_START; i < IO_KEYBOARD_END; i++)
@ -101,6 +116,7 @@ int main()
localRobot->AddModule(newKeyboard); localRobot->AddModule(newKeyboard);
newKeyboard = NULL; newKeyboard = NULL;
} }
// ourDisplay->Print("Keyboard Ready"); ourDisplay->NewLine(); // Debug output
//Init Position Tracker //Init Position Tracker
for(uint8 i = IO_POSITION_TRACKER_START; i < IO_POSITION_TRACKER_END; i++) for(uint8 i = IO_POSITION_TRACKER_START; i < IO_POSITION_TRACKER_END; i++)
@ -109,6 +125,7 @@ int main()
localRobot->AddModule(newPositionTracker); localRobot->AddModule(newPositionTracker);
newPositionTracker = NULL; newPositionTracker = NULL;
} }
// ourDisplay->Print("PositionTracker Ready"); ourDisplay->NewLine(); // Debug output
//Init Ball Tracker //Init Ball Tracker
for(uint8 i = IO_BALL_TRACKER_START; i < IO_BALL_TRACKER_END; i++) for(uint8 i = IO_BALL_TRACKER_START; i < IO_BALL_TRACKER_END; i++)
@ -117,6 +134,7 @@ int main()
localRobot->AddModule(newBallTracker); localRobot->AddModule(newBallTracker);
newBallTracker = NULL; newBallTracker = NULL;
} }
// ourDisplay->Print("Balltracker Ready"); ourDisplay->NewLine(); // Debug output
//Init Navigators //Init Navigators
for(uint8 i = IO_NAVIGATOR_START; i < IO_NAVIGATOR_END; i++) for(uint8 i = IO_NAVIGATOR_START; i < IO_NAVIGATOR_END; i++)
@ -125,63 +143,110 @@ int main()
localRobot->AddModule(newNavigator); localRobot->AddModule(newNavigator);
newNavigator = NULL; newNavigator = NULL;
} }
// ourDisplay->Print("Navigator Ready"); ourDisplay->NewLine(); // Debug output
// ourDisplay->Print("All Ready"); ourDisplay->NewLine(); // Debug output
sleep(1);
ourDisplay->Clear();
IR_Sensor* ourSensor = NULL; IR_Sensor* ourSensor = NULL;
Distance_Sensor* distanceSensor = NULL;
uint16 value = 0; uint16 value = 0;
int8 value2 = 0; int8 value2 = 0;
Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN);
Keyboard* ourKeyboard = localRobot->GetModule<Keyboard>(IO_KEYBOARD_MAIN); Keyboard* ourKeyboard = localRobot->GetModule<Keyboard>(IO_KEYBOARD_MAIN);
uint32 i = 1; uint32 i = 1;
Navigator* ourNavigator = localRobot->GetModule<Navigator>(IO_NAVIGATOR_MAIN); Navigator* ourNavigator = localRobot->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
float rotation = 0; float rotation = 0;
ourDisplay->Print("Starting...", 1, 1); Mouse_Sensor* mouse_left = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
Mouse_Sensor* mouse_right = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
//Run //Run
while(true) while(true)
{ {
/*ourDisplay->Print(i++,1,1);
ourDisplay->Print(mouse_left->GetXMovement(),1,2);
ourDisplay->Print(" ");
ourDisplay->Print(mouse_left->GetYMovement(),10,2);
ourDisplay->Print(" ");
ourDisplay->Print(mouse_right->GetXMovement(),1,3);
ourDisplay->Print(" ");
ourDisplay->Print(mouse_right->GetYMovement(),10,3);
ourDisplay->Print(" ");
ourDisplay->Print(" ",1,4); // clear
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
value = distanceSensor->GetDistance();
ourDisplay->Print(value, 1, 4);
ourDisplay->Print(";");
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_90_DEG);
value = distanceSensor->GetDistance();
ourDisplay->Print(value);
ourDisplay->Print(";");
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_180_DEG);
value = distanceSensor->GetDistance();
ourDisplay->Print(value);
ourDisplay->Print(";");
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_270_DEG);
value = distanceSensor->GetDistance();
ourDisplay->Print(value);
ourDisplay->Print(";");
msleep(500);*/
uint8 someInput = ourKeyboard->GetInput(); uint8 someInput = ourKeyboard->GetInput();
//ourDisplay->Clear();
ourDisplay->Print(i++,1,1);
//ourDisplay->Print("Ready to accept...", 1, 2);
ourDisplay->Print(someInput, 1, 3);
ourDisplay->PrintFloat(rotation, 1, 4);
switch(someInput) switch(someInput)
{ {
case 0:
ourNavigator->Stop();
case 1: case 1:
ourNavigator->Drive(225.0f * PI / 180.0f, rotation, 200, rotation); ourNavigator->Drive(225, rotation, 200, rotation);
break; break;
case 2: case 2:
ourNavigator->Drive(180.0f * PI / 180.0f, rotation, 200, rotation); ourNavigator->Drive(180, rotation, 200, rotation);
break; break;
case 3: case 3:
ourNavigator->Drive(135.0f * PI / 180.0f, rotation, 200, rotation); ourNavigator->Drive(135, rotation, 200, rotation);
break; break;
case 4: case 4:
ourNavigator->Drive(270.0f * PI / 180.0f, rotation, 200, rotation); ourNavigator->Drive(270, rotation, 200, rotation);
break; break;
case 5: case 5:
ourNavigator->Stop(); ourNavigator->Drive(0, rotation, 0, rotation);
break; break;
case 6: case 6:
ourNavigator->Drive(90.0f * PI / 180.0f, rotation, 200, rotation); ourNavigator->Drive(90, rotation, 200, rotation);
break; break;
case 7: case 7:
ourNavigator->Drive(315.0f * PI / 180.0f, rotation, 200, rotation); ourNavigator->Drive(315, rotation, 200, rotation);
break; break;
case 8: case 8:
ourNavigator->Drive(0.0f * PI / 180.0f, rotation, 200, rotation); ourNavigator->Drive(0, rotation, 200, rotation);
break; break;
case 9: case 9:
ourNavigator->Drive(45.0f * PI / 180.0f, rotation, 200, rotation); ourNavigator->Drive(45, rotation, 200, rotation);
break; break;
case 10: case 10:
rotation -= 10;
break;
case 12:
rotation += 10; rotation += 10;
break; break;
case 12:
rotation -= 10;
break;
} }
msleep(50);/*
ourDisplay->Clear(); msleep(50);
ourDisplay->Print(i++);
/*
//ourDisplay->Clear();
ourDisplay->Print(i++,1,1);
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG); ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);
value = ourSensor->GetIRIntensity(); value = ourSensor->GetIRIntensity();
ourDisplay->Print(value, 1, 2); ourDisplay->Print(value, 1, 2);
@ -213,8 +278,10 @@ int main()
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG); ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG);
value = ourSensor->GetIRIntensity(); value = ourSensor->GetIRIntensity();
ourDisplay->Print(value); ourDisplay->Print(value);
ourDisplay->Print(";");*/ ourDisplay->Print(";");
msleep(500);
*/
localRobot->Update(); localRobot->Update();
//ourDisplay->PrintFloat(ourBallTracker->GetBallDirection(), 1, 4); //ourDisplay->PrintFloat(ourBallTracker->GetBallDirection(), 1, 4);

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" #include "navigator.h"
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
@ -19,7 +154,135 @@ void Navigator::Stop()
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void Navigator::Rotate(float rotationSpeed) 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(rotationSpeed, 255.0f);;
this->direction = EMPTY_FLOAT; this->direction = EMPTY_FLOAT;
this->targetAngle = EMPTY_FLOAT; this->targetAngle = EMPTY_FLOAT;
@ -257,3 +520,4 @@ void Navigator::CalculateEngines()
Stop(); 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 #ifndef _NAVIGATOR_H
#define _NAVIGATOR_H #define _NAVIGATOR_H
@ -68,4 +140,5 @@ public:
} }
}; };
>>>>>>> .r199
#endif #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 *hardwareDDR |= pin;//Set pin output
*hardwarePort |= pin;//Activate port *hardwarePort |= pin;//Activate port
usleep(10);//Wait for 10µs usleep(10);//Wait for 10µs
//*hardwarePort &= ~pin;//Deactivate port *hardwarePort &= ~pin;//Deactivate port
*hardwareDDR &= ~pin;//Set pin input *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 //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 //Calculate duration of response
while(*hardwarePin & pin) while((*hardwarePin & pin)&&(result < 300000))
{ {
result++; result++;
asm volatile("nop"); 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); 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->pinSCK = (1 << 6);
this->registerConfig = 0x00; this->registerConfig = 0x00;
this->registerPixelData = 0x08; this->registerPixelData = 0x08;
this->registerSqual = 0x4; this->registerSqual = 0x04;
this->registerDeltaX = 0x3; this->registerDeltaX = 0x03;
this->registerDeltaY = 0x2; this->registerDeltaY = 0x02;
this->configReset = 0x80; this->configReset = 0x80;
this->configAwake = 0x01; this->configAwake = 0x01;
break; break;
@ -52,13 +52,13 @@ public:
this->hardwarePort = &PORTC; this->hardwarePort = &PORTC;
this->hardwareDDR = &DDRC; this->hardwareDDR = &DDRC;
this->hardwarePin = &PINC; this->hardwarePin = &PINC;
this->pinSDA = (1 << 5); this->pinSDA = (1 << 7);
this->pinSCK = (1 << 7); this->pinSCK = (1 << 5);
this->registerConfig = 0x00; this->registerConfig = 0x00;
this->registerPixelData = 0x08; this->registerPixelData = 0x08;
this->registerSqual = 0x4; this->registerSqual = 0x04;
this->registerDeltaX = 0x3; this->registerDeltaX = 0x03;
this->registerDeltaY = 0x2; this->registerDeltaY = 0x02;
this->configReset = 0x80; this->configReset = 0x80;
this->configAwake = 0x01; this->configAwake = 0x01;
break; break;

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

@ -39,7 +39,7 @@ public:
this->settingIllumination = 76; this->settingIllumination = 76;
this->settingCursorPosition = 79; this->settingCursorPosition = 79;
msleep(100); msleep(100);
uart1_init(103);//9600 BAUD at 16MHz Atmel uart1_init(51);//19200 BAUD at 16MHz Atmel
msleep(100); msleep(100);
break; break;
default: default:
@ -52,6 +52,8 @@ public:
this->settingCursorPosition = 0; this->settingCursorPosition = 0;
break; break;
} }
Clear();
} }
protected: protected:
@ -137,6 +139,7 @@ public:
void NewLine() void NewLine()
{ {
SendCommand(commandNewLine); SendCommand(commandNewLine);
ReturnCursor();
} }
bool GetCursorVisible() bool GetCursorVisible()

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

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

8
source/Concept/Framework/robot.c
View file

@ -13,8 +13,8 @@ Robot::Robot()
DDRB = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7); DDRB = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7);
PORTB = (1 << 1); PORTB = (1 << 1);
//All output except PC4/PC5 (mousesensor SDA) //All output except PC4/PC7 (mousesensor SDA)
DDRC = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 6) | (1 << 7); DDRC = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 5) | (1 << 6);
PORTC = 0; PORTC = 0;
//All output except PD0+1(I2C) + 2+3(RS232) //All output except PD0+1(I2C) + 2+3(RS232)
@ -101,8 +101,8 @@ uint16 Robot::GetADCValue(uint8 channel)
uint32 result = 0; uint32 result = 0;
//Activate ADC and set division factor to 8 //Activate ADC and set division factor to 128
ADCSRA = (1 << ADEN) | (1 << ADPS1) | (1 << ADPS0); ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);
//Set multiplexer channel //Set multiplexer channel
ADMUX = channel; ADMUX = channel;

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

@ -33,9 +33,8 @@ inline void msleep(int msec)
inline void usleep(int usec) inline void usleep(int usec)
{ {
usec *= 100;
for (int s=0; s<usec; s++) { for (int s=0; s<usec; s++) {
for (long int i=0; i<1405; i++) { for (long int i=0; i<3; i++) {
asm volatile("nop"); asm volatile("nop");
} }
} }