diff options
Diffstat (limited to 'source/Concept/Framework/modules')
28 files changed, 1594 insertions, 0 deletions
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<Engine>(i))->SetSpeed(0);
+ (parent->GetModule<Engine>(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<Engine>(i))->SetSpeed(rotationSpeed);
+ (parent->GetModule<Engine>(i))->SetEnabled(true);
+ }
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::Update()
+{
+ Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(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<Display>(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1);
+
+ //Wait for response
+ while(!(PINC & pin)){asm volatile("nop");}
+
+ (parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1);
+
+ //Calculate duration of response
+ while(*hardwarePin & pin)
+ {
+ result++;
+ asm volatile("nop");
+ }
+
+ (parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1);
+
+ return (float(result) * DISTANCE_PER_VALUE);
+}
diff --git a/source/Concept/Framework/modules/input/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<IR_Sensor>(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 |