From 3c3c628b617dc53f0b7b59285c7d67888074c33d Mon Sep 17 00:00:00 2001 From: sicarius Date: Sun, 18 Feb 2007 00:14:00 +0000 Subject: +++ Additional Codework --- .../Concept/Framework/modules/executor/navigator.c | 199 +++++++++++++++++++++ .../Concept/Framework/modules/executor/navigator.h | 53 ++++++ .../Framework/modules/input/distance_sensor.c | 32 ++++ .../Framework/modules/input/distance_sensor.h | 71 ++++++++ source/Concept/Framework/modules/input/ir_sensor.c | 9 + source/Concept/Framework/modules/input/ir_sensor.h | 62 +++++++ source/Concept/Framework/modules/input/keyboard.c | 1 + source/Concept/Framework/modules/input/keyboard.h | 87 +++++++++ .../Concept/Framework/modules/input/mouse_sensor.c | 1 + .../Concept/Framework/modules/input/mouse_sensor.h | 199 +++++++++++++++++++++ source/Concept/Framework/modules/input/sensor.c | 1 + source/Concept/Framework/modules/input/sensor.h | 27 +++ .../Framework/modules/interpreter/ball_tracker.c | 125 +++++++++++++ .../Framework/modules/interpreter/ball_tracker.h | 35 ++++ .../modules/interpreter/position_tracker.c | 7 + .../modules/interpreter/position_tracker.h | 45 +++++ source/Concept/Framework/modules/io_module.c | 1 + source/Concept/Framework/modules/io_module.h | 45 +++++ source/Concept/Framework/modules/output/display.c | 1 + source/Concept/Framework/modules/output/display.h | 198 ++++++++++++++++++++ source/Concept/Framework/modules/output/dribbler.c | 1 + source/Concept/Framework/modules/output/dribbler.h | 118 ++++++++++++ source/Concept/Framework/modules/output/engine.c | 1 + source/Concept/Framework/modules/output/engine.h | 124 +++++++++++++ source/Concept/Framework/modules/output/kicker.c | 1 + source/Concept/Framework/modules/output/kicker.h | 84 +++++++++ source/Concept/Framework/modules/output/led.c | 1 + source/Concept/Framework/modules/output/led.h | 65 +++++++ 28 files changed, 1594 insertions(+) create mode 100755 source/Concept/Framework/modules/executor/navigator.c create mode 100755 source/Concept/Framework/modules/executor/navigator.h create mode 100755 source/Concept/Framework/modules/input/distance_sensor.c create mode 100755 source/Concept/Framework/modules/input/distance_sensor.h create mode 100755 source/Concept/Framework/modules/input/ir_sensor.c create mode 100755 source/Concept/Framework/modules/input/ir_sensor.h create mode 100755 source/Concept/Framework/modules/input/keyboard.c create mode 100755 source/Concept/Framework/modules/input/keyboard.h create mode 100755 source/Concept/Framework/modules/input/mouse_sensor.c create mode 100755 source/Concept/Framework/modules/input/mouse_sensor.h create mode 100755 source/Concept/Framework/modules/input/sensor.c create mode 100755 source/Concept/Framework/modules/input/sensor.h create mode 100755 source/Concept/Framework/modules/interpreter/ball_tracker.c create mode 100755 source/Concept/Framework/modules/interpreter/ball_tracker.h create mode 100755 source/Concept/Framework/modules/interpreter/position_tracker.c create mode 100755 source/Concept/Framework/modules/interpreter/position_tracker.h create mode 100755 source/Concept/Framework/modules/io_module.c create mode 100755 source/Concept/Framework/modules/io_module.h create mode 100755 source/Concept/Framework/modules/output/display.c create mode 100755 source/Concept/Framework/modules/output/display.h create mode 100755 source/Concept/Framework/modules/output/dribbler.c create mode 100755 source/Concept/Framework/modules/output/dribbler.h create mode 100755 source/Concept/Framework/modules/output/engine.c create mode 100755 source/Concept/Framework/modules/output/engine.h create mode 100755 source/Concept/Framework/modules/output/kicker.c create mode 100755 source/Concept/Framework/modules/output/kicker.h create mode 100755 source/Concept/Framework/modules/output/led.c create mode 100755 source/Concept/Framework/modules/output/led.h (limited to 'source/Concept/Framework/modules') diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c new file mode 100755 index 0000000..459d4c1 --- /dev/null +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -0,0 +1,199 @@ +#include "navigator.h" + +//----------------------------------------------------------------------------- +void Navigator::Stop() +{ + this->direction = -1.0f; + this->targetAngle = -1.0f; + this->targetX = -1.0f; + this->targetY = -1.0f; + this->speed = 0; + this->rotationSpeed = 0; + + for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) + { + (parent->GetModule(i))->SetSpeed(0); + (parent->GetModule(i))->SetEnabled(true); + } +} + +//----------------------------------------------------------------------------- +void Navigator::Rotate(float rotationSpeed) +{ + this->rotationSpeed = rotationSpeed; + this->direction = -1.0f; + this->targetAngle = -1.0f; + this->targetX = -1.0f; + this->targetY = -1.0f; + this->speed = 0; + + for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) + { + (parent->GetModule(i))->SetSpeed(rotationSpeed); + (parent->GetModule(i))->SetEnabled(true); + } +} + +//----------------------------------------------------------------------------- +void Navigator::Update() +{ + Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); + + bool hasDistanceToDrive = true; + bool hasOrientationToAdjust = true; + + //Check for distance to drive + if((targetX >= 0) != (targetY >= 0)) + { + targetX = -1.0f; + targetY = -1.0f; + + hasDistanceToDrive = false; + } + else if(targetX >= 0 && targetY >= 0) + { + if(distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f) + { + targetX = -1.0f; + targetY = -1.0f; + + hasDistanceToDrive = false; + } + else + { + hasDistanceToDrive = true; + } + } + else + { + if(direction >= 0) + { + hasDistanceToDrive = true; + } + else + { + hasDistanceToDrive = false; + } + } + + //Check for orientation to adjust + if(targetAngle >= 0) + { + if(fabs(targetAngle - locationeer->GetOrientation()) < 0.1f) + { + hasOrientationToAdjust = false; + } + else + { + hasOrientationToAdjust = true; + } + } + else + { + if(rotationSpeed != 0) + { + hasOrientationToAdjust = true; + } + else + { + hasOrientationToAdjust = false; + } + } + + //Calculate directional/rotational engine speed + if(hasDistanceToDrive) + { + + + float maxRobotSpeed = 255.0f * sqrt(2) / 2.0f; + + if(speed > maxRobotSpeed) + { + speed = maxRobotSpeed; + } + + maxMotorSpeed = speed / (sqrt(2) / 2); + + + + relAngel = direction - orientation + + robotSpeed = sin(45) * maxMotorSpeed + maxMotorSpeed = robotSpeed / sin(45) + + if(relAngel > 45) + { + sin(relAngel) * (speed / sin(45)) * sin(relAngel + 45); + + back = speed / sin(relAngel); + } + else + { + + } + + left = + back = sin(relAngel) * speed + + + direction = 0: + orientation = 0: + left = speed + right = -speed + back = 0 + + direction = 0: + orientation = 90: + left = speed + right = speed + back = (sinVcos)(45) * speed + + direction = 0: + orientation = 45: + left = speed + right = 0 + back = -speed + + direction = 0: + orientation = 180: + left = -speed + right = speed + back = 0 + + } + else if(!hasOrientationToAdjust) + { + Stop(); + } + else + { + } +} + +// Aktualieren ohne Parameter +/*void Navigator::Update() { + // Richtung in x und y-Kompontente zerlegen + double y = cos((double)direction*0.01745); // richtung ist winkel + double x = sin((double)direction*0.01745); + + // Abweichung der Ausrichtung ermitteln(als winkel) + int w = sensor.GetAusrichtung() - ausrichtung; + + // Stärke der einzelnen Motoren berechnen + double v0 = (-x+sqrt(3)*y)/2; + double v1 = x; + double v2 = (-x-sqrt(3)*y)/2; + + // Ausgerechnete Stärke an die Motoren übergeben + board.motor(0,(int)((double)v0*speed +w)); + board.motor(1,(int)((double)v1*speed +w)); + board.motor(2,(int)((double)v2*speed +w)); +} + +// Aktualieren mit allen Parametern +void Navigator::Drive(float newDirection, float newAngle, float newSpeed) { + SetDirection(newDirection); + SetAngle(newAngle); + SetSpeed(newSpeed); + Update(); // Und anwenden +}*/ diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h new file mode 100755 index 0000000..ed8b3c8 --- /dev/null +++ b/source/Concept/Framework/modules/executor/navigator.h @@ -0,0 +1,53 @@ +#ifndef _NAVIGATOR_H +#define _NAVIGATOR_H + +#include "../../stdafx.h" + +class Navigator : public IO_Module +{ +public: + Navigator() + { + this->parent = NULL; + this->moduleId = 0; + this->direction = -1.0f; + this->targetAngle = -1.0f; + this->targetX = -1.0f; + this->targetY = -1.0f; + this->speed = 0; + this->rotationSpeed = 0; + } + + Navigator(uint32 navigatorId) + { + this->parent = NULL; + this->moduleId = navigatorId; + this->direction = -1.0f; + this->targetAngle = -1.0f; + this->targetX = -1.0f; + this->targetY = -1.0f; + this->speed = 0; + this->rotationSpeed = 0; + } + +protected: + float direction; + float targetAngle; + float targetX; + float targetY; + float speed; + 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); + + void Rotate(float rotationSpeed); +}; + +#endif diff --git a/source/Concept/Framework/modules/input/distance_sensor.c b/source/Concept/Framework/modules/input/distance_sensor.c new file mode 100755 index 0000000..977784f --- /dev/null +++ b/source/Concept/Framework/modules/input/distance_sensor.c @@ -0,0 +1,32 @@ +#include "distance_sensor.h" + +//----------------------------------------------------------------------------- +float Distance_Sensor::GetDistance() +{ + uint32 result = 0; + + //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 + + (parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1); + + //Wait for response + while(!(PINC & pin)){asm volatile("nop");} + + (parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1); + + //Calculate duration of response + while(*hardwarePin & pin) + { + result++; + asm volatile("nop"); + } + + (parent->GetModule(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1); + + return (float(result) * DISTANCE_PER_VALUE); +} diff --git a/source/Concept/Framework/modules/input/distance_sensor.h b/source/Concept/Framework/modules/input/distance_sensor.h new file mode 100755 index 0000000..8e6aa21 --- /dev/null +++ b/source/Concept/Framework/modules/input/distance_sensor.h @@ -0,0 +1,71 @@ +#ifndef _DISTANCE_SENSOR_H +#define _DISTANCE_SENSOR_H + +#include "../../stdafx.h" +#include "sensor.h" + +class Distance_Sensor : public Sensor +{ +public: + Distance_Sensor() + { + this->parent = NULL; + this->moduleId = 0; + this->hardwarePort = NULL; + this->hardwareDDR = NULL; + this->hardwarePin = NULL; + this->pin = 0; + } + + Distance_Sensor(uint32 sensorId) + { + this->parent = NULL; + this->moduleId = sensorId; + + switch(sensorId) + { + case IO_SENSOR_DISTANCE_0_DEG: + this->hardwarePort = &PORTC; + this->hardwareDDR = &DDRC; + this->hardwarePin = &PINC; + this->pin = (1 << 0); + break; + case IO_SENSOR_DISTANCE_90_DEG: + this->hardwarePort = &PORTC; + this->hardwareDDR = &DDRC; + this->hardwarePin = &PINC; + this->pin = (1 << 1); + break; + case IO_SENSOR_DISTANCE_180_DEG: + this->hardwarePort = &PORTC; + this->hardwareDDR = &DDRC; + this->hardwarePin = &PINC; + this->pin = (1 << 2); + break; + case IO_SENSOR_DISTANCE_270_DEG: + this->hardwarePort = &PORTC; + this->hardwareDDR = &DDRC; + this->hardwarePin = &PINC; + this->pin = (1 << 3); + break; + default: + this->hardwarePort = NULL; + this->hardwareDDR = NULL; + this->hardwarePin = NULL; + this->pin = 0; + break; + } + } + +protected: + //Hardware + volatile uint8* hardwarePort; + volatile uint8* hardwareDDR; + volatile uint8* hardwarePin; + uint8 pin; + +public: + float GetDistance(); +}; + +#endif diff --git a/source/Concept/Framework/modules/input/ir_sensor.c b/source/Concept/Framework/modules/input/ir_sensor.c new file mode 100755 index 0000000..c34feed --- /dev/null +++ b/source/Concept/Framework/modules/input/ir_sensor.c @@ -0,0 +1,9 @@ +#include "ir_sensor.h" + +//----------------------------------------------------------------------------- +uint16 IR_Sensor::GetIRIntensity() +{ + if(!parent) return 0; + + return parent->GetADCValue(channel); +} diff --git a/source/Concept/Framework/modules/input/ir_sensor.h b/source/Concept/Framework/modules/input/ir_sensor.h new file mode 100755 index 0000000..30e6ea4 --- /dev/null +++ b/source/Concept/Framework/modules/input/ir_sensor.h @@ -0,0 +1,62 @@ +#ifndef _IR_SENSOR_H +#define _IR_SENSOR_H + +#include "../../defines.h" +#include "../../robot.h" +#include "sensor.h" + +class IR_Sensor : public Sensor +{ +public: + IR_Sensor() + { + this->parent = NULL; + this->moduleId = 0; + } + + IR_Sensor(uint32 sensorId) + { + this->parent = NULL; + this->moduleId = sensorId; + + switch(sensorId) + { + case IO_SENSOR_IR_0_DEG: + this->channel = 0; + break; + case IO_SENSOR_IR_30_DEG: + this->channel = 1; + break; + case IO_SENSOR_IR_60_DEG: + this->channel = 2; + break; + case IO_SENSOR_IR_100_DEG: + this->channel = 3; + break; + case IO_SENSOR_IR_180_DEG: + this->channel = 4; + break; + case IO_SENSOR_IR_260_DEG: + this->channel = 5; + break; + case IO_SENSOR_IR_300_DEG: + this->channel = 6; + break; + case IO_SENSOR_IR_330_DEG: + this->channel = 7; + break; + default: + this->channel = 8; + break; + } + } + +protected: + //Hardware + uint8 channel; + +public: + uint16 GetIRIntensity(); +}; + +#endif diff --git a/source/Concept/Framework/modules/input/keyboard.c b/source/Concept/Framework/modules/input/keyboard.c new file mode 100755 index 0000000..65fc7cf --- /dev/null +++ b/source/Concept/Framework/modules/input/keyboard.c @@ -0,0 +1 @@ +#include "keyboard.h" diff --git a/source/Concept/Framework/modules/input/keyboard.h b/source/Concept/Framework/modules/input/keyboard.h new file mode 100755 index 0000000..243bb6c --- /dev/null +++ b/source/Concept/Framework/modules/input/keyboard.h @@ -0,0 +1,87 @@ +#ifndef _KEYBOARD_H +#define _KEYBOARD_H + +#include "../../stdafx.h" + +class Keyboard : public IO_Module +{ +public: + Keyboard() + { + this->parent = NULL; + this->moduleId = 0; + this->commandSetting = 0; + this->settingClearBuffer = 0; + } + + Keyboard(uint32 keyboardId) + { + this->parent = NULL; + this->moduleId = keyboardId; + + switch(keyboardId) + { + case IO_KEYBOARD_MAIN: + this->commandSetting = 27; + this->settingClearBuffer = 123; + break; + default: + this->commandSetting = 0; + this->settingClearBuffer = 0; + break; + } + } + +protected: + //Commands + uint8 commandSetting; + //Settings + uint8 settingClearBuffer; + + void SendCommand(uint8 newCommand) + { + switch(moduleId) + { + case IO_KEYBOARD_MAIN: + uart1_putc(newCommand); + break; + default: + break; + } + } + +public: + uint8 GetInput() + { + uint16 input = uart1_getc(); + + if(input == 0x100)//no data + { + return 0xEE;//empty + } + else if(input >= '0' && input <= '9') + { + return (uint8)(input - '0'); + } + else if(input == '*') + { + return 10; + } + else if(input == '#') + { + return 11; + } + else + { + return 0xFF;//unknown + } + } + + void ClearKeyBuffer() + { + SendCommand(commandSetting); + SendCommand(settingClearBuffer); + } +}; + +#endif diff --git a/source/Concept/Framework/modules/input/mouse_sensor.c b/source/Concept/Framework/modules/input/mouse_sensor.c new file mode 100755 index 0000000..214a13e --- /dev/null +++ b/source/Concept/Framework/modules/input/mouse_sensor.c @@ -0,0 +1 @@ +#include "mouse_sensor.h" diff --git a/source/Concept/Framework/modules/input/mouse_sensor.h b/source/Concept/Framework/modules/input/mouse_sensor.h new file mode 100755 index 0000000..30f9d53 --- /dev/null +++ b/source/Concept/Framework/modules/input/mouse_sensor.h @@ -0,0 +1,199 @@ +#ifndef _MOUSE_SENSOR_H +#define _MOUSE_SENSOR_H + +#include "../../stdafx.h" +#include "sensor.h" + +class Mouse_Sensor : public Sensor +{ +public: + Mouse_Sensor() + { + this->parent = NULL; + this->moduleId = 0; + this->hardwarePort = NULL; + this->hardwareDDR = NULL; + this->hardwarePin = NULL; + this->pinSDA = 0; + this->pinSCK = 0; + this->registerConfig = 0; + this->registerPixelData = 0; + this->registerSqual = 0; + this->registerDeltaX = 0; + this->registerDeltaY = 0; + this->configReset = 0; + this->configAwake = 0; + this->newImage = false; + } + + Mouse_Sensor(uint32 sensorId) + { + this->parent = NULL; + this->moduleId = sensorId; + this->newImage = false; + + switch(sensorId) + { + case IO_SENSOR_MOUSE_LEFT: + this->hardwarePort = &PORTC; + this->hardwareDDR = &DDRC; + this->hardwarePin = &PINC; + this->pinSDA = (1 << 4); + this->pinSCK = (1 << 6); + this->registerConfig = 0x00; + this->registerPixelData = 0x08; + this->registerSqual = 0x4; + this->registerDeltaX = 0x3; + this->registerDeltaY = 0x2; + this->configReset = 0x80; + this->configAwake = 0x01; + break; + case IO_SENSOR_MOUSE_RIGHT: + this->hardwarePort = &PORTC; + this->hardwareDDR = &DDRC; + this->hardwarePin = &PINC; + this->pinSDA = (1 << 5); + this->pinSCK = (1 << 7); + this->registerConfig = 0x00; + this->registerPixelData = 0x08; + this->registerSqual = 0x4; + this->registerDeltaX = 0x3; + this->registerDeltaY = 0x2; + this->configReset = 0x80; + this->configAwake = 0x01; + break; + default: + this->hardwarePort = NULL; + this->hardwareDDR = NULL; + this->hardwarePin = NULL; + this->pinSDA = 0; + this->pinSCK = 0; + this->registerConfig = 0; + this->registerPixelData = 0; + this->registerSqual = 0; + this->registerDeltaX = 0; + this->registerDeltaY = 0; + this->configReset = 0; + this->configAwake = 0; + break; + } + + *hardwareDDR |= pinSCK; + *hardwarePort &= ~pinSCK; + + Write(registerConfig, configReset); + Write(registerConfig, configAwake); + } + +protected: + //Hardware + volatile uint8* hardwarePort; + volatile uint8* hardwareDDR; + volatile uint8* hardwarePin; + uint8 pinSDA; + uint8 pinSCK; + bool newImage; + //Registers and Settings + uint8 registerConfig; + uint8 registerPixelData; + uint8 registerSqual; + uint8 registerDeltaX; + uint8 registerDeltaY; + uint8 configReset; + uint8 configAwake; + +public: + void WriteByte(uint8 newByte) + { + *hardwareDDR |= pinSDA;//Set SDA output + + for(uint8 i = 0; i < 8; i++) + { + *hardwarePort &= ~pinSCK;//prepare SCK + + //write data + *hardwarePort = (*hardwarePort & (~(*hardwarePin))) | + ((newByte >> 7) * pinSDA); + + newByte = newByte << 1;//prepare next byte + asm volatile("nop"); + + *hardwarePort |= pinSCK; + } + } + + void Write(int8 adr, uint8 data) + { + WriteByte(adr | 0x80); + WriteByte(data); + usleep(100); + } + + uint8 ReadByte() + { + uint8 data=0; + + *hardwareDDR &= ~pinSDA;//Set SDA input + + for(uint8 i = 0; i < 8; i++) + { + *hardwarePort &= ~pinSCK;//Prepare data + data = data << 1; + + asm volatile("nop"); + *hardwarePort |= pinSCK;//Prepare for reading + + data |= (*hardwarePin & pinSDA) / pinSDA; + } + + return data; + } + + uint8 Read(uint8 adr) + { + WriteByte(adr); + usleep(100); + + return ReadByte(); + } + + void ImagePrepare() + { + Write(registerConfig, configAwake); + Write(registerPixelData, 0x00); + + newImage = true; + } + + uint8 ImageRead() + { + uint8 pixel = Read(registerPixelData); + if(newImage) + { + while (!(pixel & 0x80))//0x80 indicates first pixel + { + pixel=Read(registerPixelData); + } + newImage = false; + } + + return pixel; + } + + uint8 GetSqual() + { + return Read(registerSqual); + } + + int8 GetXMovement() + { + return (int8)(Read(registerDeltaX)); + } + + int8 GetYMovement() + { + return (int8)(Read(registerDeltaY)); + } +}; + +#endif diff --git a/source/Concept/Framework/modules/input/sensor.c b/source/Concept/Framework/modules/input/sensor.c new file mode 100755 index 0000000..3036159 --- /dev/null +++ b/source/Concept/Framework/modules/input/sensor.c @@ -0,0 +1 @@ +#include "sensor.h" diff --git a/source/Concept/Framework/modules/input/sensor.h b/source/Concept/Framework/modules/input/sensor.h new file mode 100755 index 0000000..e44c739 --- /dev/null +++ b/source/Concept/Framework/modules/input/sensor.h @@ -0,0 +1,27 @@ +#ifndef _SENSOR_H +#define _SENSOR_H + +#include "../../defines.h" +#include "../io_module.h" + +class Sensor : public IO_Module +{ +public: + Sensor() + { + this->parent = NULL; + this->moduleId = 0; + } + + Sensor(uint32 sensorId) + { + this->parent = NULL; + this->moduleId = sensorId; + } + +protected: + +public: +}; + +#endif diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.c b/source/Concept/Framework/modules/interpreter/ball_tracker.c new file mode 100755 index 0000000..6679d3e --- /dev/null +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.c @@ -0,0 +1,125 @@ +#include "ball_tracker.h" + +//----------------------------------------------------------------------------- +void Ball_Tracker::Update() +{ + uint8 sensorCount = (IO_SENSOR_IR_330_DEG - IO_SENSOR_IR_0_DEG) + 1; + uint16 intensity[sensorCount]; + uint8 greatestIntensity = 0; + for(uint8 i = 0; i < sensorCount; i++) + { + IR_Sensor* currentSensor = parent->GetModule(i + IO_SENSOR_IR_0_DEG); + + intensity[i] = 1023 - currentSensor->GetIRIntensity(); + + if(intensity[i] < 24) + { + intensity[i] = 0; + } + + if(intensity[i] > intensity[greatestIntensity]) + { + greatestIntensity = i; + } + } + + if(intensity[greatestIntensity]) + { + uint8 secondIntensity = 0xFF; + uint8 leftSensor = (greatestIntensity + 1) % sensorCount; + uint8 rightSensor = (greatestIntensity + sensorCount - 1) % sensorCount; + + if(intensity[leftSensor]) + { + secondIntensity = leftSensor; + } + + if(intensity[rightSensor] > intensity[leftSensor]) + { + secondIntensity = rightSensor; + } + + float mainDirection; + + switch(greatestIntensity + IO_SENSOR_IR_0_DEG) + { + case IO_SENSOR_IR_0_DEG: + mainDirection = 0; + break; + case IO_SENSOR_IR_30_DEG: + mainDirection = 1.0f * PI / 6.0f; + break; + case IO_SENSOR_IR_60_DEG: + mainDirection = 1.0f * PI / 3.0f; + break; + case IO_SENSOR_IR_100_DEG: + mainDirection = 5.0f * PI / 9.0f; + break; + case IO_SENSOR_IR_180_DEG: + mainDirection = PI; + break; + case IO_SENSOR_IR_260_DEG: + mainDirection = 13.0f * PI / 9.0f; + break; + case IO_SENSOR_IR_300_DEG: + mainDirection = 15.0f * PI / 9.0f; + break; + case IO_SENSOR_IR_330_DEG: + mainDirection = 33.0f * PI / 18.0f; + break; + default: + mainDirection = -1.0f; + return; + break; + } + + if(secondIntensity != 0xFF) + { + float secondDirection; + + switch(secondIntensity + IO_SENSOR_IR_0_DEG) + { + case IO_SENSOR_IR_0_DEG: + secondDirection = 0; + break; + case IO_SENSOR_IR_30_DEG: + secondDirection = 1.0f * PI / 6.0f; + break; + case IO_SENSOR_IR_60_DEG: + secondDirection = 1.0f * PI / 3.0f; + break; + case IO_SENSOR_IR_100_DEG: + secondDirection = 5.0f * PI / 9.0f; + break; + case IO_SENSOR_IR_180_DEG: + secondDirection = PI; + break; + case IO_SENSOR_IR_260_DEG: + secondDirection = 13.0f * PI / 9.0f; + break; + case IO_SENSOR_IR_300_DEG: + secondDirection = 15.0f * PI / 9.0f; + break; + case IO_SENSOR_IR_330_DEG: + secondDirection = 33.0f * PI / 18.0f; + break; + default: + secondDirection = -1.0f; + return; + break; + } + + direction = (intensity[greatestIntensity] * mainDirection + + intensity[secondIntensity] * secondDirection) / + (intensity[greatestIntensity] + intensity[secondIntensity]); + } + else + { + direction = mainDirection; + } + } + else + { + direction = -1.0f; + } +} diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.h b/source/Concept/Framework/modules/interpreter/ball_tracker.h new file mode 100755 index 0000000..ed8801f --- /dev/null +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.h @@ -0,0 +1,35 @@ +#ifndef _BALL_TRACKER_H +#define _BALL_TRACKER_H + +#include "../../stdafx.h" + +class Ball_Tracker : public IO_Module +{ +public: + Ball_Tracker() + { + this->parent = NULL; + this->moduleId = 0; + this->direction = -1.0f; + } + + Ball_Tracker(uint32 trackerId) + { + this->parent = NULL; + this->moduleId = trackerId; + this->direction = -1.0f; + } + +protected: + float direction; + +public: + void Update(); + + float GetBallDirection() + { + return direction; + } +}; + +#endif diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c new file mode 100755 index 0000000..262069a --- /dev/null +++ b/source/Concept/Framework/modules/interpreter/position_tracker.c @@ -0,0 +1,7 @@ +#include "position_tracker.h" + +//----------------------------------------------------------------------------- +void Position_Tracker::Update() +{ + //insert code here +} diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h new file mode 100755 index 0000000..b31dd8f --- /dev/null +++ b/source/Concept/Framework/modules/interpreter/position_tracker.h @@ -0,0 +1,45 @@ +#ifndef _POSITION_TRACKER_H +#define _POSITION_TRACKER_H + +#include "../../stdafx.h" + +class Position_Tracker : public IO_Module +{ +public: + Position_Tracker() + { + this->parent = NULL; + this->moduleId = 0; + } + + Position_Tracker(uint32 trackerId) + { + this->parent = NULL; + this->moduleId = trackerId; + } + +protected: + float positionX; + float positionY; + float orientation; + +public: + void Update(); + + float GetPositionX() + { + return positionX; + } + + float GetPositionY() + { + return positionY; + } + + float GetOrientation() + { + return orientation; + } +}; + +#endif diff --git a/source/Concept/Framework/modules/io_module.c b/source/Concept/Framework/modules/io_module.c new file mode 100755 index 0000000..3549af5 --- /dev/null +++ b/source/Concept/Framework/modules/io_module.c @@ -0,0 +1 @@ +#include "io_module.h" diff --git a/source/Concept/Framework/modules/io_module.h b/source/Concept/Framework/modules/io_module.h new file mode 100755 index 0000000..383eefb --- /dev/null +++ b/source/Concept/Framework/modules/io_module.h @@ -0,0 +1,45 @@ +#ifndef _MODULE_H +#define _MODULE_H + +#include "../defines.h" +#include "../tools.h" + +class Robot; + +class IO_Module +{ +public: + IO_Module() + { + this->parent = NULL; + this->moduleId = 0; + } + + IO_Module(uint32 moduleId) + { + this->parent = NULL; + this->moduleId = moduleId; + } + +protected: + Robot* parent; + uint32 moduleId; + +public: + Robot* GetParent() + { + return parent; + } + + void SetParent(Robot* newParent) + { + parent = newParent; + } + + uint32 GetId() + { + return moduleId; + } +}; + +#endif diff --git a/source/Concept/Framework/modules/output/display.c b/source/Concept/Framework/modules/output/display.c new file mode 100755 index 0000000..82fbb41 --- /dev/null +++ b/source/Concept/Framework/modules/output/display.c @@ -0,0 +1 @@ +#include "display.h" diff --git a/source/Concept/Framework/modules/output/display.h b/source/Concept/Framework/modules/output/display.h new file mode 100755 index 0000000..e221120 --- /dev/null +++ b/source/Concept/Framework/modules/output/display.h @@ -0,0 +1,198 @@ +#ifndef _DISPLAY_H +#define _DISPLAY_H + +#include "../../stdafx.h" + +class Display : public IO_Module +{ +public: + Display() + { + this->parent = NULL; + this->moduleId = 0; + this->cursorVisible = false; + this->illuminationEnabled = true; + this->commandClear = 0; + this->commandReturnCursor = 0; + this->commandNewLine = 0; + this->commandSetting = 0; + this->settingCursorVisible = 0; + this->settingIllumination = 0; + this->settingCursorPosition = 0; + } + + Display(uint32 displayId) + { + this->parent = NULL; + this->moduleId = displayId; + this->cursorVisible = false; + this->illuminationEnabled = true; + + switch(displayId) + { + case IO_DISPLAY_MAIN: + this->commandClear = 12; + this->commandReturnCursor = 13; + this->commandNewLine = 10; + this->commandSetting = 27; + this->settingCursorVisible = 67; + this->settingIllumination = 76; + this->settingCursorPosition = 79; + msleep(100); + uart1_init(103);//9600 BAUD at 16MHz Atmel + msleep(100); + break; + default: + this->commandClear = 0; + this->commandReturnCursor = 0; + this->commandNewLine = 0; + this->commandSetting = 0; + this->settingCursorVisible = 0; + this->settingIllumination = 0; + this->settingCursorPosition = 0; + break; + } + } + +protected: + bool cursorVisible; + bool illuminationEnabled; + //Commands + uint8 commandClear; + uint8 commandReturnCursor; + uint8 commandNewLine; + uint8 commandSetting; + //Settings + uint8 settingCursorVisible; + uint8 settingIllumination; + uint8 settingCursorPosition; + + void SendCommand(uint8 newCommand) + { + switch(moduleId) + { + case IO_DISPLAY_MAIN: + uart1_putc(newCommand); + break; + default: + break; + } + } + +public: + void Print(char* newString) + { + switch(moduleId) + { + case IO_DISPLAY_MAIN: + uart1_puts(newString); + break; + default: + break; + } + } + + void Print(int32 newInteger) + { + char buffer[12]; + ltoa(newInteger, buffer, 10); + Print(buffer); + } + + void PrintFloat(float newFloat) + { + Print((int32)(newFloat)); + Print("."); + Print(abs((uint32)(newFloat - float((int32)(newFloat)) * 100000))); + } + + void Print(char* newString, uint8 xPos, uint8 yPos) + { + SetCursorPosition(xPos, yPos); + Print(newString); + } + + void Print(int32 newInteger, uint8 xPos, uint8 yPos) + { + SetCursorPosition(xPos, yPos); + Print(newInteger); + } + + void PrintFloat(float newFloat, uint8 xPos, uint8 yPos) + { + SetCursorPosition(xPos, yPos); + PrintFloat(newFloat); + } + + void Clear() + { + SendCommand(commandClear); + } + + void ReturnCursor() + { + SendCommand(commandReturnCursor); + } + + void NewLine() + { + SendCommand(commandNewLine); + } + + bool GetCursorVisible() + { + return cursorVisible; + } + + void SetCursorVisible(bool newStatus) + { + cursorVisible = newStatus; + + SendCommand(commandSetting); + SendCommand(settingCursorVisible); + + if(cursorVisible) + { + SendCommand(1); + } + else + { + SendCommand(0); + } + } + + bool GetLightingEnabled() + { + return illuminationEnabled; + } + + void SetLightingEnabled(bool newStatus) + { + illuminationEnabled = newStatus; + + SendCommand(commandSetting); + SendCommand(settingIllumination); + + if(illuminationEnabled) + { + SendCommand(1); + } + else + { + SendCommand(0); + } + } + + void SetCursorPosition(uint8 newX, uint8 newY) + { + if(!newX || newX > 20) return; + if(!newY || newY > 4) return; + + SendCommand(commandSetting); + SendCommand(settingCursorPosition); + SendCommand(newX); + SendCommand(newY); + } +}; + +#endif diff --git a/source/Concept/Framework/modules/output/dribbler.c b/source/Concept/Framework/modules/output/dribbler.c new file mode 100755 index 0000000..20557e4 --- /dev/null +++ b/source/Concept/Framework/modules/output/dribbler.c @@ -0,0 +1 @@ +#include "dribbler.h" diff --git a/source/Concept/Framework/modules/output/dribbler.h b/source/Concept/Framework/modules/output/dribbler.h new file mode 100755 index 0000000..b11af69 --- /dev/null +++ b/source/Concept/Framework/modules/output/dribbler.h @@ -0,0 +1,118 @@ +#ifndef _DRIBBLER_H +#define _DRIBBLER_H + +#include "../../stdafx.h" + +class Dribbler : public IO_Module +{ +public: + Dribbler() + { + this->enabled = false; + this->curSpeed = 0; + this->parent = NULL; + this->moduleId = 0; + this->hardwarePort = NULL; + this->portPower = NULL; + this->pinForward = 0; + this->pinReverse = 0; + this->pinPower = 0; + } + + Dribbler(uint32 dribblerId) + { + this->enabled = false; + this->curSpeed = 1.0f; + this->parent = NULL; + this->moduleId = dribblerId; + + switch(dribblerId) + { + case IO_DRIBBLER_MAIN: + this->hardwarePort = &PORTD; + this->portPower = &PORTA; + this->pinForward = (1 << 6); + this->pinReverse = (1 << 7); + this->pinPower = (1 << 5); + break; + default: + this->hardwarePort = NULL; + this->portPower = NULL; + this->pinForward = 0; + this->pinReverse = 0; + this->pinPower = 0; + break; + } + + UpdateDirection(); + } + +protected: + bool enabled; + float curSpeed; + + //Hardware + volatile uint8* hardwarePort; + volatile uint8* portPower; + uint8 pinForward; + uint8 pinReverse; + uint8 pinPower; + + void UpdateDirection() + { + if(enabled) + { + if(curSpeed > 0) + { + *hardwarePort |= pinForward; + *hardwarePort &= ~pinReverse; + } + else if(curSpeed < 0) + { + *hardwarePort |= pinReverse; + *hardwarePort &= ~pinForward; + } + else + { + *hardwarePort |= pinForward; + *hardwarePort |= pinReverse; + } + + *portPower |= pinPower; + } + else + { + *hardwarePort &= ~pinForward; + *hardwarePort &= ~pinReverse; + + *portPower &= ~pinPower; + } + } + +public: + float GetSpeed() + { + return curSpeed; + } + + void SetSpeed(float newSpeed) + { + curSpeed = newSpeed; + + UpdateDirection(); + } + + bool GetEnabled() + { + return enabled; + } + + void SetEnabled(bool newStatus) + { + enabled = newStatus; + + UpdateDirection(); + } +}; + +#endif diff --git a/source/Concept/Framework/modules/output/engine.c b/source/Concept/Framework/modules/output/engine.c new file mode 100755 index 0000000..5c14c17 --- /dev/null +++ b/source/Concept/Framework/modules/output/engine.c @@ -0,0 +1 @@ +#include "engine.h" diff --git a/source/Concept/Framework/modules/output/engine.h b/source/Concept/Framework/modules/output/engine.h new file mode 100755 index 0000000..27b9905 --- /dev/null +++ b/source/Concept/Framework/modules/output/engine.h @@ -0,0 +1,124 @@ +#ifndef _ENGINE_H +#define _ENGINE_H + +#include "../../stdafx.h" + +class Engine : public IO_Module +{ +public: + Engine() + { + this->enabled = false; + this->curSpeed = 0; + this->parent = NULL; + this->moduleId = 0; + this->hardwarePort = NULL; + this->pwmSpeed = NULL; + this->pinForward = 0; + this->pinReverse = 0; + } + + Engine(uint32 engineId) + { + this->enabled = false; + this->curSpeed = 0; + this->parent = NULL; + this->moduleId = engineId; + + switch(engineId) + { + case IO_ENGINE_DRIVE_LEFT: + this->hardwarePort = &PORTB; + this->pwmSpeed = &OCR1A; + this->pinForward = (1 << 0); + this->pinReverse = (1 << 1); + break; + case IO_ENGINE_DRIVE_BACK: + this->hardwarePort = &PORTB; + this->pwmSpeed = &OCR1B; + this->pinForward = (1 << 2); + this->pinReverse = (1 << 3); + break; + case IO_ENGINE_DRIVE_RIGHT: + this->hardwarePort = &PORTD; + this->pwmSpeed = &OCR3A; + this->pinForward = (1 << 5); + this->pinReverse = (1 << 4); + break; + default: + this->hardwarePort = NULL; + this->pwmSpeed = NULL; + this->pinForward = 0; + this->pinReverse = 0; + break; + } + + *this->pwmSpeed = 0; + } + +protected: + bool enabled; + float curSpeed; + + //Hardware + volatile uint8* hardwarePort; + volatile uint16* pwmSpeed; + uint8 pinForward; + uint8 pinReverse; + + void UpdateDirection() + { + if(enabled) + { + if(curSpeed > 0) + { + *hardwarePort |= pinForward; + *hardwarePort &= ~pinReverse; + } + else if(curSpeed < 0) + { + *hardwarePort |= pinReverse; + *hardwarePort &= ~pinForward; + } + else + { + *hardwarePort |= pinForward; + *hardwarePort |= pinReverse; + } + } + else + { + *hardwarePort &= ~pinForward; + *hardwarePort &= ~pinReverse; + } + } + +public: + float GetSpeed() + { + return curSpeed; + } + + void SetSpeed(float newSpeed) + { + curSpeed = newSpeed; + + *pwmSpeed = (abs((int16)(newSpeed / SPEED_PER_PWM))); + + UpdateDirection(); + } + + bool GetEnabled() + { + return enabled; + } + + void SetEnabled(bool newStatus) + { + enabled = newStatus; + + UpdateDirection(); + } +}; + +#endif diff --git a/source/Concept/Framework/modules/output/kicker.c b/source/Concept/Framework/modules/output/kicker.c new file mode 100755 index 0000000..6670efb --- /dev/null +++ b/source/Concept/Framework/modules/output/kicker.c @@ -0,0 +1 @@ +#include "kicker.h" diff --git a/source/Concept/Framework/modules/output/kicker.h b/source/Concept/Framework/modules/output/kicker.h new file mode 100755 index 0000000..080666a --- /dev/null +++ b/source/Concept/Framework/modules/output/kicker.h @@ -0,0 +1,84 @@ +#ifndef _KICKER_H +#define _KICKER_H + +#include "../../stdafx.h" + +class Kicker : public IO_Module +{ +public: + Kicker() + { + this->enabled = false; + this->parent = NULL; + this->moduleId = 0; + this->portPower = NULL; + this->portForward = NULL; + this->portReverse = NULL; + this->pinPower = 0; + this->pinForward = 0; + this->pinReverse = 0; + } + + Kicker(uint32 kickerId) + { + this->enabled = false; + this->parent = NULL; + this->moduleId = kickerId; + + switch(kickerId) + { + case IO_KICKER_MAIN: + this->portPower = &PORTG; + this->portForward = &PORTA; + this->portReverse = &PORTE; + this->pinPower = (1 << 3); + this->pinForward = (1 << 2); + this->pinReverse = (1 << 6); + break; + default: + this->portPower = NULL; + this->portForward = NULL; + this->portReverse = NULL; + this->pinPower = 0; + this->pinForward = 0; + this->pinReverse = 0; + break; + } + + *this->portForward |= this->pinForward; + *this->portReverse &= ~this->pinReverse; + } + +protected: + bool enabled; + + //Hardware + volatile uint8* portPower; + volatile uint8* portForward; + volatile uint8* portReverse; + uint8 pinPower; + uint8 pinForward; + uint8 pinReverse; + +public: + bool GetEnabled() + { + return enabled; + } + + void SetEnabled(bool newStatus) + { + enabled = newStatus; + + if(enabled) + { + *portPower |= pinPower; + } + else + { + *portPower &= ~pinPower; + } + } +}; + +#endif diff --git a/source/Concept/Framework/modules/output/led.c b/source/Concept/Framework/modules/output/led.c new file mode 100755 index 0000000..687b2db --- /dev/null +++ b/source/Concept/Framework/modules/output/led.c @@ -0,0 +1 @@ +#include "led.h" diff --git a/source/Concept/Framework/modules/output/led.h b/source/Concept/Framework/modules/output/led.h new file mode 100755 index 0000000..08e7466 --- /dev/null +++ b/source/Concept/Framework/modules/output/led.h @@ -0,0 +1,65 @@ +#ifndef _LED_H +#define _LED_H + +#include "../../stdafx.h" + +class Led : public IO_Module +{ +public: + Led() + { + this->enabled = false; + this->parent = NULL; + this->moduleId = 0; + this->hardwarePort = NULL; + this->pinPower = 0; + } + + Led(uint32 ledId) + { + this->enabled = false; + this->parent = NULL; + this->moduleId = ledId; + + switch(ledId) + { + case IO_LED_MAIN: + this->hardwarePort = &PORTB; + this->pinPower = (1 << 1); + break; + default: + this->hardwarePort = NULL; + this->pinPower = 0; + break; + } + } + +protected: + bool enabled; + + //Hardware + volatile uint8* hardwarePort; + uint8 pinPower; + +public: + bool GetEnabled() + { + return enabled; + } + + void SetEnabled(bool newStatus) + { + enabled = newStatus; + + if(enabled) + { + *hardwarePort &= ~pinPower; + } + else + { + *hardwarePort |= pinPower; + } + } +}; + +#endif -- cgit v1.2.3