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

Compare commits

base: neocturne:e1eb964ba72dfcb79bed19a136c1ce335c6def36
Branches Tags
neocturne:main
..
compare: neocturne:655dd7522fb60e2ef0e68437337178184109f347
Branches Tags
neocturne:main

No commits in common. "e1eb964ba72dfcb79bed19a136c1ce335c6def36" and "655dd7522fb60e2ef0e68437337178184109f347" have entirely different histories.
e1eb964ba7 ... 655dd7522f

37 changed files with 527 additions and 1708 deletions
Show stats Expand all files Collapse all files

63
Organisation/Gebühren.txt Normal file
View file

@ -0,0 +1,63 @@
:::Teilnahmegebühren, Qualifikationstunier Magdeburg:::
Teilnehmerzahl: 12 Personen
7 davon im Rescue-Team,
5 davon im Soccer-Team
Teilnahmegebühr pro Person: 10€
Teilnahmegebühren für Rescue-Team: 70€
Teilnahmegebühren für Soccer-Team: 50€
Teilnahmegebühren Gesamt: 120€
Gebühren werden überwiesen an:
Empfänger: Manuela Kanneberg
Stadtsparkasse Magdeburg, Blz 8105 3272
Kontonummer 4000021422
Vollständig überweisen bis spätestens 7. Februar 2007.
Überweisung wurde getätigt von: Jakob Runge
Geld wird eingesammelt von: Jakob Runge
:::Jugendherberge Magdeburg:::
Teilnehmerzahl: 10 Personen
(Jan Baumann und Roman Bierbach fahren nicht mit)
5 davon im Rescue-Team,
5 davon im Soccer-Team
Preis pro Person und Nacht: 19€
Übernachtungen: 2 Stück
Preis für Rescue-Team: 190€
Preis für Soccer-Team: 190€
Preis, Gesamt: 380€
Übernachtung erfolgt in Mehrbettzimmer, Frühstück inclusive
Geld wird eingesammelt und überwiesen von Jakob Runge
:::Bahnfahrt-Hin:::
Teilnemherzahl: 10 Personen
5 davon im Rescue-Team
5 davon im Soccer-Team
Fahrtgebühren pro Person: 11,45€
Fahrtgebühren, Rescue-Team:57,25€
Fahrtgebühren, Soccer-Team:57,25€
Fahrtgebühren, Gesamt: 114,50€
Geld wurde ausgelegt von Marian Meyer
Geld wird eingesammelt von Jakob Runge, und weitergegeben an Marian Meyer
:::Bahnfahrt, Rück:::
Teilnehmerzahl: 10 Personen
5 davon im Rescue-Team
5 davon im Soccer-Team
Fahrgebühren pro Person: 6,60€
Fahrtgebühren, Rescue-Team:33,00€
Fahrtgebühren, Soccer-Team:33,00€
Fahrtgebühren, Gesamt: 66,00€

BIN
Organisation/Kosten.ods Normal file
View file

Binary file not shown.

BIN
Organisation/Kosten.pdf Normal file
View file

Binary file not shown.

65
Organisation/Magdeburg.txt Normal file
View file

@ -0,0 +1,65 @@
Schule:
Lauenburgische Gelehrtenschule Ratzeburg
23909 Ratzeburg
Schleswig-Holstein
Chef(Anmeldung):
Jakob Runge
04544/1824
Teams:
CompAG- Rescue
RoboRescue secondary
7 Mitglieder:
Annika Dzakowski
15 Jahre
Weiblich
Bengt Jaeschke
14 Jahre
Männlich
Jan Baumann
15 Jahre
Männlich
Lena Schiffer
14 Jahre
Weiblich
Matthias Schiffer
17 Jahre
Männlich
Roman Bierbach
15 Jahre
Männlich
Tobias Runge
14 Jahre
Männlich
CompAG - Soccer
Soccer 2vs2 secondary
5 Mitglieder:
Jakob Runge
18 Jahre
Männlich
Marian Meyer
17 Jahre
Männlich
Marius Jahrens
18 Jahre
Männlich
Phillipp Schütz
14 Jahre
Männlich
Steffanie Walter
15 Jahre
Weiblich

24
Organisation/Packliste.txt Normal file
View file

@ -0,0 +1,24 @@
Unbedingt mitnehmen:
Zahnbürste
Zahncreme
Handtücher
Waschlappen
Duschzeug
Kleidung für 3 Tage
(Unterwäche, T-shirts, Hose, usw.)
(Regen mit einplanen)
Bettbezug
Bettlaken
Geld (min.50 Euro)
Für weibliche Teilnehmer die Erlaubnis
das sie mit im Jungszimmer schlafen dürfen!!! (falls gebraucht)
Techniksachen: Laptop, Handy, Camera, MP3 Player
"Freizeitbeschäftigungen" falls es Wartezeiten gibt

29
Organisation/TimeTable.txt Normal file
View file

@ -0,0 +1,29 @@
:::Zeitplan des Soccerteams:::
Teaminterne Ereignisse:
Offizielle Ereignisse:
Anmeldeschluss, Robocup Junior:
31.01.07
Überweisungsfrist für Magdeburg
07.02.07
Qualifikationstunier in Magdeburg (Messegelände)
23.02.07 - 25.02.07
->Offizielles Qualifikationstunier für deutsche Meisterschaft
->Abfahrt RZ->Magdeburg:
Bahnhof RZ, spätestens um 10:15 Uhr anwesend sein !
Qualifikationstunier in Vöringen
02.03.07 - 04.03.07
->Offizielles Qualifikationstunier für deutsche Meisterschaft
Deutsche Meisterschaft in Hannover
April
->Offizielles Qualifikationstunier für WM
WM in Atlanta
1.07.07 - 10.07.07

57
Organisation/Todo.txt Normal file
View file

@ -0,0 +1,57 @@
Hardware:
Getan:
Grundgerüst fertiggestellt
Hauptplatine ist montiert
Motorsteuerungen sind montiert
KeyLcd kist montiert
Kicker können montiert werden
Achsen-hüllsen sind gebohrt
Motoren können montiert werden
Grundstücke für Befestigung von Tastsensoren(Maschendraht)
Maussensoren eingebaut
Ultraschallsensoren befestigt
Motoren fertiggestellt
Ballsensoren fertiggestellt
Eigene Platinen fertiggestellt
Ballsensoren angeschlossen
Aussegungen für Dribbler
Todo:
Dribbler optimieren
Sensorhardware anschließen und testen
Verkabelung
Software:
Getan:
Disskusion um Grundkonzept
weitere Ausarbeitungen am Design
Programmierumgebung eingerichtet
Platinen ausreichend konfigueriert(fuses gesetzt)
Code ins Framework integriert
Todo:
Navigation abschließen
TESTEN !
Sensorik fertigstellen
TESTEN TESTEN TESTEN
Logik !!!
Allgemein:
Getan:
Teilnahmegebühren(120€) überwiesen
Todo:
Anmeldung abschließen
->10€ Kosten von Teilnehmern an Jakob
Magdeburg organisieren
->Hin-/Rückfahrt
->Unterkunft
->Verpflegung
->Kosten
->Verkehrswege/Lage

27
Organisation/contacts.txt Normal file
View file

@ -0,0 +1,27 @@
Members:
Marius Jahrens:
Telefon: 04541/802724
Handy: 0176/28835751
Email: the-matrixman@web.de
Marian Meyer:
Telefon: 04541/883188
Handy: 0157/72542713
Email: meyma@meymaris.de
Jakob Runge:
Telefon: 04544/1824 oder 04544/344
Handy: Nicht vorhanden
Email: sicarius@helix360.de
Phillip Schütz:
Telefon: Nicht angeschlossen
Handy: 0174 955 4846
Email: paladin_mof@hotmail.com
Geburtstag: 21.4.1992
Steffi Walter:
Telefon: 04541/859230
Handy: 01734681528
Email: ???

BIN
Sponsorenschreiben.doc Normal file
View file

Binary file not shown.

BIN
Sponsorenschreiben.pdf Normal file
View file

Binary file not shown.

72
source/Concept/Framework/Concept.vcproj
View file

@ -399,78 +399,6 @@
</File> </File>
</Filter> </Filter>
</Filter> </Filter>
<Filter
Name="Logic"
Filter="">
<Filter
Name="Source Files"
Filter="">
<File
RelativePath=".\modules\logic\logic.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\modules\logic\logic.h">
</File>
</Filter>
</Filter>
<Filter
Name="Wireless"
Filter="">
<Filter
Name="Source Files"
Filter="">
<File
RelativePath=".\modules\wireless.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\modules\wireless.h">
</File>
</Filter>
</Filter>
<Filter
Name="Aktuator"
Filter="">
<Filter
Name="Source Files"
Filter="">
<File
RelativePath=".\modules\executor\aktuator.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\modules\executor\aktuator.h">
</File>
</Filter>
</Filter>
<Filter
Name="Command_Handler"
Filter="">
<Filter
Name="Source Files"
Filter="">
<File
RelativePath=".\modules\interpreter\command_handler.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\modules\interpreter\command_handler.h">
</File>
</Filter>
</Filter>
</Filter> </Filter>
<Filter <Filter
Name="Hardware Interface" Name="Hardware Interface"

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

File diff suppressed because one or more lines are too long

168
source/Concept/Framework/atmega128io.c
View file

