Willem Melching / Mbed 2 deprecated bootje_v4

Dependencies:   mbed

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?

        

            
                
UserRevisionLine numberNew 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