![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
invoer/kompas.cpp@0:bec310bde899, 2011-09-27 (annotated)
- Committer:
- pd0wm
- Date:
- Tue Sep 27 19:46:30 2011 +0000
- Revision:
- 0:bec310bde899
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pd0wm | 0:bec310bde899 | 1 | #include "kompas.h" |
pd0wm | 0:bec310bde899 | 2 | |
pd0wm | 0:bec310bde899 | 3 | Kompas::Kompas():compass(p9, p10) { |
pd0wm | 0:bec310bde899 | 4 | koers = 0; |
pd0wm | 0:bec310bde899 | 5 | prev = 0; |
pd0wm | 0:bec310bde899 | 6 | cal = 0; |
pd0wm | 0:bec310bde899 | 7 | |
pd0wm | 0:bec310bde899 | 8 | wait(0.5); |
pd0wm | 0:bec310bde899 | 9 | |
pd0wm | 0:bec310bde899 | 10 | char compass_out[3]; |
pd0wm | 0:bec310bde899 | 11 | compass_out[0] = 0xF1; |
pd0wm | 0:bec310bde899 | 12 | compass_out[1] = 0x05; |
pd0wm | 0:bec310bde899 | 13 | compass_out[1] = 0x02; |
pd0wm | 0:bec310bde899 | 14 | compass.write(ADDR,compass_out,3); |
pd0wm | 0:bec310bde899 | 15 | wait_ms(1); |
pd0wm | 0:bec310bde899 | 16 | refresh.attach(this,&Kompas::update,0.1); |
pd0wm | 0:bec310bde899 | 17 | |
pd0wm | 0:bec310bde899 | 18 | |
pd0wm | 0:bec310bde899 | 19 | |
pd0wm | 0:bec310bde899 | 20 | } |
pd0wm | 0:bec310bde899 | 21 | |
pd0wm | 0:bec310bde899 | 22 | void Kompas::update(void) { |
pd0wm | 0:bec310bde899 | 23 | //wat variabeles om als zend en ontvang buffer te fungeren |
pd0wm | 0:bec310bde899 | 24 | char compass_out[1]; |
pd0wm | 0:bec310bde899 | 25 | char compass_in[6]; |
pd0wm | 0:bec310bde899 | 26 | int i; |
pd0wm | 0:bec310bde899 | 27 | |
pd0wm | 0:bec310bde899 | 28 | //als het kompas niet bezig met calibreren kan er gelezen worden |
pd0wm | 0:bec310bde899 | 29 | if (cal == 0) { |
pd0wm | 0:bec310bde899 | 30 | |
pd0wm | 0:bec310bde899 | 31 | //zet 0x50(="sample heading") in zend buffer |
pd0wm | 0:bec310bde899 | 32 | compass_out[0] = 0x50; |
pd0wm | 0:bec310bde899 | 33 | //stuur deze buffer over de bus |
pd0wm | 0:bec310bde899 | 34 | compass.write(ADDR,compass_out,1); |
pd0wm | 0:bec310bde899 | 35 | |
pd0wm | 0:bec310bde899 | 36 | //geef het kompas even de tijd om de opdracht uit te voeren |
pd0wm | 0:bec310bde899 | 37 | wait_ms(1); |
pd0wm | 0:bec310bde899 | 38 | |
pd0wm | 0:bec310bde899 | 39 | //Daarna kunnen er 6 8bits waardes uitgelezen worden |
pd0wm | 0:bec310bde899 | 40 | compass.read(ADDR,compass_in,6); |
pd0wm | 0:bec310bde899 | 41 | |
pd0wm | 0:bec310bde899 | 42 | |
pd0wm | 0:bec310bde899 | 43 | //Deze 6 8 bits waardes moeten omgerekend worden naar 3 16 bits waardes |
pd0wm | 0:bec310bde899 | 44 | int compass_heading = (int)compass_in[0] << 8 | (int)compass_in[1]; |
pd0wm | 0:bec310bde899 | 45 | int compass_pitch = (int)compass_in[2] << 8 | (int)compass_in[3]; |
pd0wm | 0:bec310bde899 | 46 | int compass_roll = (int)compass_in[4] << 8 | (int)compass_in[5]; |
pd0wm | 0:bec310bde899 | 47 | int n = compass_heading / 10; |
pd0wm | 0:bec310bde899 | 48 | |
pd0wm | 0:bec310bde899 | 49 | |
pd0wm | 0:bec310bde899 | 50 | int verschil = n - prev; |
pd0wm | 0:bec310bde899 | 51 | if (verschil > 180) |
pd0wm | 0:bec310bde899 | 52 | verschil -= 360; |
pd0wm | 0:bec310bde899 | 53 | if (verschil < -180) |
pd0wm | 0:bec310bde899 | 54 | verschil += 360; |
pd0wm | 0:bec310bde899 | 55 | |
pd0wm | 0:bec310bde899 | 56 | if (abs(verschil) > 60) { |
pd0wm | 0:bec310bde899 | 57 | if (verschil > 0) |
pd0wm | 0:bec310bde899 | 58 | prev += 20; |
pd0wm | 0:bec310bde899 | 59 | else |
pd0wm | 0:bec310bde899 | 60 | prev -= 20; |
pd0wm | 0:bec310bde899 | 61 | } else { |
pd0wm | 0:bec310bde899 | 62 | |
pd0wm | 0:bec310bde899 | 63 | |
pd0wm | 0:bec310bde899 | 64 | //n is ok, nu gemiddelde berekenen |
pd0wm | 0:bec310bde899 | 65 | |
pd0wm | 0:bec310bde899 | 66 | //buffer doorschuiven |
pd0wm | 0:bec310bde899 | 67 | for (i = 0;i != 9; i++) |
pd0wm | 0:bec310bde899 | 68 | opslag[i] = opslag[i+1]; |
pd0wm | 0:bec310bde899 | 69 | |
pd0wm | 0:bec310bde899 | 70 | opslag[9] = n; |
pd0wm | 0:bec310bde899 | 71 | |
pd0wm | 0:bec310bde899 | 72 | //gemiddelde berekenen |
pd0wm | 0:bec310bde899 | 73 | int som = 0; |
pd0wm | 0:bec310bde899 | 74 | for (i = 0;i != 10; i++) |
pd0wm | 0:bec310bde899 | 75 | som += opslag[i]; |
pd0wm | 0:bec310bde899 | 76 | |
pd0wm | 0:bec310bde899 | 77 | koers = (som / 20); |
pd0wm | 0:bec310bde899 | 78 | koers *= 2; |
pd0wm | 0:bec310bde899 | 79 | |
pd0wm | 0:bec310bde899 | 80 | |
pd0wm | 0:bec310bde899 | 81 | |
pd0wm | 0:bec310bde899 | 82 | |
pd0wm | 0:bec310bde899 | 83 | } |
pd0wm | 0:bec310bde899 | 84 | |
pd0wm | 0:bec310bde899 | 85 | |
pd0wm | 0:bec310bde899 | 86 | } else |
pd0wm | 0:bec310bde899 | 87 | koers = 0; |
pd0wm | 0:bec310bde899 | 88 | } |
pd0wm | 0:bec310bde899 | 89 | int Kompas::get(void) { |
pd0wm | 0:bec310bde899 | 90 | return koers; |
pd0wm | 0:bec310bde899 | 91 | |
pd0wm | 0:bec310bde899 | 92 | } |
pd0wm | 0:bec310bde899 | 93 | |
pd0wm | 0:bec310bde899 | 94 | void Kompas::simulate(int inc) { |
pd0wm | 0:bec310bde899 | 95 | koers += inc; |
pd0wm | 0:bec310bde899 | 96 | if (koers > 360) |
pd0wm | 0:bec310bde899 | 97 | koers -= 360; |
pd0wm | 0:bec310bde899 | 98 | if (koers < 0) |
pd0wm | 0:bec310bde899 | 99 | koers += 360; |
pd0wm | 0:bec310bde899 | 100 | } |
pd0wm | 0:bec310bde899 | 101 | |
pd0wm | 0:bec310bde899 | 102 | void Kompas::startcal(void) { |
pd0wm | 0:bec310bde899 | 103 | char compass_out[1]; |
pd0wm | 0:bec310bde899 | 104 | if (cal == 0) { |
pd0wm | 0:bec310bde899 | 105 | cal = 1; |
pd0wm | 0:bec310bde899 | 106 | compass_out[0] = 0x71; |
pd0wm | 0:bec310bde899 | 107 | compass.write(ADDR,compass_out,1); |
pd0wm | 0:bec310bde899 | 108 | } |
pd0wm | 0:bec310bde899 | 109 | } |
pd0wm | 0:bec310bde899 | 110 | |
pd0wm | 0:bec310bde899 | 111 | void Kompas::stopcal(void) { |
pd0wm | 0:bec310bde899 | 112 | char compass_out[1]; |
pd0wm | 0:bec310bde899 | 113 | if (cal == 1) { |
pd0wm | 0:bec310bde899 | 114 | compass_out[0] = 0x7E; |
pd0wm | 0:bec310bde899 | 115 | compass.write(ADDR,compass_out,1); |
pd0wm | 0:bec310bde899 | 116 | cal = 0; |
pd0wm | 0:bec310bde899 | 117 | } |
pd0wm | 0:bec310bde899 | 118 | } |
pd0wm | 0:bec310bde899 | 119 |