@ -581,173 +581,5 @@ void uart1_puts_p(const char *progmem_s )
}/* uart1_puts_p */ }/* uart1_puts_p */
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
// TWI-Driver
/*!
* TWI Bus initialsieren
* @return Resultat der Aktion
*/
int8 Init_TWI(void){
TWAR = OWN_ADR; /*!< Eigenen Slave Adresse setzen */
TWBR = 64; /*!< Setze Baudrate auf 100 KHz */
/*!< 16 MHz xtal */
TWCR = (1<<TWEN); /*!< TWI-Interface einschalten */
return 1;
}
/*!
* TWI Buss schliesen
* @return Resultat der Aktion
*/
int8 Close_TWI(void){
TWCR = (0<<TWEN); /*!< TWI-Interface ausschalten */
return 0;
}
/*!
* Warte auf TWI interrupt
*/
void Wait_TWI_int(void){
while (!(TWCR & (1<<TWINT)))
;
}
/*!
* Sende Start Sequence
* @return Resultat der Aktion
*/
uint8 Send_start(void){
TWCR = ((1<<TWINT)+(1<<TWSTA)+(1<<TWEN)); /*!< Sende START */
Wait_TWI_int(); /*!< Warte auf TWI interrupt */
if((TWSR != START)&&(TWSR != REP_START)) /*!< Ist der Status ein Anderer als Start (0x08) oder wiederholter Start (0x10) */
return TWSR; /*!< -> error und Rueckgabe TWSR. */
return SUCCESS; /*!< wenn OK Rueckgabe SUCCESS */
}
/*!
* Sende Stop Sequence
*/
void Send_stop(void){
TWCR = ((1<<TWEN)+(1<<TWINT)+(1<<TWSTO));
}
/*!
* Hier wird der eigentliche TWI-Treiber angesprochen
* @param *data_pack Container mit den Daten fuer den Treiber
* @return Resultat der Aktion
*/
uint8 Send_to_TWI(tx_type *data_pack){
uint8 state,i,j;
state = SUCCESS;
for(i=0;(data_pack[i].slave_adr != OWN_ADR)&&(state == SUCCESS);i++) {
state = Send_start();
if (state == SUCCESS)
state = Send_adr(data_pack[i].slave_adr);
/*!
* Abhaengig von W/R senden oder empfangen
*/
if(!(data_pack[i].slave_adr & R)) {
if (state == SUCCESS){
/*!
* Wenn W bis alle Daten gesendet sind
*/
for(j=0;((j<data_pack[i].size)&&(state == SUCCESS));j++)
state = Send_byte(data_pack[i].data_ptr[j]);
}
}
else{
if (state == MRX_ADR_NACK) {
state = Send_start();
}
if (state == SUCCESS){
/*!
* Wenn R bis alle Daten empfangen sind
*/
for(j=0;((j<data_pack[i].size)&&(state == SUCCESS));j++){
/*!
* Wenn wir keine Daten mehr erwarten NACK senden
*/
if(j == data_pack[i].size-1)
state = Get_byte(data_pack[i].data_ptr++,0);
else
state = Get_byte(data_pack[i].data_ptr++,1);
}
}
}
Send_stop();
}
Close_TWI();
return state;
}
/*!
* Sende ein Byte
* @param data das zu uebertragende Byte
*/
uint8 Send_byte(uint8 data){
Wait_TWI_int();
TWDR = data;
TWCR = ((1<<TWINT)+(1<<TWEN));
Wait_TWI_int();
if(TWSR != MTX_DATA_ACK)
return TWSR;
return SUCCESS;
}
/*!
* Sende Slave Adresse
* @param adr die gewuenschte Adresse
* @return Resultat der Aktion
*/
uint8 Send_adr(uint8 adr){
Wait_TWI_int();
TWDR = adr;
TWCR = ((1<<TWINT)+(1<<TWEN));
Wait_TWI_int();
if((TWSR != MTX_ADR_ACK)&&(TWSR != MRX_ADR_ACK))
return TWSR;
return SUCCESS;
}
/*!
* Empfange ein Byte
* @param *rx_ptr Container fuer die Daten
* @param last_byte Flag ob noch Daten erwartet werden
* @return Resultat der Aktion
*/
uint8 Get_byte(uint8 *rx_ptr,uint8 last_byte){
Wait_TWI_int();
if(last_byte)
TWCR = ((1<<TWINT)+(1<<TWEA)+(1<<TWEN));
else
TWCR = ((1<<TWINT)+(1<<TWEN));
Wait_TWI_int();
*rx_ptr = TWDR;
if(((TWSR == MRX_DATA_NACK)&&(last_byte == 0))||(TWSR == MRX_DATA_ACK))
return SUCCESS;
return TWSR;
}
#endif #endif

95
source/Concept/Framework/atmega128io.h
View file

@ -6,8 +6,6 @@
#include <avr/io.h> #include <avr/io.h>
#include <avr/interrupt.h> #include <avr/interrupt.h>
#include "defines.h"
/************************************************************************ /************************************************************************
Title: Interrupt UART library with receive/transmit circular buffers Title: Interrupt UART library with receive/transmit circular buffers
Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury
@ -182,97 +180,4 @@ extern void uart1_puts_p(const char *s );
/**@}*/ /**@}*/
/////////////////////////////////////////////////////
/////////////////////////////////////////////////////
// TWI-Driver
/*!
Struktur Definition
tx_type ist eine Datenstruktur um den TWI Treiber anzusprechen
und behinhaltet folgende Informationen:
Slave Adresse + Datenrichtung
Anzahl der zu uebertragendenden Bytes (Senden oder Empfangen)
Pointer auf den Sende- oder Empfangspuffer
*/
typedef struct
{
uint8 slave_adr; /*!< Slave Adresse and W/R byte */
uint8 size; /*!< Anzahl der Bytes, die gesendet oder empfagen werden sollen */
uint8 *data_ptr; /*!< Pointer zum Sende und Empfangs Puffer */
}tx_type;
/*!
* Hier wird der eigentliche TWI-Treiber angesprochen
* @param *data_pack Container mit den Daten fuer den Treiber
* @return Resultat der Aktion
*/
extern uint8 Send_to_TWI(tx_type *data_pack);
/*!
* Sende ein Byte
* @param data das zu uebertragende Byte
*/
extern uint8 Send_byte(uint8 data);
/*!
* Empfange ein Byte
* @param *rx_ptr Container f<EFBFBD>r die Daten
* @param last_byte Flag ob noch Daten erwartet werden
* @return Resultat der Aktion
*/
extern uint8 Get_byte(uint8 *rx_ptr, uint8 last_byte);
/*!
* Sende Start Sequence
* @return Resultat der Aktion
*/
extern uint8 Send_start(void);
/*!
* Sende Slave Adresse
* @param adr die gewuenschte Adresse
* @return Resultat der Aktion
*/
extern uint8 Send_adr(uint8 adr);
/*!
* Sende Stop Sequence
*/
extern void Send_stop(void);
/*!
* Warte auf TWI interrupt
*/
extern void Wait_TWI_int(void);
/*!
* TWI Bus initialsieren
* @return Resultat der Aktion
*/
extern int8 Init_TWI(void);
/*!
* TWI Bus schliesen
* @return Resultat der Aktion
*/
extern int8 Close_TWI(void);
#define W 0 /*!< Daten Transfer Richtung Schreiben */
#define R 1 /*!< Daten Transfer Richtung Lesen */
#define OWN_ADR 60 /*!< Die eigene Slave Adresse */
#define SUCCESS 0xFF /*!< Status Code alles OK */
/*!
TWI Stautus Register Definitionen
*/
/*!< Genereller Master Statuscode */
#define START 0x08 /*!< START wurde uebertragen */
#define REP_START 0x10 /*!< Wiederholter START wurde uebertragen */
/*!< Master Sender Statuscode */
#define MTX_ADR_ACK 0x18 /*!< SLA+W wurde uebertragen und ACK empfangen */
#define MTX_ADR_NACK 0x20 /*!< SLA+W wurde uebertragen und NACK empfangen */
#define MTX_DATA_ACK 0x28 /*!< Datenbyte wurde uebertragen und ACK empfangen */
#define MTX_DATA_NACK 0x30 /*!< Datenbyte wurde uebertragen und NACK empfangen */
#define MTX_ARB_LOST 0x38 /*!< Schlichtung verloren in SLA+W oder Datenbytes */
/*!< Master Empfaenger Statuscode */
#define MRX_ARB_LOST 0x38 /*!< Schlichtung verloren in SLA+R oder NACK bit */
#define MRX_ADR_ACK 0x40 /*!< SLA+R wurde uebertragen und ACK empfangen */
#define MRX_ADR_NACK 0x48 /*!< SLA+R wurde uebertragen und NACK empfangen */
#define MRX_DATA_ACK 0x50 /*!< Datenbyte wurde empfangen und ACK gesendet */
#define MRX_DATA_NACK 0x58 /*!< Datenbyte wurde empfangen und NACK gesendet */
#endif #endif

10
source/Concept/Framework/default/Makefile
View file

