88 lines
2.2 KiB
C
88 lines
2.2 KiB
C
#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;
|
|
}
|
|
|
|
|