summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Global.c10
-rw-r--r--Global.h41
-rw-r--r--Navigation.c46
-rw-r--r--Navigation.h31
-rw-r--r--Robocup.aps1
-rw-r--r--Robocup.c171
-rw-r--r--Sensors.c88
-rw-r--r--Sensors.h149
-rw-r--r--qfixSoccerBoard.c299
-rw-r--r--qfixSoccerBoard.h101
10 files changed, 937 insertions, 0 deletions
diff --git a/Global.c b/Global.c
new file mode 100644
index 0000000..29fdf79
--- /dev/null
+++ b/Global.c
@@ -0,0 +1,10 @@
+#include "Global.h"
+
+
+void *operator new(size_t sz) {
+ return malloc(sz);
+}
+
+void operator delete(void *p) {
+ free(p);
+}
diff --git a/Global.h b/Global.h
new file mode 100644
index 0000000..1cf4ccc
--- /dev/null
+++ b/Global.h
@@ -0,0 +1,41 @@
+#ifndef _ROBOCUP_GLOBAL_H_
+#define _ROBOCUP_GLOBAL_H_
+
+#include <stdlib.h>
+
+
+// Geschwindigkeiten
+#define DEFAULT_SPEED 255
+#define SPEED_RUECK 50
+
+// Status
+#define STATUS_OK 0
+#define STATUS_WEISS 1
+#define STATUS_HINDERNIS_L 2 // Hindernis vorne links
+#define STATUS_HINDERNIS_R 3 // Hindernis vorne rechts
+#define STATUS_HINDERNIS_WAR_L 4 // Hindernis war vorne links
+#define STATUS_HINDERNIS_WAR_R 5 // Hindernis war vorne rechts
+#define STATUS_HINDERNIS_VOR_L 6 // Hindernis ist gleich vorne links
+#define STATUS_HINDERNIS_VOR_R 7 // Hindernis ist gleich vorne rechts
+
+// Sensoren
+#define KALI_MINDIFF 80 // minimale Abweichung s/w
+#define KALI_WEISSMAXDIFF 5 // maximale Abweichung weiss1/weiss2
+
+#define HINDERNIS_ERKENNUNGSWERT 155 // 100
+#define HINDERNIS_ERKENNUNGSWERT_SEITL 80
+#define HINDERNIS_SOLLWERT 90
+#define HINDERNIS_ABW 5 // 10
+#define HINDERNIS_SLEEP 100
+
+// Opfer
+#define OPFER_TIMEOUT 500
+#define OPFER_WAIT 2000
+#define OPFER_TIMER 1000
+#define OPFER_BEEP 440
+
+
+void *operator new(size_t sz);
+void operator delete(void *p);
+
+#endif
diff --git a/Navigation.c b/Navigation.c
new file mode 100644
index 0000000..9a3f69d
--- /dev/null
+++ b/Navigation.c
@@ -0,0 +1,46 @@
+#include "Navigation.h"
+
+
+void Navigation::start() {
+ int r = m_speed, l = m_speed;
+
+
+ switch(m_richtung)
+ {
+ case -2:
+ l = 0;
+ break;
+ case -1:
+ l /= 4;
+ break;
+ case 1:
+ r /= 4;
+ break;
+ case 2:
+ r = 0;
+ }
+
+ m_board->motor(MOTOR_PORT_LINKS, l);
+ m_board->motor(MOTOR_PORT_RECHTS, r);
+
+ m_gestartet = true;
+}
+
+void Navigation::stop() {
+ m_board->motor(MOTOR_PORT_LINKS, 0);
+ m_board->motor(MOTOR_PORT_RECHTS, 0);
+
+ m_gestartet = false;
+}
+
+void Navigation::setSpeed(int speed) {
+ m_speed = (speed > 255) ? 255 : (speed < -255) ? -255 : speed;
+
+ if(m_gestartet) start();
+}
+
+void Navigation::setRichtung(int richtung) {
+ m_richtung = richtung;
+
+ if(m_gestartet) start();
+}
diff --git a/Navigation.h b/Navigation.h
new file mode 100644
index 0000000..3f3f1ec
--- /dev/null
+++ b/Navigation.h
@@ -0,0 +1,31 @@
+#include "qfixSoccerBoard.h"
+#include "Global.h"
+
+
+// Physikalische Motorausgänge
+#define MOTOR_PORT_LINKS 0
+#define MOTOR_PORT_RECHTS 1
+
+
+class Navigation {
+ private:
+ SoccerBoard *m_board;
+ bool m_gestartet;
+ int m_speed;
+ int m_richtung;
+ public:
+ Navigation(SoccerBoard *board) {
+ m_gestartet = false;
+ m_speed = DEFAULT_SPEED;
+ m_richtung = 0;
+
+ m_board = board;
+ }
+
+ int getRichtung() {return m_richtung;}
+
+ void start();
+ void stop();
+ void setSpeed(int speed);
+ void setRichtung(int richtung);
+};
diff --git a/Robocup.aps b/Robocup.aps
new file mode 100644
index 0000000..fff9965
--- /dev/null
+++ b/Robocup.aps
@@ -0,0 +1 @@
+<AVRStudio><MANAGEMENT><ProjectName>Robocup</ProjectName><Created>16-Feb-2007 18:11:37</Created><LastEdit>22-Feb-2007 17:07:39</LastEdit><ICON>241</ICON><ProjectType>0</ProjectType><Created>16-Feb-2007 18:11:37</Created><Version>4</Version><Build>4, 12, 0, 491</Build><ProjectTypeName>AVR GCC</ProjectTypeName></MANAGEMENT><CODE_CREATION><ObjectFile>default\Robocup.elf</ObjectFile><EntryFile></EntryFile><SaveFolder>C:\Dokumente und Einstellungen\Emily\Desktop\Robocup\</SaveFolder></CODE_CREATION><DEBUG_TARGET><CURRENT_TARGET>JTAGICE mkII</CURRENT_TARGET><CURRENT_PART>ATmega128.xml</CURRENT_PART><BREAKPOINTS></BREAKPOINTS><IO_EXPAND><HIDE>false</HIDE></IO_EXPAND><REGISTERNAMES><Register>R00</Register><Register>R01</Register><Register>R02</Register><Register>R03</Register><Register>R04</Register><Register>R05</Register><Register>R06</Register><Register>R07</Register><Register>R08</Register><Register>R09</Register><Register>R10</Register><Register>R11</Register><Register>R12</Register><Register>R13</Register><Register>R14</Register><Register>R15</Register><Register>R16</Register><Register>R17</Register><Register>R18</Register><Register>R19</Register><Register>R20</Register><Register>R21</Register><Register>R22</Register><Register>R23</Register><Register>R24</Register><Register>R25</Register><Register>R26</Register><Register>R27</Register><Register>R28</Register><Register>R29</Register><Register>R30</Register><Register>R31</Register></REGISTERNAMES><COM>Auto</COM><COMType>0</COMType><WATCHNUM>0</WATCHNUM><WATCHNAMES><Pane0></Pane0><Pane1></Pane1><Pane2></Pane2><Pane3></Pane3></WATCHNAMES><BreakOnTrcaeFull>0</BreakOnTrcaeFull></DEBUG_TARGET><Debugger><Triggers></Triggers></Debugger><AVRGCCPLUGIN><FILES><SOURCEFILE>Robocup.c</SOURCEFILE><SOURCEFILE>qfixSoccerBoard.c</SOURCEFILE><SOURCEFILE>Sensors.c</SOURCEFILE><SOURCEFILE>Global.c</SOURCEFILE><SOURCEFILE>Navigation.c</SOURCEFILE><HEADERFILE>qfixSoccerBoard.h</HEADERFILE><HEADERFILE>Sensors.h</HEADERFILE><HEADERFILE>Global.h</HEADERFILE><HEADERFILE>Navigation.h</HEADERFILE></FILES><CONFIGS><CONFIG><NAME>default</NAME><USESEXTERNALMAKEFILE>NO</USESEXTERNALMAKEFILE><EXTERNALMAKEFILE></EXTERNALMAKEFILE><PART>at90can128</PART><HEX>1</HEX><LIST>0</LIST><MAP>0</MAP><OUTPUTFILENAME>Robocup.elf</OUTPUTFILENAME><OUTPUTDIR>default\</OUTPUTDIR><ISDIRTY>1</ISDIRTY><OPTIONS><OPTION><FILE>Robocup.c</FILE><OPTIONLIST></OPTIONLIST></OPTION></OPTIONS><INCDIRS><INCLUDE>C:\Programme\WinAVR\include\</INCLUDE></INCDIRS><LIBDIRS/><LIBS/><LINKOBJECTS/><OPTIONSFORALL>-Wall -gdwarf-2 -xc++ -DF_CPU=16000000UL -O0 -fsigned-char</OPTIONSFORALL><LINKEROPTIONS></LINKEROPTIONS><SEGMENTS/></CONFIG></CONFIGS><LASTCONFIG>default</LASTCONFIG><USES_WINAVR>1</USES_WINAVR><GCC_LOC>C:\Programme\WinAVR\bin\avr-gcc.exe</GCC_LOC><MAKE_LOC>C:\Programme\WinAVR\utils\bin\make.exe</MAKE_LOC></AVRGCCPLUGIN><ProjectFiles><Files><Name>C:\Dokumente und Einstellungen\Emily\Desktop\Robocup\qfixSoccerBoard.h</Name><Name>C:\Dokumente und Einstellungen\Emily\Desktop\Robocup\Sensors.h</Name><Name>C:\Dokumente und Einstellungen\Emily\Desktop\Robocup\Global.h</Name><Name>C:\Dokumente und Einstellungen\Emily\Desktop\Robocup\Navigation.h</Name><Name>C:\Dokumente und Einstellungen\Emily\Desktop\Robocup\Robocup.c</Name><Name>C:\Dokumente und Einstellungen\Emily\Desktop\Robocup\qfixSoccerBoard.c</Name><Name>C:\Dokumente und Einstellungen\Emily\Desktop\Robocup\Sensors.c</Name><Name>C:\Dokumente und Einstellungen\Emily\Desktop\Robocup\Global.c</Name><Name>C:\Dokumente und Einstellungen\Emily\Desktop\Robocup\Navigation.c</Name></Files></ProjectFiles><Files><File00000><FileId>00000</FileId><FileName>Robocup.c</FileName><Status>1</Status></File00000><File00001><FileId>00001</FileId><FileName>Sensors.c</FileName><Status>1</Status></File00001><File00002><FileId>00002</FileId><FileName>Sensors.h</FileName><Status>1</Status></File00002><File00003><FileId>00003</FileId><FileName>Global.h</FileName><Status>1</Status></File00003><File00004><FileId>00004</FileId><FileName>qfixSoccerBoard.h</FileName><Status>1</Status></File00004><File00005><FileId>00005</FileId><FileName>qfixSoccerBoard.c</FileName><Status>1</Status></File00005><File00006><FileId>00006</FileId><FileName>Global.c</FileName><Status>1</Status></File00006><File00007><FileId>00007</FileId><FileName>Navigation.c</FileName><Status>1</Status></File00007><File00008><FileId>00008</FileId><FileName>Navigation.h</FileName><Status>1</Status></File00008></Files><Workspace><File00000><Position>303 73 1025 462</Position><LineCol>71 0</LineCol></File00000><File00001><Position>303 72 1025 461</Position><LineCol>77 52</LineCol><State>Maximized</State></File00001><File00002><Position>294 66 1016 458</Position><LineCol>16 43</LineCol></File00002><File00003><Position>364 183 990 425</Position><LineCol>20 23</LineCol></File00003><File00004><Position>386 212 1012 454</Position><LineCol>94 0</LineCol></File00004><File00005><Position>297 68 1019 460</Position><LineCol>294 0</LineCol></File00005><File00006><Position>320 125 946 367</Position><LineCol>0 0</LineCol></File00006><File00007><Position>342 154 968 396</Position><LineCol>14 0</LineCol></File00007><File00008><Position>364 183 990 425</Position><LineCol>14 0</LineCol></File00008></Workspace><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio>
diff --git a/Robocup.c b/Robocup.c
new file mode 100644
index 0000000..08258df
--- /dev/null
+++ b/Robocup.c
@@ -0,0 +1,171 @@
+#include "Global.h"
+#include "Navigation.h"
+#include "Sensors.h"
+#include "qfixSoccerBoard.h"
+
+
+
+void OpferSignal(SoccerBoard *board) {
+ board->ledOn(0);
+ board->beep(OPFER_BEEP);
+ board->sleep(OPFER_WAIT);
+ board->ledOff(0);
+ board->beepOff();
+}
+
+
+int main() {
+ int status = STATUS_OK;
+ int linie = 0, last_linie = 0;
+ SoccerBoard board;
+ Sensors sensors(&board);
+ Navigation nav(&board);
+
+ do {
+ sensors.update();
+ } while(!sensors.kalibrieren());
+
+ board.ledOn(0);
+ board.waitForButton(0);
+ board.ledOff(0);
+ board.sleep(4000);
+
+ nav.start();
+
+ while(true) {
+ sensors.update();
+ sensors.auswerten();
+
+ last_linie = linie;
+ linie = sensors.getLinie();
+
+ switch(status) {
+ case STATUS_OK:
+ if(linie == 0) sensors.kalibrieren();
+
+ if(sensors.getOpfer() && board.timer()) {
+ nav.stop();
+ OpferSignal(&board);
+ nav.start();
+ board.setTimer(OPFER_TIMER);
+ break;
+ }
+
+ if(sensors.hindernisV()) {
+ nav.setSpeed(SPEED_RUECK);
+ nav.setRichtung(0);
+ board.sleep(HINDERNIS_SLEEP);
+
+ nav.setSpeed(DEFAULT_SPEED);
+
+ if(sensors.getDsVL() > sensors.getDsVR()) {
+ // Hindernis eher links -> wir fahren rechts rum
+ nav.setRichtung(2);
+ status = STATUS_HINDERNIS_VOR_L; // status variable setzen
+ }
+ else {
+ // Hindernis eher rechts -> wir fahren links rum
+ nav.setRichtung(-2);
+ status = STATUS_HINDERNIS_VOR_R; // status variable setzen
+ }
+
+ break;
+ }
+
+ switch(linie) {
+ case LINIE_FEHLER:
+ break;
+ case LINIE_WEISS:
+ /*if(abs(last_linie) == 2) break;
+
+ nav.setRichtung(0);
+ status = STATUS_WEISS;*/
+
+ break;
+ default:
+ nav.setRichtung(linie);
+ }
+
+ break;
+ case STATUS_WEISS:
+ if(sensors.getOpfer() && board.timer()) {
+ nav.stop();
+ OpferSignal(&board);
+ nav.start();
+ board.setTimer(OPFER_TIMER);
+ continue;
+ }
+
+ if(linie != LINIE_WEISS) status = STATUS_OK;
+ break;
+ case STATUS_HINDERNIS_VOR_L:
+ if(sensors.hindernisL() && linie == LINIE_WEISS) status = STATUS_HINDERNIS_L;
+ break;
+ case STATUS_HINDERNIS_VOR_R:
+ if(sensors.hindernisR() && linie == LINIE_WEISS) status = STATUS_HINDERNIS_R;
+ break;
+ case STATUS_HINDERNIS_L:
+ if(sensors.hindernisVL()) {
+ nav.setSpeed(SPEED_RUECK);
+ nav.setRichtung(0);
+ board.sleep(HINDERNIS_SLEEP);
+
+ nav.setSpeed(DEFAULT_SPEED);
+ nav.setRichtung(2);
+ status = STATUS_HINDERNIS_VOR_L;
+ }
+ else if(linie == LINIE_WEISS) {
+ if(sensors.getDsL() > HINDERNIS_SOLLWERT + HINDERNIS_ABW)
+ nav.setRichtung(2);
+ else if(sensors.getDsL() < HINDERNIS_SOLLWERT - HINDERNIS_ABW)
+ nav.setRichtung(-2);
+ else
+ nav.setRichtung(0);
+ }
+ else {
+ status = STATUS_HINDERNIS_WAR_L;
+ nav.setRichtung(0);
+ }
+ break;
+ case STATUS_HINDERNIS_R:
+ if(sensors.hindernisVR()) {
+ nav.setSpeed(SPEED_RUECK);
+ nav.setRichtung(0);
+ board.sleep(HINDERNIS_SLEEP);
+
+ nav.setSpeed(DEFAULT_SPEED);
+ nav.setRichtung(-2);
+ status = STATUS_HINDERNIS_VOR_R;
+ }
+ else if(linie == LINIE_WEISS) {
+ if(sensors.getDsR() > HINDERNIS_SOLLWERT + HINDERNIS_ABW)
+ nav.setRichtung(-2);
+ else if(sensors.getDsR() < HINDERNIS_SOLLWERT - HINDERNIS_ABW)
+ nav.setRichtung(2);
+ else
+ nav.setRichtung(0);
+ }
+ else {
+ status = STATUS_HINDERNIS_WAR_R;
+ nav.setRichtung(0);
+ }
+ break;
+ case STATUS_HINDERNIS_WAR_L:
+ if(linie != LINIE_WEISS) break;
+
+ board.sleep(40);
+ status = STATUS_OK;
+ nav.setRichtung(2);
+
+ break;
+ case STATUS_HINDERNIS_WAR_R:
+ if(linie != LINIE_WEISS) break;
+
+ board.sleep(40);
+ status = STATUS_OK;
+ nav.setRichtung(-2);
+ }
+ }
+
+ return 0;
+}
diff --git a/Sensors.c b/Sensors.c
new file mode 100644
index 0000000..ffbdb96
--- /dev/null
+++ b/Sensors.c
@@ -0,0 +1,88 @@
+#include "qfixSoccerBoard.h"
+#include "Global.h"
+#include "Sensors.h"
+
+
+void Sensor::update() {
+ m_board->analog(m_port);
+ m_board->analog(m_port);
+ m_wert = m_board->analog(m_port);
+}
+
+
+
+void Sensors::update() {
+ m_lsLinks->update();
+ m_lsMitte->update();
+ m_lsRechts->update();
+
+ m_dsL->update();
+ m_dsVL->update();
+ m_dsV->update();
+ m_dsVR->update();
+ m_dsR->update();
+}
+
+bool Sensors::kalibrieren() {
+ int links = m_lsLinks->getWert();
+ int mitte = m_lsMitte->getWert();
+ int rechts = m_lsRechts->getWert();
+
+ int weiss = (rechts + links) / 2;
+ int diff = mitte - weiss;
+
+
+ if(links > mitte || rechts > mitte) return false;
+
+ if(diff < KALI_MINDIFF) return false;
+ if(abs(rechts - links) > KALI_WEISSMAXDIFF) return false;
+
+ if(!m_kalibriert) {
+ m_gw_sg = mitte - diff/8;
+ m_gw_gw = weiss + diff/8;
+ m_gw_ws = weiss - diff/8;
+ m_gw_sw = weiss + diff/2;
+
+ m_kalibriert = true;
+ }
+ else {
+ m_gw_sg = m_gw_sg/2 + mitte/2 - diff/16;
+ m_gw_gw = m_gw_gw/2 + weiss/2 + diff/16;
+ m_gw_ws = m_gw_ws/2 + weiss/2 - diff/16;
+ m_gw_sw = m_gw_sw/2 + weiss/2 + diff/4;
+ }
+
+ return true;
+}
+
+void Sensors::auswerten() {
+ // Linie
+ m_liniePos = int(SW_SCHWARZ(m_lsRechts)) - int(SW_SCHWARZ(m_lsLinks)); //Linie links oder rechts -> -1 oder 1, beides oder keines -> 0
+ m_liniePos *= (1+int(SW_WEISS(m_lsMitte))); //Kurvenstärke bestimmen
+
+ if((!m_liniePos) && SW_WEISS(m_lsMitte)) //alles weiß?
+ m_liniePos = LINIE_WEISS;
+
+ if(SW_SCHWARZ(m_lsLinks) && SW_SCHWARZ(m_lsRechts)) // beide äußeren schwarz? -> fehler
+ m_liniePos = LINIE_FEHLER;
+
+
+ // Opfer
+ if(m_sLinks) m_sLinks--;
+ if(m_sMitte) m_sMitte--;
+ if(m_sRechts) m_sRechts--;
+
+ if(m_gLinks) m_gLinks--;
+ if(m_gMitte) m_gMitte--;
+ if(m_gRechts) m_gRechts--;
+
+ if(C_SILBER(m_lsLinks)) m_sLinks = OPFER_TIMEOUT;
+ if(C_SILBER(m_lsMitte)) m_sMitte = OPFER_TIMEOUT;
+ if(C_SILBER(m_lsRechts)) m_sRechts = OPFER_TIMEOUT;
+
+ if(C_GRUEN(m_lsLinks)) m_gLinks = OPFER_TIMEOUT;
+ if(C_GRUEN(m_lsMitte)) m_gMitte = OPFER_TIMEOUT;
+ if(C_GRUEN(m_lsRechts)) m_gRechts = OPFER_TIMEOUT;
+}
+
+
diff --git a/Sensors.h b/Sensors.h
new file mode 100644
index 0000000..54e3d4b
--- /dev/null
+++ b/Sensors.h
@@ -0,0 +1,149 @@
+#ifndef _ROBOCUP_SENSORS_H_
+#define _ROBOCUP_SENSORS_H_
+
+#include "qfixSoccerBoard.h"
+
+#define SENSOR_PORT_LINIE_LINKS 0
+#define SENSOR_PORT_LINIE_MITTE 1
+#define SENSOR_PORT_LINIE_RECHTS 2
+
+#define SENSOR_PORT_D_L 3
+#define SENSOR_PORT_D_VL 4
+#define SENSOR_PORT_D_V 5
+#define SENSOR_PORT_D_VR 6
+#define SENSOR_PORT_D_R 7
+
+#define LINIE_WEISS 70
+#define LINIE_FEHLER 71
+
+#define SW_SCHWARZ(sensor) (sensor->getWert() > m_gw_sw)
+#define SW_WEISS(sensor) (sensor->getWert() <= m_gw_sw)
+
+#define C_SCHWARZ(sensor) (sensor->getWert() > m_gw_sg)
+#define C_GRUEN(sensor) (sensor->getWert() > m_gw_gw && sensor->getWert() <= m_gw_sg)
+#define C_WEISS(sensor) (sensor->getWert() > m_gw_ws && sensor->getWert() <= m_gw_gw)
+#define C_SILBER(sensor) (sensor->getWert() <= m_gw_ws)
+
+
+class Sensor {
+ private:
+ SoccerBoard *m_board;
+ int m_port;
+ int m_wert;
+
+ public:
+ Sensor(SoccerBoard *board, int port) {
+ m_board = board;
+ m_port = port;
+ }
+
+ void update();
+
+ int getWert() {
+ return m_wert;
+ }
+};
+
+
+class Sensors {
+ private:
+ SoccerBoard *m_board;
+
+ bool m_kalibriert;
+
+ Sensor *m_lsLinks, *m_lsMitte, *m_lsRechts;
+ int m_sLinks, m_sMitte, m_sRechts;
+ int m_gLinks, m_gMitte, m_gRechts;
+ int m_liniePos;
+
+ int m_gw_sg; // Grenzwert schwarz/grün
+ int m_gw_gw; // Grenzwert grün/weiß
+ int m_gw_ws; // Grenzwert weiß/silber
+ int m_gw_sw; // Grenzwert schwarz/weiß
+
+ Sensor *m_dsL, *m_dsVL, *m_dsV, *m_dsVR, *m_dsR;
+ public:
+ Sensors(SoccerBoard *board) {
+ m_board = board;
+
+ m_kalibriert = false;
+
+ m_lsLinks = new Sensor(board, SENSOR_PORT_LINIE_LINKS);
+ m_lsMitte = new Sensor(board, SENSOR_PORT_LINIE_MITTE);
+ m_lsRechts = new Sensor(board, SENSOR_PORT_LINIE_RECHTS);
+
+ m_dsL = new Sensor(board, SENSOR_PORT_D_L);
+ m_dsVL = new Sensor(board, SENSOR_PORT_D_VL);
+ m_dsV = new Sensor(board, SENSOR_PORT_D_V);
+ m_dsVR = new Sensor(board, SENSOR_PORT_D_VR);
+ m_dsR = new Sensor(board, SENSOR_PORT_D_R);
+
+ m_sLinks = m_sMitte = m_sRechts = 0;
+ m_gLinks = m_gMitte = m_gRechts = 0;
+
+ m_liniePos = 0;
+ }
+
+ ~Sensors() {
+ delete m_lsLinks;
+ delete m_lsMitte;
+ delete m_lsRechts;
+
+ delete m_dsL;
+ delete m_dsVL;
+ delete m_dsV;
+ delete m_dsVR;
+ delete m_dsR;
+ }
+
+ int getLinie() {return m_liniePos;}
+ bool getOpfer() {
+ return /*(m_sLinks && m_sMitte && m_sRechts) || */(m_gLinks && m_gMitte && m_gRechts);
+ }
+
+ bool hindernisL() {
+ return (m_dsL->getWert() > HINDERNIS_ERKENNUNGSWERT_SEITL);
+ }
+
+ bool hindernisVL() {
+ return (m_dsVL->getWert() > HINDERNIS_ERKENNUNGSWERT);
+ }
+
+ bool hindernisV() {
+ return (m_dsV->getWert() > HINDERNIS_ERKENNUNGSWERT);
+ }
+
+ bool hindernisVR() {
+ return (m_dsVR->getWert() > HINDERNIS_ERKENNUNGSWERT);
+ }
+
+ bool hindernisR() {
+ return m_dsR->getWert();
+ }
+
+ int getDsL() {
+ return m_dsL->getWert();
+ }
+
+ int getDsVL() {
+ return m_dsVL->getWert();
+ }
+
+ int getDsV() {
+ return m_dsV->getWert();
+ }
+
+ int getDsVR() {
+ return m_dsVR->getWert();
+ }
+
+ int getDsR() {
+ return m_dsR->getWert();
+ }
+
+ void update();
+ void auswerten();
+ bool kalibrieren();
+};
+
+#endif
diff --git a/qfixSoccerBoard.c b/qfixSoccerBoard.c
new file mode 100644
index 0000000..06bd3b8
--- /dev/null
+++ b/qfixSoccerBoard.c
@@ -0,0 +1,299 @@
+#include "qfixSoccerBoard.h"
+
+
+static int speedMotor0 = 0;
+static int speedMotor1 = 0;
+//static int speedMotor2 = 0;
+//static int speedMotor3 = 0;
+//static int speedMotor4 = 0;
+//static int speedMotor5 = 0;
+
+static unsigned long timerVal = 0;
+static unsigned long sleepVal = 0;
+
+static int beepFreq = 0;
+static int beepFreqTimer = 0;
+static long beepTimer = 0;
+
+
+// PWM routine //
+SIGNAL (SIG_OVERFLOW0)
+{
+ const int OFFSET=50; // motor does not work with very low ratio
+ static int counter=255+OFFSET;
+
+ if (speedMotor0==0) cbi(PORTB, 3); // enable1 = 0
+ else if (abs(speedMotor0)+OFFSET >= counter) sbi(PORTB, 3); // enable1 = 1
+ else cbi(PORTB, 3); // enable1 = 0
+
+ if (speedMotor1==0) cbi(PORTG, 2); // enable2 = 0
+ else if (abs(speedMotor1)+OFFSET >= counter) sbi(PORTG, 2); // enable2 = 1
+ else cbi(PORTG, 2); // enable2 = 0
+
+ /*if (speedMotor2==0) cbi(PORTB, 4); // enable3 = 0
+ else if (abs(speedMotor2)+OFFSET >= counter) sbi(PORTB, 4); // enable3 = 1
+ else cbi(PORTB, 4); // enable3 = 0
+
+ if (speedMotor3==0) cbi(PORTB, 6); // enable4 = 0
+ else if (abs(speedMotor3)+OFFSET >= counter) sbi(PORTB, 6); // enable4 = 1
+ else cbi(PORTB, 6); // enable4 = 0
+
+ if (speedMotor4==0) cbi(PORTB, 5); // enable5 = 0
+ else if (abs(speedMotor4)+OFFSET >= counter) sbi(PORTB, 5); // enable5 = 1
+ else cbi(PORTB, 5); // enable5 = 0
+
+ if (speedMotor5==0) cbi(PORTB, 7); // enable6 = 0
+ else if (abs(speedMotor5)+OFFSET >= counter) sbi(PORTB, 7); // enable6 = 1
+ else cbi(PORTB, 7); // enable6 = 0*/
+
+ if (counter==0) counter=255+OFFSET;
+ else counter--;
+
+ if(timerVal > 0) timerVal--;
+ if(sleepVal > 0) sleepVal--;
+
+ if(beepTimer > 0) beepTimer--;
+
+ if(beepTimer == 0) cbi(PORTB, 4);
+ else {
+ if(beepFreqTimer) beepFreqTimer--;
+ else beepFreqTimer = beepFreq;
+
+ if(beepFreqTimer > beepFreq/2) sbi(PORTB, 4);
+ else cbi(PORTB, 4);
+ }
+}
+
+void initTimer()
+{
+ TIMSK0=1; // timer 0 overflow for interrupt (8 bit timer)
+ TCCR0A=2; // prescaler 8 => 0.5us
+ TCNT0=56;
+ sei(); // enable interrupts
+}
+
+
+SoccerBoard::SoccerBoard()
+{
+ // PORT A: Digital In (PA0, PA1) //
+ DDRA= 255 - (1+2); // all bits output (motor direction) except PA0,PA1 (digital in)
+ PORTA=1+2; // set pullups for digital in
+
+ // PORT B: LEDs (PB0, PB2), Motor enable (PB3-PB7) //
+ DDRB = 1+4; // PB0 + PB4 = LEDs -> output
+ DDRB |= 8+16+32+64+128; // PB3 - PB7 = Motor enable -> output
+ PORTB |= 1+4; // set bits 0 and 2 -> LEDs off
+ PORTB &= 255-(8+16+32+64+128); // clear bits 3-7 -> motor disable
+
+ // PORT C: Power Output //
+ DDRC = 255; // direction port D, all bits output
+ PORTC = 0; // clear all bits -> power on
+
+ // PORT D: I2C, USB, CAN //
+ DDRD = 0; // all bits input
+ PORTD = 1+2; // set bits 0,1 -> I2C pullUps
+
+ // PORT E: Digital In (PE2 - PE7) //
+ DDRE=0; // all bits input
+ PORTE=4+8+16+32+64+128; // set pullups for digital in
+
+ // PORT F: Analog In //
+ DDRF=0; // all bits input
+ ADCSRA=128; // set A/D enable bit (ADEN)
+
+ // PORT G: Buttons (PG3, PG4), motor enable (PG2)
+ DDRG = BV(PG2); // PG2 output
+ PORTG= BV(PG3)+BV(PG4); // set pullups for buttons
+ cbi(PORTG,PG2); // clear PG2 -> motor disable
+
+ initTimer();
+}
+
+
+void SoccerBoard::ledOn(int i)
+{
+ if (i==0) cbi(PORTB, PB0); // clear bit -> LED on
+ else if (i==1) cbi(PORTB, PB2); // clear bit -> LED on
+}
+
+
+void SoccerBoard::ledOff(int i)
+{
+ if (i==0) sbi(PORTB, PB0); // set bit -> LED off
+ else if (i==1) sbi(PORTB, PB2); // set bit -> LED off
+}
+
+void SoccerBoard::ledsOff()
+{
+ PORTB|=BV(PB0)+BV(PB2); // set bits -> LEDs off
+}
+
+void SoccerBoard::led(int i, bool state)
+{
+ if (state) ledOn(i); else ledOff(i);
+}
+
+
+void SoccerBoard::powerOn(int i)
+{
+ if ((i<0) || (i>7)) return;
+ cbi(PORTC, i);
+}
+
+
+void SoccerBoard::powerOff(int i)
+{
+ if ((i<0) || (i>7)) return;
+ sbi(PORTC, i);
+}
+
+
+void SoccerBoard::power(int i, bool state)
+{
+ if (state) powerOn(i); else powerOff(i);
+}
+
+
+bool SoccerBoard::button(int i)
+{
+ if (i==0) return ( (PING & BV(PG4)) == 0);
+ else if (i==1) return ( (PING & BV(PG3)) == 0);
+ else return false; // bad approach...
+}
+
+
+void SoccerBoard::ledMeter(int i,int maxvalue = 256)
+{
+ //led(0, (i>100)); //modified by meyma
+ //led(1, (i>200));
+
+ if(i < (maxvalue / 4))
+ {
+ ledOff(0);
+ ledOff(1);
+ }
+ else if(i < (maxvalue / 2))
+ {
+ ledOn(0);
+ ledOff(1);
+ }
+ else if(i < ((maxvalue /4) * 3))
+ {
+ ledOff(0);
+ ledOn(1);
+ }
+ else {
+ ledOn(0);
+ ledOn(1);
+ }
+}
+
+void SoccerBoard::motor(int i, int speed)
+{
+ if ((i<0) || (i>1)) return;
+
+ if (i==0) {
+ speedMotor0 = speed;
+ if (speed>0) sbi(PORTA, 3); // input1 = 1
+ else cbi(PORTA, 3); // input1 = 0
+ }
+
+ else if (i==1) {
+ speedMotor1 = speed;
+ if (speed>0) sbi(PORTA, 2); // input2 = 1
+ else cbi(PORTA, 2); // input2 = 0
+ }
+
+ /*else if (i==2) {
+ speedMotor2 = speed;
+ if (speed>0) sbi(PORTA, 5); // input3 = 1
+ else cbi(PORTA, 5); // input3 = 0
+ }
+
+ else if (i==3) {
+ speedMotor3 = speed;
+ if (speed>0) sbi(PORTA, 4); // input4 = 1
+ else cbi(PORTA, 4); // input4 = 0
+ }
+
+ else if (i==4) {
+ speedMotor4 = speed;
+ if (speed>0) sbi(PORTA, 7); // input5 = 1
+ else cbi(PORTA, 7); // input5 = 0
+ }
+
+ else if (i==5) {
+ speedMotor5 = speed;
+ if (speed>0) sbi(PORTA, 6); // input6 = 1
+ else cbi(PORTA, 6); // input6 = 0
+ }*/
+}
+
+void SoccerBoard::motorsOff()
+{
+ motor(0,0);
+ motor(1,0);
+ motor(2,0);
+ motor(3,0);
+ motor(4,0);
+ motor(5,0);
+}
+
+
+// return 0-255 //
+int SoccerBoard::analog(int i)
+{
+ if ((i<0) || (i>7)) return -1;
+
+ ADMUX=i; // select analog input and start A/D
+ sbi(ADMUX, ADLAR); // left adjust -> we use only ADCH
+ sbi(ADCSRA, ADSC); // start conversion
+ while (ADCSRA & 64); // wait until ADSC is low again
+ int value = ADCH; // read 8 bit value fom ADCH
+ return value;
+}
+
+
+bool SoccerBoard::digital(int i)
+{
+ if ((i<0) || (i>7)) return false; // bad solution...
+
+ if (i==0) return (PINA & 1);
+ else if (i==1) return (PINA & 2);
+ else return (PINE & (1<<i)) ;
+}
+
+
+void SoccerBoard::waitForButton(int i)
+{
+ if ((i<0) || (i>3)) return; // bad solution...
+ while (!button(i)) { /* do nothing */ }
+ while (button(i)) { /* do nothing */ }
+}
+
+void SoccerBoard::beep(int freq) {
+ beep(freq, -1);
+}
+
+void SoccerBoard::beep(int freq, long msecs) {
+ beepTimer = 7*msecs;
+ beepFreq = 7000 / freq;
+}
+
+void SoccerBoard::beepOff() {
+ beepTimer = 0;
+}
+
+void SoccerBoard::setTimer(unsigned long msecs) {
+ timerVal = 7*msecs;
+}
+
+bool SoccerBoard::timer() {
+ return (timerVal <= 0);
+}
+
+void SoccerBoard::sleep(unsigned long msecs) {
+ sleepVal = 7*msecs;
+
+ while(sleepVal > 0);
+}
diff --git a/qfixSoccerBoard.h b/qfixSoccerBoard.h
new file mode 100644
index 0000000..9278504
--- /dev/null
+++ b/qfixSoccerBoard.h
@@ -0,0 +1,101 @@
+#include <qfix/qfix.h>
+
+
+#ifndef qfixSoccerBoard_h
+#define qfixSoccerBoard_h
+
+
+/**
+* \class SoccerBoard
+* \brief Represents the controller board "SoccerBoard".
+* \author Stefan Enderle
+*
+* The class SoccerBoard represents the
+* physical SoccerBoard with all its inputs and outputs.
+* With this class it is possible to drive the motors,
+* put on LEDs, check the buttons and get data from the
+* analog and digital inputs.
+*/
+
+class SoccerBoard
+{
+
+public:
+ /** Constructor for the SoccerBoard class.
+ */
+ SoccerBoard();
+
+ /** Puts on LED i. i must be 0 or 1.
+ */
+ void ledOn(int i);
+
+ /** Puts off LED i. i must be 0 or 1.
+ */
+ void ledOff(int i);
+
+ /** Puts off all LEDs.
+ */
+ void ledsOff();
+
+ /** Puts LED i on if state is true, else off. i must be 0 or 1.
+ */
+ void led(int i, bool state);
+
+ /** Puts the power output i on
+ */
+ void powerOn(int i);
+
+ /** Puts the power output i off
+ */
+ void powerOff(int i);
+
+ /** Puts the power output i on if state is true, else off.
+ */
+ void power(int i, bool state);
+
+ /** Checks the state of button i. If it is pressed, true is returned,
+ * else false.
+ */
+ bool button(int i);
+
+ /** Uses the four LEDs on the board to display the value i
+ * with 0 <= i <= 255
+ */
+ void ledMeter(int i,int maxvalue);
+
+ /** Sets motor i to the given speed. -255 <= speed <= 255.
+ */
+ void motor(int i, int speed);
+
+ void stop(int i);
+
+ /** Puts off all motors.
+ */
+ void motorsOff();
+
+ /** returns the value of the analog port i. 0 <= value <= 255.
+ */
+ int analog(int i);
+
+ /** returns true if the digital port is logical high, else false.
+ */
+ bool digital(int i);
+
+ /** Waits until button i is pressed and released again.
+ */
+ void waitForButton(int i);
+
+ void beep(int freq);
+ void beep(int freq, long msecs);
+ void beepOff();
+
+ void setTimer(unsigned long msecs);
+ bool timer();
+
+ void sleep(unsigned long msecs);
+};
+
+
+void initTimer();
+
+#endif
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-21 11:47:02 +0100