@ -34,13 +34,13 @@ HEX_EEPROM_FLAGS += --change-section-lma .eeprom=0
## Include Directories ## Include Directories
INCLUDES = -I"Z:\rc2007-soccer\source\Concept\Framework\modules" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\executor" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\input" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\interpreter" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\logic" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\output" INCLUDES = -I"Y:\Concept\Framework\modules" -I"Y:\Concept\Framework\modules\executor" -I"Y:\Concept\Framework\modules\input" -I"Y:\Concept\Framework\modules\interpreter" -I"Y:\Concept\Framework\modules\logic" -I"Y:\Concept\Framework\modules\output"
## Libraries ## Libraries
LIBS = -lm LIBS = -lm
## Objects that must be built in order to link ## Objects that must be built in order to link
OBJECTS = io_module.o atmega128io.o main.o robot.o tools.o distance_sensor.o ir_sensor.o keyboard.o mouse_sensor.o sensor.o position_tracker.o display.o dribbler.o engine.o kicker.o led.o ball_tracker.o navigator.o aktuator.o wireless.o logic.o command_handler.o obstacle_tracker.o OBJECTS = io_module.o atmega128io.o main.o robot.o tools.o distance_sensor.o ir_sensor.o keyboard.o mouse_sensor.o sensor.o position_tracker.o display.o dribbler.o engine.o kicker.o led.o ball_tracker.o navigator.o aktuator.o wireless.o logic.o
## Objects explicitly added by the user ## Objects explicitly added by the user
LINKONLYOBJECTS = LINKONLYOBJECTS =
@ -112,12 +112,6 @@ wireless.o: ../modules/wireless.c
logic.o: ../modules/logic/logic.c logic.o: ../modules/logic/logic.c
$(CC) $(INCLUDES) $(CFLAGS) -c $< $(CC) $(INCLUDES) $(CFLAGS) -c $<
command_handler.o: ../modules/interpreter/command_handler.c
$(CC) $(INCLUDES) $(CFLAGS) -c $<
obstacle_tracker.o: ../modules/interpreter/obstacle_tracker.c
$(CC) $(INCLUDES) $(CFLAGS) -c $<
##Link ##Link
$(TARGET): $(OBJECTS) $(TARGET): $(OBJECTS)
$(CC) $(LDFLAGS) $(OBJECTS) $(LINKONLYOBJECTS) $(LIBDIRS) $(LIBS) -o $(TARGET) $(CC) $(LDFLAGS) $(OBJECTS) $(LINKONLYOBJECTS) $(LIBDIRS) $(LIBS) -o $(TARGET)

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

