summaryrefslogtreecommitdiffstats
path: root/Sensors.c
diff options
context:
space:
mode:
Diffstat (limited to 'Sensors.c')
-rw-r--r--Sensors.c88
1 files changed, 88 insertions, 0 deletions
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;
+}
+
+