![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Diff: invoer/kompas.cpp
- Revision:
- 0:bec310bde899
diff -r 000000000000 -r bec310bde899 invoer/kompas.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/invoer/kompas.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,119 @@ +#include "kompas.h" + +Kompas::Kompas():compass(p9, p10) { + koers = 0; + prev = 0; + cal = 0; + + wait(0.5); + + char compass_out[3]; + compass_out[0] = 0xF1; + compass_out[1] = 0x05; + compass_out[1] = 0x02; + compass.write(ADDR,compass_out,3); + wait_ms(1); + refresh.attach(this,&Kompas::update,0.1); + + + +} + +void Kompas::update(void) { + //wat variabeles om als zend en ontvang buffer te fungeren + char compass_out[1]; + char compass_in[6]; + int i; + + //als het kompas niet bezig met calibreren kan er gelezen worden + if (cal == 0) { + + //zet 0x50(="sample heading") in zend buffer + compass_out[0] = 0x50; + //stuur deze buffer over de bus + compass.write(ADDR,compass_out,1); + + //geef het kompas even de tijd om de opdracht uit te voeren + wait_ms(1); + + //Daarna kunnen er 6 8bits waardes uitgelezen worden + compass.read(ADDR,compass_in,6); + + + //Deze 6 8 bits waardes moeten omgerekend worden naar 3 16 bits waardes + int compass_heading = (int)compass_in[0] << 8 | (int)compass_in[1]; + int compass_pitch = (int)compass_in[2] << 8 | (int)compass_in[3]; + int compass_roll = (int)compass_in[4] << 8 | (int)compass_in[5]; + int n = compass_heading / 10; + + + int verschil = n - prev; + if (verschil > 180) + verschil -= 360; + if (verschil < -180) + verschil += 360; + + if (abs(verschil) > 60) { + if (verschil > 0) + prev += 20; + else + prev -= 20; + } else { + + + //n is ok, nu gemiddelde berekenen + + //buffer doorschuiven + for (i = 0;i != 9; i++) + opslag[i] = opslag[i+1]; + + opslag[9] = n; + + //gemiddelde berekenen + int som = 0; + for (i = 0;i != 10; i++) + som += opslag[i]; + + koers = (som / 20); + koers *= 2; + + + + + } + + + } else + koers = 0; +} +int Kompas::get(void) { + return koers; + +} + +void Kompas::simulate(int inc) { + koers += inc; + if (koers > 360) + koers -= 360; + if (koers < 0) + koers += 360; +} + +void Kompas::startcal(void) { + char compass_out[1]; + if (cal == 0) { + cal = 1; + compass_out[0] = 0x71; + compass.write(ADDR,compass_out,1); + } +} + +void Kompas::stopcal(void) { + char compass_out[1]; + if (cal == 1) { + compass_out[0] = 0x7E; + compass.write(ADDR,compass_out,1); + cal = 0; + } +} +