@ -60,49 +60,11 @@
#define DISTANCE_PER_VALUE 1 #define DISTANCE_PER_VALUE 1
#define TICKS_PER_CM 205.0f #define TICKS_PER_CM 205.0f
//#define TICKS_PER_CM 90.0f //#define TICKS_PER_CM 90.0f
#define ULTRASONIC_PER_CM 2.0f
#define WIRELESS_ACTIVE false
#define PI 3.14159265358979323846f #define PI 3.14159265358979323846f
#define CYCLES_PER_CORRECTION 20 #define CYCLES_PER_CORRECTION 20
#define EMPTY_FLOAT 81188.1484f #define EMPTY_FLOAT 81188.1484f
#define BALL_HELD_INTENSITY 900
#define KEEPER_LEFT_ANGLE 20.0f * PI / 180.0f
#define KEEPER_RIGHT_ANGLE (2 * PI) - (20.0f * PI / 180.0f)
#define COMMAND_BUFFER_SIZE 20
#define DEFAULT_SPEED 200
#define DEFAULT_ROTATION_SPEED 200
#define AREA_BOUNDS_X 20000.0f
#define AREA_BOUNDS_Y 5000.0f
#define WALL_DISTANCE_TOLERANCE 30.0f
#define ENEMY_ROBOT_RADIUS 40.0f
#define HOME_LOC_X -500.0f
#define HOME_LOC_Y 0
#define ENEMY_LOC_X 500.0f
#define ENEMY_LOC_Y 0
#define DEFENCE_L_LOC_X -500.0f
#define DEFENCE_L_LOC_Y 70.0f
#define DEFENCE_R_LOC_X -500.0f
#define DEFENCE_R_LOC_Y -70.0f
#define DISTANCE_TOLERANCE 1.0f
#define ORIENTATION_TOLERANCE 0.1f
#define MAX_OBSTACLE_COUNT 20
#define OBSTACLE_DECAY 100
#define BALL_DIRECTION_FRONT_L 45.0f * PI / 180.0f
#define BALL_DIRECTION_FRONT_R 315.0f * PI / 180.0f
#define WIRELESS_CODE "SPASS!" #define WIRELESS_CODE "SPASS!"
enum WirelessCommands enum WirelessCommands
{ {
@ -201,33 +163,17 @@ enum IOModuleNames
IO_KEYBOARD_END, IO_KEYBOARD_END,
//Command Handler
IO_COMMAND_HANDLER_START = IO_KEYBOARD_END,
IO_COMMAND_HANDLER_MAIN = IO_COMMAND_HANDLER_START,
IO_COMMAND_HANDLER_END,
//Position Tracker //Position Tracker
IO_POSITION_TRACKER_START = IO_COMMAND_HANDLER_END, IO_POSITION_TRACKER_START = IO_KEYBOARD_END,
IO_POSITION_TRACKER_MAIN, IO_POSITION_TRACKER_MAIN,
IO_POSITION_TRACKER_END, IO_POSITION_TRACKER_END,
//Obstacle Tracker //Position Tracker
IO_OBSTACLE_TRACKER_START = IO_POSITION_TRACKER_END, IO_BALL_TRACKER_START = IO_POSITION_TRACKER_END,
IO_OBSTACLE_TRACKER_MAIN,
IO_OBSTACLE_TRACKER_END,
//Ball Tracker
IO_BALL_TRACKER_START = IO_OBSTACLE_TRACKER_END,
IO_BALL_TRACKER_MAIN, IO_BALL_TRACKER_MAIN,

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

@ -122,22 +122,6 @@ int main()
newKeyboard = NULL; newKeyboard = NULL;
} }
//Init Command Handler
for(uint8 i = IO_COMMAND_HANDLER_START; i < IO_COMMAND_HANDLER_END; i++)
{
Command_Handler* newCommandHandler = new Command_Handler(i);
localRobot->AddModule(newCommandHandler);
newCommandHandler = NULL;
}
//Init Obstacle Tracker
for(uint8 i = IO_OBSTACLE_TRACKER_START; i < IO_OBSTACLE_TRACKER_END; i++)
{
Obstacle_Tracker* newObstacleTracker = new Obstacle_Tracker(i);
localRobot->AddModule(newObstacleTracker);
newObstacleTracker = NULL;
}
//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++)
{ {
@ -175,8 +159,6 @@ int main()
//Debug code //Debug code
Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN); Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN);
ourDisplay->Clear(); ourDisplay->Clear();
ourDisplay->Print("Roboter fertig gestartet",1,1);
sleep(1);
IR_Sensor* ourSensor = NULL; IR_Sensor* ourSensor = NULL;
@ -184,68 +166,47 @@ int main()
uint16 value = 0; uint16 value = 0;
int8 value2 = 0; int8 value2 = 0;
Command_Handler* ourCommandHandler = localRobot->GetModule<Command_Handler>(IO_COMMAND_HANDLER_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);
Position_Tracker* ourPositionTracker = localRobot->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN); Position_Tracker* ourPosition_Tracker = localRobot->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
Ball_Tracker* ourBallTracker = localRobot->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN); Ball_Tracker* ourBallTracker = localRobot->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
Dribbler* ourDribbler = localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN); Dribbler* ourDribbler = localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN);
//ourDribbler->SetSpeed(1); ourDribbler->SetSpeed(1);
//ourDribbler->SetEnabled(true); ourDribbler->SetEnabled(true);
Aktuator* ourAktuator = localRobot->GetModule<Aktuator>(IO_AKTUATOR_MAIN); Aktuator* ourAktuator = localRobot->GetModule<Aktuator>(IO_AKTUATOR_MAIN);
Logic* ourLogic = localRobot->GetModule<Logic>(IO_LOGIC_MAIN);
Wireless* ourWireless = localRobot->GetModule<Wireless>(IO_WIRELESS_MAIN);
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetSpeed(200); float rotation = 0;
float speed = 200;
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(200); //Mouse_Sensor* mouse_left = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true); //Mouse_Sensor* mouse_right = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
//sleep(2);
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetSpeed(200);
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetEnabled(true);
//sleep(2);
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(200);
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);
/*(localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetSpeed(200);
(localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetEnabled(true);*/
/*(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetSpeed(80);
(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetEnabled(true);*/
(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetSpeed(255);
(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetEnabled(true);
(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(255);
(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);
sleep(2);
//Run //Run
while(true) while(true)
{ {
i++; ourDisplay->Print(i++,1,1);
/*if(!(i % 20)) msleep(100);
if(!(i % 1))
{ {
ourDisplay->Clear(); ourDisplay->Clear();
msleep(10); //ourDisplay->Print(ourPosition_Tracker->GetPositionX(),1,2);
ourCommandHandler->PrintCommand(); //ourDisplay->Print(ourPosition_Tracker->GetPositionY(),1,3);
//ourDisplay->Print(ourPosition_Tracker->GetOrientation() * 180.0f / PI,1,4);
} }
localRobot->Update(); if(!(i % 50))
if(ourCommandHandler->displayDistanceSensors && !(i % 20))
{ {
ourAktuator->Kick();
}
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG); distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
value = distanceSensor->GetDistance(); value = distanceSensor->GetDistance();
ourDisplay->Print(value, 1, 4); ourDisplay->Print(value, 1, 2);
ourDisplay->Print(";"); ourDisplay->Print(";");
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_90_DEG); distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_90_DEG);
value = distanceSensor->GetDistance(); value = distanceSensor->GetDistance();
@ -253,50 +214,72 @@ int main()
ourDisplay->Print(";"); ourDisplay->Print(";");
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_180_DEG); distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_180_DEG);
value = distanceSensor->GetDistance(); value = distanceSensor->GetDistance();
ourDisplay->Print(value); ourDisplay->Print(value, 1, 3);
ourDisplay->Print(";"); ourDisplay->Print(";");
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_270_DEG); distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_270_DEG);
value = distanceSensor->GetDistance(); value = distanceSensor->GetDistance();
ourDisplay->Print(value); ourDisplay->Print(value);
ourDisplay->Print(";"); ourDisplay->Print(";");
uint8 someInput = ourKeyboard->GetInput();
//ourDisplay->Print("Ready to accept...", 1, 2);
switch(someInput)
{
case 0:
ourNavigator->Stop();
return 0;
break;
case 1:
ourNavigator->Drive(225, rotation, speed, rotation);
break;
case 2:
ourNavigator->Drive(180, rotation, speed, rotation);
break;
case 3:
ourNavigator->Drive(135, rotation, speed, rotation);
break;
case 4:
ourNavigator->Drive(270, rotation, speed, rotation);
break;
case 5:
ourNavigator->Drive(0, rotation, 0, rotation);
break;
case 6:
ourNavigator->Drive(90, rotation, speed, rotation);
break;
case 7:
ourNavigator->Drive(315, rotation, speed, rotation);
break;
case 8:
ourNavigator->Drive(0, rotation, speed, rotation);
break;
case 9:
ourNavigator->Drive(45, rotation, speed, rotation);
break;
case 10:
ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker
break;
case 12:
ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker
break;
} }
if(ourCommandHandler->ticksPerCmOffset && !(i % 20)) //ourDisplay->Clear();
{ /*ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);
ourDisplay->Print(ourCommandHandler->ticksPerCmOffset * 10.0f, 10, 1);
}
if(ourCommandHandler->displayPositionTracker && !(i % 20))
{
ourDisplay->Print(ourPositionTracker->GetPositionX(), 1, 2);
ourDisplay->Print(ourPositionTracker->GetPositionY(), 1, 3);
ourDisplay->Print(ourPositionTracker->GetOrientation(), 1, 4);
}
if(ourCommandHandler->displayPositionTracker && !(i % 20))
{
ourDisplay->Print(ourPositionTracker->GetPositionX(), 1, 2);
ourDisplay->Print(ourPositionTracker->GetPositionY(), 1, 3);
ourDisplay->Print(ourPositionTracker->GetOrientation() * 180.0f / PI, 1, 4);
}
if(ourCommandHandler->displayBallTracker && !(i % 20))
{
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);
ourDisplay->Print(";"); ourDisplay->Print(";");
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_30_DEG); ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_30_DEG);
value = ourSensor->GetIRIntensity(); value = ourSensor->GetIRIntensity();
ourDisplay->Print(value, 6, 2); ourDisplay->Print(value);
ourDisplay->Print(";"); ourDisplay->Print(";");
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_60_DEG); ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_60_DEG);
value = ourSensor->GetIRIntensity(); value = ourSensor->GetIRIntensity();
ourDisplay->Print(value, 11, 2); ourDisplay->Print(value);
ourDisplay->Print(";"); ourDisplay->Print(";");
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_100_DEG); ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_100_DEG);
value = ourSensor->GetIRIntensity(); value = ourSensor->GetIRIntensity();
ourDisplay->Print(value, 16, 2); ourDisplay->Print(value);
ourDisplay->Print(";"); ourDisplay->Print(";");
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_180_DEG); ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_180_DEG);
value = ourSensor->GetIRIntensity(); value = ourSensor->GetIRIntensity();
@ -304,43 +287,22 @@ int main()
ourDisplay->Print(";"); ourDisplay->Print(";");
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_260_DEG); ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_260_DEG);
value = ourSensor->GetIRIntensity(); value = ourSensor->GetIRIntensity();
ourDisplay->Print(value, 6, 3); ourDisplay->Print(value);
ourDisplay->Print(";"); ourDisplay->Print(";");
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_300_DEG); ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_300_DEG);
value = ourSensor->GetIRIntensity(); value = ourSensor->GetIRIntensity();
ourDisplay->Print(value, 11, 3); ourDisplay->Print(value);
ourDisplay->Print(";"); ourDisplay->Print(";");
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, 16, 3); ourDisplay->Print(value);
ourDisplay->Print(";"); ourDisplay->Print(";");*/
if(ourBallTracker->KnowsBallDirection())
{
ourDisplay->Print(ourBallTracker->GetBallDirection() * 180.0f / PI, 1, 4);
}
else
{
ourDisplay->Print("Ball not found", 1, 4);
}
if(ourBallTracker->HasBall()) { localRobot->Update();
ourDisplay->Print("Has Ball", 5, 4);
}
}
//if(!(i % 20)) //ourDisplay->Print(ourBallTracker->GetBallDirection() * 180.0f / PI, 1, 4);
//{
// ourDisplay->Print(ourWireless->Recieve(), 10, 1);
//}
if(ourBallTracker->KnowsBallDirection()) //ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0);
{
ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0);
}
else
{
ourNavigator->Stop();
}*/
} }
//Cleanup //Cleanup

11
source/Concept/Framework/modules/executor/aktuator.c
View file

@ -1,11 +0,0 @@
#include "aktuator.h"
//-----------------------------------------------------------------------------
void Aktuator::Kick()
{
(parent->GetModule<Kicker>(IO_KICKER_MAIN))->SetEnabled(true);//aktivate kicker
(parent->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetSpeed(-1);//aktivate dribbler reverse
msleep(10);//wait 100us
(parent->GetModule<Kicker>(IO_KICKER_MAIN))->SetEnabled(false);//deaktivate kicker
(parent->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetSpeed(1);//deaktivate dribbler reverse
}

27
source/Concept/Framework/modules/executor/aktuator.h
View file

@ -1,27 +0,0 @@
#ifndef _AKTUATOR_H
#define _AKTUATOR_H
#include "../../stdafx.h"
class Aktuator : public IO_Module
{
public:
Aktuator()
{
this->parent = NULL;
this->moduleId = 0;
}
Aktuator(uint32 aktuatorId)
{
this->parent = NULL;
this->moduleId = aktuatorId;
}
protected:
public:
void Kick();
};
#endif

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

@ -59,14 +59,14 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed) void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed)
{ {
if(newX == EMPTY_FLOAT || newY == EMPTY_FLOAT) return; if(newX < 0 || newY < 0) return;
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN); Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
this->rotationSpeed = min(rotationSpeed, 255.0f); this->rotationSpeed = min(rotationSpeed, 255.0f);
this->targetX = newX; this->targetX = newX;
this->targetY = newY; this->targetY = newY;
this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY); this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);;
this->targetAngle = newAngle * PI / 180.0f; this->targetAngle = newAngle * PI / 180.0f;
this->robotSpeed = newSpeed; this->robotSpeed = newSpeed;
@ -89,93 +89,39 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
} }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void Navigator::RotateTo(float newAngle, float rotationSpeed) void Navigator::Update()
{
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
this->targetX = EMPTY_FLOAT;
this->targetY = EMPTY_FLOAT;
this->direction = EMPTY_FLOAT;
this->rotationSpeed = min(rotationSpeed, 255.0f);
this->targetAngle = newAngle * PI / 180.0f;
this->robotSpeed = 0;
if(targetAngle - locationeer->GetOrientation() > PI)
{
if(rotationSpeed > 0)
{
rotationSpeed = -rotationSpeed;
}
}
else
{
if(rotationSpeed < 0)
{
rotationSpeed = -rotationSpeed;
}
}
CalculateDirection();
}
//-----------------------------------------------------------------------------
bool Navigator::TargetReached()
{ {
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN); Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
bool targetReached = false; bool targetReached = false;
bool targetAngleReached = false;
if(!HasTarget() || (distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < DISTANCE_TOLERANCE)) if(HasTarget() && distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
{ {
targetX = EMPTY_FLOAT;
targetY = EMPTY_FLOAT;
direction = EMPTY_FLOAT;
robotSpeed = 0;
targetReached = true; targetReached = true;
} }
return targetReached; if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
}
//-----------------------------------------------------------------------------
bool Navigator::AngleReached()
{ {
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN); targetAngle = EMPTY_FLOAT;
rotationSpeed = 0;
bool targetAngleReached = false;
if(!HasTargetAngle() || (fabs(targetAngle - locationeer->GetOrientation()) < 0.1f))
{
targetAngleReached = true; targetAngleReached = true;
} }
return targetAngleReached; if(targetReached && targetAngleReached)
}
//-----------------------------------------------------------------------------
void Navigator::Update()
{
if(this->direction == EMPTY_FLOAT || (HasTarget() && TargetReached()))
{
if(this->rotationSpeed == 0 || (HasTargetAngle() && AngleReached()))
{ {
Stop(); Stop();
} }
else if(!AngleReached()) else if(targetReached || targetAngleReached)
{ {
RotateTo(this->targetAngle, this->rotationSpeed);
}
else
{
Rotate(this->rotationSpeed);
}
}
else
{
if(this->rotationSpeed == 0 || (HasTargetAngle() && AngleReached()))
{
this->rotationSpeed = 0;
this->targetAngle = EMPTY_FLOAT;
CalculateDirection(); CalculateDirection();
} }
}
if(!(correctionCountdown--)) if(!(correctionCountdown--))
{ {
@ -230,10 +176,12 @@ void Navigator::CalculateEngines()
vBack = ((float)vBack * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed; vBack = ((float)vBack * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
vRight = ((float)vRight * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed; vRight = ((float)vRight * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vBack,10,2); //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vLeft,10,2);
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vBack,10,3); //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vBack,10,3);
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vRight,10,4); //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vRight,10,4);
// Transfer the values to the engines // Transfer the values to the engines
Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT); Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
curEngine->SetSpeed(vLeft); curEngine->SetSpeed(vLeft);

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

@ -49,7 +49,6 @@ public:
void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed); void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed);
void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed); void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed);
void RotateTo(float newAngle,float roationSpeed);
void Rotate(float rotationSpeed); void Rotate(float rotationSpeed);
@ -66,34 +65,6 @@ public:
{ {
return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT); return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);
} }
float GetTargetX()
{
return targetX;
}
float GetTargetY()
{
return targetY;
}
float GetDirection()
{
return direction;
}
bool HasTargetAngle()
{
return (targetAngle != EMPTY_FLOAT);
}
bool TargetReached();
bool AngleReached();
bool IsMoving()
{
return (direction != EMPTY_FLOAT || rotationSpeed != 0);
}
}; };
#endif #endif

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

