Diff: invoer/kompas.cpp

Revision:

0:bec310bde899

--- /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;
+    }
+}
+