From 98c623dc8969151c0872a2d102d486f9a735d614 Mon Sep 17 00:00:00 2001
From: neoraider <devnull@localhost>
Date: Tue, 3 Apr 2007 12:47:03 +0000
Subject: Neues Rescue-Programm angefangen

---
 Sensors.c | 88 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 88 insertions(+)
 create mode 100644 Sensors.c

(limited to 'Sensors.c')

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;
+}
+
+
-- 
cgit v1.2.3