@ -1,159 +1,41 @@
#include "distance_sensor.h" #include "distance_sensor.h"
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void Distance_Sensor::SetSignalFactor(uint8 factor) float Distance_Sensor::GetDistance()
{ {
if(factor > 16) uint32 result = 0;
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("Gen Pulse; Pin:",1,4);
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print((int)(*hardwarePin & pin));
msleep(500);
//Generate pulse
*hardwareDDR |= pin;//Set pin output
*hardwarePort |= pin;//Activate port
usleep(10);//Wait for 10ľs
*hardwareDDR &= ~pin;//Set pin input
*hardwarePort &= ~pin;//Deactivate port
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("Wait response; Pin:",1,4);
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print((int)(*hardwarePin & pin));
uint16 i;
//Wait for response
for(i = 0; (!(*hardwarePin & pin)) && (i < 1000); i++) { asm volatile("nop"); }
//Calculate duration of response
while((*hardwarePin & pin)&&(result < 300000))
{ {
factor = 16; result++;
asm volatile("nop");
asm volatile("nop");
asm volatile("nop");
asm volatile("nop");
asm volatile("nop");
asm volatile("nop");
asm volatile("nop");
} }
uint8 temp[2]; return (float(result) * DISTANCE_PER_VALUE);
uint8 state;
tx_type tx_frame[2];
state = SUCCESS;
tx_frame[0].slave_adr = this->slaveAddr + W;
tx_frame[0].size = 2;
tx_frame[0].data_ptr = temp;
tx_frame[0].data_ptr[0] = 1;
tx_frame[0].data_ptr[1] = factor;
tx_frame[1].slave_adr = OWN_ADR;
state = Send_to_TWI(tx_frame);
}
//-----------------------------------------------------------------------------
void Distance_Sensor::SetSlaveAddress(uint8 newSlaveAddress)
{
uint8 temp[2];
uint8 state;
tx_type tx_frame[2];
state = SUCCESS;
tx_frame[0].slave_adr = this->slaveAddr + W;
tx_frame[0].size = 2;
tx_frame[0].data_ptr = temp;
tx_frame[0].data_ptr[0] = 0;
tx_frame[0].data_ptr[1] = 160;
tx_frame[1].slave_adr = OWN_ADR;
state = Send_to_TWI(tx_frame);
msleep(60);
tx_frame[0].data_ptr[1] = 170;
state = Send_to_TWI(tx_frame);
msleep(60);
tx_frame[0].data_ptr[1] = 165;
state = Send_to_TWI(tx_frame);
msleep(60);
tx_frame[0].data_ptr[1] = newSlaveAddress;
state = Send_to_TWI(tx_frame);
}
//-----------------------------------------------------------------------------
void Distance_Sensor::SetRange(unsigned int millimeters){
uint8 temp[2];
uint8 state;
tx_type tx_frame[2];
state = SUCCESS;
millimeters = (millimeters/43);
tx_frame[0].slave_adr = this->slaveAddr+W;
tx_frame[0].size = 2;
tx_frame[0].data_ptr = temp;
tx_frame[0].data_ptr[0] = 2;
tx_frame[0].data_ptr[1] = millimeters;
tx_frame[1].slave_adr = OWN_ADR;
state = Send_to_TWI(tx_frame);
}
/*!
* Messung ausloesen
* @param metric_unit 0x50 in Zoll, 0x51 in cm, 0x52 ms
* @return Resultat der Aktion
*/
uint8 Distance_Sensor::srf10_ping(uint8 metric_unit){
uint8 temp[2];
uint8 state;
tx_type tx_frame[2];
state = SUCCESS;
tx_frame[0].slave_adr = this->slaveAddr+W;
tx_frame[0].size = 2;
tx_frame[0].data_ptr = temp;
tx_frame[0].data_ptr[0] = SRF10_COMMAND;
tx_frame[0].data_ptr[1] = metric_unit;
tx_frame[1].slave_adr = OWN_ADR;
state = Send_to_TWI(tx_frame);
return state;
}
//-----------------------------------------------------------------------------
uint8 Distance_Sensor::ReadRegister(uint8 registerToRead){
uint8 temp;
uint8 value;
uint8 state;
tx_type tx_frame[3];
state = SUCCESS;
value = 0;
tx_frame[0].slave_adr = this->slaveAddr+W;
tx_frame[0].size = 1;
tx_frame[0].data_ptr = &temp;
tx_frame[0].data_ptr[0] = registerToRead;
tx_frame[1].slave_adr = this->slaveAddr+R;
tx_frame[1].size = 1;
tx_frame[1].data_ptr = &value;
tx_frame[2].slave_adr = OWN_ADR;
state = Send_to_TWI(tx_frame);
return value;
}
/*!
* Messung starten Ergebniss aufbereiten und zurueckgeben
* @return Messergebniss
*/
uint16 Distance_Sensor::srf10_get_measure(){
char hib;
char lob;
char state;
state = SUCCESS;
state = srf10_ping(SRF10_CENTIMETERS);
msleep(10); //Optimierungs Potential
lob=ReadRegister(SRF10_LOB);
msleep(10); //Optimierungs Potential
hib=ReadRegister(SRF10_HIB);
return (hib*256)+lob;
}
//-----------------------------------------------------------------------------
uint16 Distance_Sensor::GetDistance()
{
return srf10_get_measure();
} }

80
source/Concept/Framework/modules/input/distance_sensor.h
View file

@ -4,20 +4,6 @@
#include "../../stdafx.h" #include "../../stdafx.h"
#include "sensor.h" #include "sensor.h"
#define SRF10_MIN_GAIN 0 /*!< setze Verstaerung auf 40 */
#define SRF10_MAX_GAIN 16 /*!< setze Verstaerung auf 700 */
#define SRF10_MIN_RANGE 0 /*!< Minimale Reichweite 43mm */
#define SRF10_MAX_RANGE 5977 /*!< Maximale Reichweite 5977mm */
#define SRF10_INCHES 0x50 /*!< Messung in INCHES */
#define SRF10_CENTIMETERS 0x51 /*!< Messung in CM */
#define SRF10_MICROSECONDS 0x52 /*!< Messung in Millisekunden */
#define SRF10_COMMAND 0 /*!< W=Befehls-Register R=Firmware*/
#define SRF10_LIGHT 1 /*!< W=Verstaerkungsfaktor R=Nicht benutzt */
#define SRF10_HIB 2 /*!< W=Reichweite R=Messung High-Byte */
#define SRF10_LOB 3 /*!< W=Nicht benutzt R=Messung Low-Byte */
class Distance_Sensor : public Sensor class Distance_Sensor : public Sensor
{ {
public: public:
@ -25,7 +11,10 @@ public:
{ {
this->parent = NULL; this->parent = NULL;
this->moduleId = 0; this->moduleId = 0;
this->slaveAddr = 0; // Set the I2C Slave-Adress this->hardwarePort = NULL;
this->hardwareDDR = NULL;
this->hardwarePin = NULL;
this->pin = 0;
} }
Distance_Sensor(uint32 sensorId) Distance_Sensor(uint32 sensorId)
@ -36,58 +25,47 @@ public:
switch(sensorId) switch(sensorId)
{ {
case IO_SENSOR_DISTANCE_0_DEG: case IO_SENSOR_DISTANCE_0_DEG:
this->slaveAddr = 0xE0; // Set the I2C Slave-Adress this->hardwarePort = &PORTC;
this->hardwareDDR = &DDRC;
this->hardwarePin = &PINC;
this->pin = (1 << 0);
break; break;
case IO_SENSOR_DISTANCE_90_DEG: case IO_SENSOR_DISTANCE_90_DEG:
this->slaveAddr = 0xE2; this->hardwarePort = &PORTC;
this->hardwareDDR = &DDRC;
this->hardwarePin = &PINC;
this->pin = (1 << 1);
break; break;
case IO_SENSOR_DISTANCE_180_DEG: case IO_SENSOR_DISTANCE_180_DEG:
this->slaveAddr = 0xE4; this->hardwarePort = &PORTC;
this->hardwareDDR = &DDRC;
this->hardwarePin = &PINC;
this->pin = (1 << 2);
break; break;
case IO_SENSOR_DISTANCE_270_DEG: case IO_SENSOR_DISTANCE_270_DEG:
this->slaveAddr = 0xE6; this->hardwarePort = &PORTC;
this->hardwareDDR = &DDRC;
this->hardwarePin = &PINC;
this->pin = (1 << 3);
break; break;
default: default:
this->slaveAddr = 0; this->hardwarePort = NULL;
this->hardwareDDR = NULL;
this->hardwarePin = NULL;
this->pin = 0;
break; break;
} }
//SetRange(100);
SetRange(2000);
SetSignalFactor(0x06);
} }
protected: protected:
//Hardware //Hardware
uint8 slaveAddr; volatile uint8* hardwarePort;
volatile uint8* hardwareDDR;
void SetSignalFactor(uint8 factor); volatile uint8* hardwarePin;
uint8 pin;
void SetRange(uint16 millimeters);
/*!
* Messung ausloesen
* @param metric_unit 0x50 in Zoll, 0x51 in cm, 0x52 ms
* @return Resultat der Aktion
*/
uint8 srf10_ping(uint8 metric_unit);
/*!
* Register auslesen
* @param srf10_register welches Register soll ausgelsen werden
* @return Inhalt des Registers
*/
uint8 ReadRegister(uint8 registerToRead);
/*!
* Messung starten Ergebniss aufbereiten und zurueckgeben
* @return Messergebniss
*/
uint16 srf10_get_measure(void);
public: public:
uint16 GetDistance(); float GetDistance();
void SetSlaveAddress(uint8 newSlaveAddress);
}; };
#endif #endif

4
source/Concept/Framework/modules/input/ir_sensor.h
View file

@ -34,11 +34,11 @@ public:
break; break;
case IO_SENSOR_IR_100_DEG: case IO_SENSOR_IR_100_DEG:
this->channel = 3; this->channel = 3;
this->intensityCorrection = 80; this->intensityCorrection = 40;
break; break;
case IO_SENSOR_IR_180_DEG: case IO_SENSOR_IR_180_DEG:
this->channel = 4; this->channel = 4;
this->intensityCorrection = 70; this->intensityCorrection = 50;
break; break;
case IO_SENSOR_IR_260_DEG: case IO_SENSOR_IR_260_DEG:
this->channel = 5; this->channel = 5;

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

@ -214,16 +214,6 @@ public:
{ {
return positionY; return positionY;
} }
void AdjustPositionX(float newPosX)
{
this->positionX = newPosX;
}
void AdjustPositionY(float newPosY)
{
this->positionY = newPosY;
}
}; };
#endif #endif

20
source/Concept/Framework/modules/interpreter/ball_tracker.c
View file

@ -3,8 +3,6 @@
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void Ball_Tracker::Update() void Ball_Tracker::Update()
{ {
Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
uint8 sensorCount = (IO_SENSOR_IR_330_DEG - IO_SENSOR_IR_0_DEG) + 1; uint8 sensorCount = (IO_SENSOR_IR_330_DEG - IO_SENSOR_IR_0_DEG) + 1;
uint16 intensity[sensorCount]; uint16 intensity[sensorCount];
uint8 greatestIntensity = 0; uint8 greatestIntensity = 0;
@ -23,18 +21,6 @@ void Ball_Tracker::Update()
{ {
greatestIntensity = i; greatestIntensity = i;
} }
if(i == 0)
{
if(intensity[i] > BALL_HELD_INTENSITY) // Ball derzeit sehr nah dran
{
if(ballHeldCounter < 10) ballHeldCounter++;
}
else if(ballHeldCounter > 0)
{
ballHeldCounter--;
}
}
} }
if(intensity[greatestIntensity]) if(intensity[greatestIntensity])
@ -131,15 +117,13 @@ void Ball_Tracker::Update()
direction = (intensity[greatestIntensity] * mainDirection + direction = (intensity[greatestIntensity] * mainDirection +
intensity[secondIntensity] * secondDirection) / intensity[secondIntensity] * secondDirection) /
(intensity[greatestIntensity] + intensity[secondIntensity]); (intensity[greatestIntensity] + intensity[secondIntensity]);
direction = easyAngle(direction);
} }
else else
{ {
direction = mainDirection; direction = mainDirection;
} }
direction += ourPositionTracker->GetOrientation();
direction = easyAngle(direction);
} }
else else
{ {

9
source/Concept/Framework/modules/interpreter/ball_tracker.h
View file

@ -11,7 +11,6 @@ public:
this->parent = NULL; this->parent = NULL;
this->moduleId = 0; this->moduleId = 0;
this->direction = EMPTY_FLOAT; this->direction = EMPTY_FLOAT;
this->ballHeldCounter = 0;
} }
Ball_Tracker(uint32 trackerId) Ball_Tracker(uint32 trackerId)
@ -19,12 +18,10 @@ public:
this->parent = NULL; this->parent = NULL;
this->moduleId = trackerId; this->moduleId = trackerId;
this->direction = EMPTY_FLOAT; this->direction = EMPTY_FLOAT;
this->ballHeldCounter = 0;
} }
protected: protected:
float direction; float direction;
uint8 ballHeldCounter;
public: public:
void Update(); void Update();
@ -36,12 +33,12 @@ public:
bool KnowsBallDirection() bool KnowsBallDirection()
{ {
return (direction != EMPTY_FLOAT); return direction != EMPTY_FLOAT;
} }
bool HasBall() bool RobotHasBall()
{ {
return (ballHeldCounter >= 3); //fill me!
} }
}; };

211
source/Concept/Framework/modules/interpreter/command_handler.c
View file

@ -1,211 +0,0 @@
#include "command_handler.h"
//-----------------------------------------------------------------------------
void Command_Handler::Update()
{
Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
Keyboard* ourKeyboard = parent->GetModule<Keyboard>(IO_KEYBOARD_MAIN);
uint8 curInput = ourKeyboard->GetInput();
while(curInput != 0xEE && curInput != 0xFF)
{
if(curInput == 10)
{
ExecuteCommand();
for(uint8 i = 0; i < this->currentCommandLength; i++)
{
this->buffer[i] = NULL;
ourDisplay->Print(" ", i + 1, 1);
}
this->currentCommandLength = 0;
}
else if(curInput == 11)
{
if(this->currentCommandLength > 0)
{
ourDisplay->Print(" ", this->currentCommandLength, 1);
this->currentCommandLength--;
this->buffer[currentCommandLength] = NULL;
}
}
else if(curInput >= 0 && curInput <= 9)
{
if(this->currentCommandLength < COMMAND_BUFFER_SIZE)
{
this->buffer[this->currentCommandLength] = curInput;
this->currentCommandLength++;
ourDisplay->Print(curInput, this->currentCommandLength, 1);
}
}
curInput = ourKeyboard->GetInput();
}
}
//-----------------------------------------------------------------------------
void Command_Handler::ExecuteCommand()
{
if(this->buffer[0] == 1)
{
if(this->buffer[1] == 1)
{
this->displayDistanceSensors = true;
this->displayPositionTracker = false;
this->displayBallTracker = false;
this->displayMouseSensors = false;
}
else if(this->buffer[1] == 2)
{
this->displayDistanceSensors = false;
this->displayPositionTracker = true;
this->displayBallTracker = false;
this->displayMouseSensors = false;
}
else if(this->buffer[1] == 3)
{
this->displayDistanceSensors = false;
this->displayPositionTracker = false;
this->displayBallTracker = true;
this->displayMouseSensors = false;
}
else if(this->buffer[1] == 4)
{
this->displayDistanceSensors = false;
this->displayPositionTracker = false;
this->displayBallTracker = false;
this->displayMouseSensors = true;
}
else if(this->buffer[1] == 0)
{
this->displayDistanceSensors = false;
this->displayPositionTracker = false;
this->displayBallTracker = false;
this->displayMouseSensors = false;
}
}
else if(this->buffer[0] == 2)
{
Logic* ourLogic = parent->GetModule<Logic>(IO_LOGIC_MAIN);
if(this->buffer[1] == 1)
{
ourLogic->SetKeeper(true);
}
else if(this->buffer[1] == 2)
{
ourLogic->SetKeeper(false);
}
}
else if(this->buffer[0] == 3)
{
if(this->buffer[1] == 1)
{
Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
ourNavigator->RotateTo(180, 200);
}
else if(this->buffer[1] == 2)
{
Aktuator* ourAktuator = parent->GetModule<Aktuator>(IO_AKTUATOR_MAIN);
ourAktuator->Kick();
}
else if(this->buffer[1] == 3)
{
Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
ourNavigator->RotateTo(float(buffer[2]) * 100.0f + float(buffer[3]) * 10.0f + float(buffer[4]), 200);
}
else if(this->buffer[1] == 4)
{
Mouse_Sensor* ourLeftMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
Mouse_Sensor* ourRightMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
this->ticksPerCmOffset += 2.5f;
ourLeftMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
ourLeftMouse->AdjustPositionY(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
ourRightMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
ourRightMouse->AdjustPositionY(3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
}
else if(this->buffer[1] == 5)
{
Mouse_Sensor* ourLeftMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
Mouse_Sensor* ourRightMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
this->ticksPerCmOffset -= 2.5f;
ourLeftMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
ourLeftMouse->AdjustPositionY(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
ourRightMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
ourRightMouse->AdjustPositionY(3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
}
}
else if(this->buffer[0] == 4)
{
Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
int16 speed = 200;
switch(this->buffer[1])
{
case 1:
ourNavigator->Drive(225, 0, speed, 0);
break;
case 2:
ourNavigator->Drive(180, 0, speed, 0);
break;
case 3:
ourNavigator->Drive(135, 0, speed, 0);
break;
case 4:
ourNavigator->Drive(270, 0, speed, 0);
break;
case 5:
ourNavigator->Rotate(DEFAULT_ROTATION_SPEED);
break;
case 6:
ourNavigator->Drive(90, 0, speed, 0);
break;
case 7:
ourNavigator->Drive(315, 0, speed, 0);
break;
case 8:
ourNavigator->Drive(0, 0, speed, 0);
break;
case 9:
ourNavigator->Drive(45, 0, speed, 0);
break;
case 0:
ourNavigator->Stop();
break;
}
}
else if(this->buffer[0] == 5)
{
Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
if(this->buffer[1] == 1)
{
ourPositionTracker->SetPosition(float(), ourPositionTracker->GetPositionY(), ourPositionTracker->GetOrientation());
}
else if(this->buffer[1] == 0)
{
ourPositionTracker->SetPosition(0, 0, 0);
}
}
else if(this->buffer[0] == 6)
{
if(this->buffer[1] == 1)
{
Wireless* ourWireless = parent->GetModule<Wireless>(IO_WIRELESS_MAIN);
ourWireless->Send("Heyho!");
}
}
}
//-----------------------------------------------------------------------------
void Command_Handler::PrintCommand()
{
Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
for(uint8 i = 0; i < currentCommandLength; i++)
{
ourDisplay->Print(this->buffer[i], i + 1, 1);
}
}

63
source/Concept/Framework/modules/interpreter/command_handler.h
View file

@ -1,63 +0,0 @@
#ifndef _COMMAND_HANDLER_H
#define _COMMAND_HANDLER_H
#include "../../stdafx.h"
class Command_Handler : public IO_Module
{
public:
Command_Handler()
{
this->parent = NULL;
this->moduleId = 0;
this->currentCommandLength = 0;
this->displayDistanceSensors = false;
this->displayPositionTracker = false;
this->displayBallTracker = false;
this->displayMouseSensors = false;
this->ticksPerCmOffset = 0;
for(uint8 i = 0; i < COMMAND_BUFFER_SIZE; i++)
{
buffer[i] = 0;
}
}
Command_Handler(uint32 commandHandlerId)
{
this->parent = NULL;
this->moduleId = commandHandlerId;
this->currentCommandLength = 0;
this->displayDistanceSensors = false;
this->displayPositionTracker = false;
this->displayBallTracker = false;
this->displayMouseSensors = false;
this->ticksPerCmOffset = 0;
for(uint8 i = 0; i < COMMAND_BUFFER_SIZE; i++)
{
buffer[i] = 0;
}
}
protected:
uint8 currentCommandLength;
uint8 buffer[COMMAND_BUFFER_SIZE];
void ExecuteCommand();
public:
void Update();
void PrintCommand();
//Command variables
bool displayDistanceSensors;
bool displayPositionTracker;
bool displayBallTracker;
bool displayMouseSensors;
float ticksPerCmOffset;
};
#endif

37
source/Concept/Framework/modules/interpreter/position_tracker.c
View file

@ -3,9 +3,6 @@
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void Position_Tracker::Update() void Position_Tracker::Update()
{ {
Command_Handler* ourCommandHandler = parent->GetModule<Command_Handler>(IO_COMMAND_HANDLER_MAIN);
Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
// We want to use the mouse-sensors // We want to use the mouse-sensors
Mouse_Sensor* mouseLeft = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT); Mouse_Sensor* mouseLeft = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
Mouse_Sensor* mouseRight = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT); Mouse_Sensor* mouseRight = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
@ -13,10 +10,6 @@ void Position_Tracker::Update()
// Generate a vector for the left mouse // Generate a vector for the left mouse
int8 leftX = mouseLeft->GetXMovement(); int8 leftX = mouseLeft->GetXMovement();
int8 leftY = mouseLeft->GetYMovement(); int8 leftY = mouseLeft->GetYMovement();
// Generate a vector for the right mouse
int8 rightX = mouseRight->GetXMovement();
int8 rightY = mouseRight->GetYMovement();
float distanceLeft = sqrt(leftX * leftX + leftY * leftY); float distanceLeft = sqrt(leftX * leftX + leftY * leftY);
float angleLeft = easyAngle(atan2(leftY, leftX) + (225.0f * PI / 180.0f)); float angleLeft = easyAngle(atan2(leftY, leftX) + (225.0f * PI / 180.0f));
@ -30,6 +23,9 @@ void Position_Tracker::Update()
movementLeftY = 0; movementLeftY = 0;
} }
// Generate a vector for the right mouse
int8 rightX = mouseRight->GetXMovement();
int8 rightY = mouseRight->GetYMovement();
float distanceRight = sqrt(rightX * rightX + rightY * rightY); float distanceRight = sqrt(rightX * rightX + rightY * rightY);
float angleRight = easyAngle(atan2(rightY, rightX) - (45.0f * PI / 180.0f)); float angleRight = easyAngle(atan2(rightY, rightX) - (45.0f * PI / 180.0f));
@ -43,14 +39,6 @@ void Position_Tracker::Update()
movementRightY = 0; movementRightY = 0;
} }
if(ourCommandHandler->displayMouseSensors)
{
ourDisplay->Print(movementLeftX, 1, 2);
ourDisplay->Print(movementLeftY, 10, 2);
ourDisplay->Print(movementRightX, 1, 3);
ourDisplay->Print(movementRightY, 10, 3);
}
// Generate vector from P:left to P:right // Generate vector from P:left to P:right
float movementDifferenceX = movementRightX - movementLeftX; float movementDifferenceX = movementRightX - movementLeftX;
float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY()); float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY());
@ -58,18 +46,17 @@ void Position_Tracker::Update()
// Calculate the difference of orientation // Calculate the difference of orientation
float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f); float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f);
if(ourCommandHandler->displayMouseSensors)
{
ourDisplay->Print(orientationDiff * 180.0f / PI, 1, 4);
}
float robotMovementX = movementDifferenceX / 2.0f; float robotMovementX = movementDifferenceX / 2.0f;
float robotMovementY = movementDifferenceY / 2.0f; float robotMovementY = movementDifferenceY / 2.0f;
robotMovementX += movementLeftX + mouseLeft->GetPositionX() + (-mouseLeft->GetPositionX() * cos(orientationDiff)); robotMovementX += movementLeftX + mouseLeft->GetPositionX() + (-mouseLeft->GetPositionX() * cos(orientationDiff));
robotMovementY += movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff)); robotMovementY += movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff));
//float robotDistance = sqrt(robotMovementX * robotMovementX + robotMovementY * robotMovementY);
float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotMovementX + sin(this->orientation + (orientationDiff / 2.0f)) * robotMovementY; float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotMovementX - sin(this->orientation + (orientationDiff / 2.0f)) * robotMovementY;
float absoluteDiffY = sin(this->orientation + (orientationDiff / 2.0f) + PI / 2.0f) * robotMovementY - cos(this->orientation + (orientationDiff / 2.0f) - PI / 2.0f) * robotMovementX; float absoluteDiffY = sin(this->orientation + (orientationDiff / 2.0f) + PI / 2.0f) * robotMovementY + cos(this->orientation + (orientationDiff / 2.0f) - PI / 2.0f) * robotMovementX;
//float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementX);
//float absoluteDiffY = sin(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementY);
if(!robotMovementX && !robotMovementY) if(!robotMovementX && !robotMovementY)
{ {
@ -87,10 +74,4 @@ void Position_Tracker::Update()
this->orientation += orientationDiff; this->orientation += orientationDiff;
this->orientation = easyAngle(this->orientation); this->orientation = easyAngle(this->orientation);
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(" ", 5, 1);
/*(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Clear();
(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(this->orientation * 180.0f / PI, 5, 1);
(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(this->positionX, 1, 2);
(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(this->positionY, 1, 3);*/
} }

16
source/Concept/Framework/modules/interpreter/position_tracker.h
View file

@ -40,21 +40,19 @@ public:
} }
// returns x-koordinate in mm // returns x-koordinate in mm
int16 GetPositionX() int GetPositionX() {
{ return (int)((positionX*10.0f)/TICKS_PER_CM);
return (int16)((positionX*10.0f)/TICKS_PER_CM);
} }
// returns y-koordinate in mm // returns y-koordinate in mm
int16 GetPositionY() int GetPositionY() {
{ return (int)((positionY*10.0f)/TICKS_PER_CM);
return (int16)((positionY*10.0f)/TICKS_PER_CM);
} }
// returns orientation // returns orientation
float GetOrientation() float GetOrientation() {
{ return 0.0f; //tmp!!!!!!!!!!
return orientation; //return orientation;
} }
}; };

271
source/Concept/Framework/modules/logic/logic.c
View file

@ -1,271 +0,0 @@
#include "logic.h"
//-----------------------------------------------------------------------------
void Logic::OnBallOwned()
{
bool wasKeeper = this->IsKeeper();
this->SetKeeper(false);
if(!WIRELESS_ACTIVE)
{
Aktuator* ourAktuator = parent->GetModule<Aktuator>(IO_AKTUATOR_MAIN);
ourAktuator->Kick();
this->SetKeeper(wasKeeper);
}
}
//-----------------------------------------------------------------------------
void Logic::OnBallLost()
{
}
//-----------------------------------------------------------------------------
void Logic::OnBecomeKeeper()
{
Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
if(distance2d(HOME_LOC_X, HOME_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) > DISTANCE_TOLERANCE)
{
status = STATUS_KEEPER_RETURN_HOME;
ourNavigator->DriveTo(HOME_LOC_X, HOME_LOC_Y, ourPositionTracker->GetOrientation(), DEFAULT_SPEED, 0);
}
else
{
UpdateKeeperMovement();
}
}
//-----------------------------------------------------------------------------
void Logic::OnBecomeAttacker()
{
Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
ourNavigator->Stop();
UpdateAttackerMovement();
}
//-----------------------------------------------------------------------------
void Logic::Update()
{
// We want to use a navigator
Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
Ball_Tracker* ourBallTracker = parent->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
Obstacle_Tracker* ourObstacleTracker = parent->GetModule<Obstacle_Tracker>(IO_OBSTACLE_TRACKER_MAIN);
if(!this->HasBall() && ourBallTracker->HasBall())
{
this->hasBall = true;
this->OnBallOwned();
}
else if(this->HasBall() && !ourBallTracker->HasBall())
{
this->hasBall = false;
this->OnBallLost();
}
switch(status)
{
case STATUS_ATTACKER_SEARCHING_AT_HOME:
case STATUS_ATTACKER_SEARCHING_AT_ENEMY:
if(ourBallTracker->KnowsBallDirection())
{
ourNavigator->Stop();
}
break;
case STATUS_KEEPER_HUNT_BALL:
if(ourBallTracker->KnowsBallDirection())
{
ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
}
else
{
ourNavigator->Stop();
}
break;
}
if(!ourNavigator->IsMoving())
{
if(this->IsKeeper())
{
UpdateKeeperMovement();
}
else
{
UpdateAttackerMovement();
}
}
if(ourNavigator->IsMoving())
{
if(ourObstacleTracker->IsObstacleOnPath())
{
avoidsObstacle = true;
for(uint8 i = 0; i < 5; i++)
{
if(ourObstacleTracker->IsObstacleOnPath(easyAngle(ourNavigator->GetDirection() - ((i - 2) * PI / 4.0f))))
{
}
ourNavigator->GetDirection();
}
}
}
}
//-----------------------------------------------------------------------------
void Logic::UpdateKeeperMovement()
{
Ball_Tracker* ourBallTracker = parent->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
if(ourBallTracker->KnowsBallDirection())
{
if(ourBallTracker->GetBallDirection() > KEEPER_LEFT_ANGLE &&
ourBallTracker->GetBallDirection() <= PI)
{
if(distance2d(DEFENCE_L_LOC_X, DEFENCE_L_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) < DISTANCE_TOLERANCE)
{
float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());
if(angleDifference > PI)
{
angleDifference = (2 * PI) - angleDifference;
}
if(angleDifference > ORIENTATION_TOLERANCE)
{
status = STATUS_KEEPER_TURN_TO_BALL;
ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
}
else
{
status = STATUS_KEEPER_HUNT_BALL;
ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
}
}
else
{
status = STATUS_KEEPER_DRIVE_TO_DEFEND;
ourNavigator->DriveTo(DEFENCE_L_LOC_X, DEFENCE_L_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
}
}
else if(ourBallTracker->GetBallDirection() < KEEPER_RIGHT_ANGLE &&
ourBallTracker->GetBallDirection() >= PI)
{
if(distance2d(DEFENCE_R_LOC_X, DEFENCE_R_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) < DISTANCE_TOLERANCE)
{
float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());
if(angleDifference > PI)
{
angleDifference = (2 * PI) - angleDifference;
}
if(angleDifference > ORIENTATION_TOLERANCE)
{
status = STATUS_KEEPER_TURN_TO_BALL;
ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
}
else
{
status = STATUS_KEEPER_HUNT_BALL;
ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
}
}
else
{
status = STATUS_KEEPER_DRIVE_TO_DEFEND;
ourNavigator->DriveTo(DEFENCE_R_LOC_X, DEFENCE_R_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
}
}
else
{
if(distance2d(HOME_LOC_X, HOME_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) < DISTANCE_TOLERANCE)
{
float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());
if(angleDifference > PI)
{
angleDifference = (2 * PI) - angleDifference;
}
if(angleDifference > ORIENTATION_TOLERANCE)
{
status = STATUS_KEEPER_TURN_TO_BALL;
ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
}
else
{
status = STATUS_KEEPER_HUNT_BALL;
ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
}
}
else
{
status = STATUS_KEEPER_DRIVE_TO_DEFEND;
ourNavigator->DriveTo(HOME_LOC_X, HOME_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
}
}
}
else
{
if(status == STATUS_KEEPER_TURN_LEFT)
{
status = STATUS_KEEPER_TURN_RIGHT;
ourNavigator->RotateTo(315, DEFAULT_ROTATION_SPEED);
}
else
{
status = STATUS_KEEPER_TURN_LEFT;
ourNavigator->RotateTo(45, DEFAULT_ROTATION_SPEED);
}
}
}
//-----------------------------------------------------------------------------
void Logic::UpdateAttackerMovement()
{
Ball_Tracker* ourBallTracker = parent->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
if(ourBallTracker->KnowsBallDirection())
{
float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());
if(angleDifference > PI)
{
angleDifference = (2 * PI) - angleDifference;
}
if(angleDifference > ORIENTATION_TOLERANCE)
{
status = STATUS_ATTACKER_TURN_TO_BALL;
ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
}
else
{
status = STATUS_ATTACKER_DRIVE_TO_BALL;
ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
}
}
else
{
if(ourPositionTracker->GetPositionX() > 0)
{
status = STATUS_ATTACKER_SEARCHING_AT_HOME;
ourNavigator->DriveTo(HOME_LOC_X, HOME_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
}
else
{
status = STATUS_ATTACKER_SEARCHING_AT_ENEMY;
ourNavigator->DriveTo(ENEMY_LOC_X, ENEMY_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
}
}
}

91
source/Concept/Framework/modules/logic/logic.h
View file

@ -1,91 +0,0 @@
#ifndef _LOGIC_H
#define _LOGIC_H
#include "../../stdafx.h"
class Logic : public IO_Module
{
public:
Logic()
{
this->parent = NULL;
this->moduleId = 0;
this->isKeeper = false;
this->status = 0;
this->hasBall = false;
this->avoidsObstacle = false;
}
Logic(uint32 logicId)
{
this->parent = NULL;
this->moduleId = logicId;
this->isKeeper = false;
this->status = 0;
this->hasBall = false;
this->avoidsObstacle = false;
}
protected:
bool isKeeper;
uint8 status;
bool hasBall;
bool avoidsObstacle;
enum LogicalStatus
{
STATUS_KEEPER_TURN_RIGHT,
STATUS_KEEPER_TURN_LEFT,
STATUS_KEEPER_RETURN_HOME,
STATUS_KEEPER_HUNT_BALL,
STATUS_KEEPER_TURN_TO_BALL,
STATUS_KEEPER_DRIVE_TO_DEFEND,
STATUS_ATTACKER_TURN_TO_BALL,
STATUS_ATTACKER_DRIVE_TO_BALL,
STATUS_ATTACKER_SEARCHING_AT_HOME,
STATUS_ATTACKER_SEARCHING_AT_ENEMY,
};
void OnBallOwned();
void OnBallLost();
void OnBecomeKeeper();
void OnBecomeAttacker();
public:
void Update();
bool IsKeeper()
{
return isKeeper;
}
bool IsAttacker()
{
return !isKeeper;
}
void SetKeeper(bool newStatus)
{
if(!this->isKeeper && newStatus)
{
this->OnBecomeKeeper();
}
else if(this->isKeeper && !newStatus)
{
this->OnBecomeAttacker();
}
this->isKeeper = newStatus;
}
bool HasBall()
{
return this->hasBall;
}
void UpdateKeeperMovement();
void UpdateAttackerMovement();
};
#endif

1
source/Concept/Framework/modules/wireless.c
View file

@ -1 +0,0 @@
#include "wireless.h"

58
source/Concept/Framework/modules/wireless.h
View file

@ -1,58 +0,0 @@
#ifndef _WIRELESS_H
#define _WIRELESS_H
#include "../../stdafx.h"
class Wireless : public IO_Module
{
public:
Wireless()
{
this->parent = NULL;
this->moduleId = 0;
}
Wireless(uint32 wirelessId)
{
this->parent = NULL;
this->moduleId = wirelessId;
switch(wirelessId)
{
case IO_WIRELESS_MAIN:
uart_init(51); // 19200 Baud at 16MHz Atmel
break;
default:
break;
}
}
protected:
uint8 transmitPower;
public:
void SetTransmitPower(uint8 newTransmitPower)
{
this->transmitPower = newTransmitPower;
char buffer[12];
ltoa(this->transmitPower-1, buffer, 10);
uart_puts(buffer);
}
void Send(char* message)
{
uart_puts(message);
}
void Send(uint8 message)
{
uart_putc(message);
}
int16 Recieve()
{
return uart_getc();
}
};
#endif

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

@ -44,9 +44,6 @@ Robot::Robot()
//Activate interrupt //Activate interrupt
sei(); sei();
// Initialiate TWI
Init_TWI();
//Interface //Interface
for(uint8 i = IO_START; i < IO_END; i++) for(uint8 i = IO_START; i < IO_END; i++)
{ {
@ -88,17 +85,13 @@ bool Robot::RemoveModule(IO_Module* oldModule)
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void Robot::Update() void Robot::Update()
{ {
GetModule<Command_Handler>(IO_COMMAND_HANDLER_MAIN)->Update(); GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN)->Update();
GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN)->Update(); GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN)->Update();
GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN)->Update();
GetModule<Obstacle_Tracker>(IO_OBSTACLE_TRACKER_MAIN)->Update();
//GetModule<Logic>(IO_LOGIC_MAIN)->Update();
GetModule<Navigator>(IO_NAVIGATOR_MAIN)->Update(); GetModule<Navigator>(IO_NAVIGATOR_MAIN)->Update();
//insert code here
} }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------

2
source/Concept/Framework/stdafx.h
View file

@ -20,9 +20,7 @@
#include "distance_sensor.h" #include "distance_sensor.h"
#include "ir_sensor.h" #include "ir_sensor.h"
#include "mouse_sensor.h" #include "mouse_sensor.h"
#include "command_handler.h"
#include "position_tracker.h" #include "position_tracker.h"
#include "obstacle_tracker.h"
#include "ball_tracker.h" #include "ball_tracker.h"
#include "navigator.h" #include "navigator.h"
#include "logic.h" #include "logic.h"