IUT CACHAN
/
codeursMecatro
Codeurs MECATRO
codeurs.cpp@0:f462be86dc4a, 2021-03-24 (annotated)
- Committer:
- iut_cachan01
- Date:
- Wed Mar 24 15:20:01 2021 +0000
- Revision:
- 0:f462be86dc4a
- Child:
- 1:ac5eb2de660e
Codeurs MECATRO V1.0
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
iut_cachan01 | 0:f462be86dc4a | 1 | #include "codeurs.h" |
iut_cachan01 | 0:f462be86dc4a | 2 | |
iut_cachan01 | 0:f462be86dc4a | 3 | static const int16_t MAX = 16384; |
iut_cachan01 | 0:f462be86dc4a | 4 | |
iut_cachan01 | 0:f462be86dc4a | 5 | Codeurs::Codeurs(PinName sda, PinName scl, int address) : _i2c(sda, scl), _address(address) |
iut_cachan01 | 0:f462be86dc4a | 6 | { |
iut_cachan01 | 0:f462be86dc4a | 7 | _gauche = 0; |
iut_cachan01 | 0:f462be86dc4a | 8 | _droit = 0; |
iut_cachan01 | 0:f462be86dc4a | 9 | _g16 = 0; |
iut_cachan01 | 0:f462be86dc4a | 10 | _d16 = 0; |
iut_cachan01 | 0:f462be86dc4a | 11 | } |
iut_cachan01 | 0:f462be86dc4a | 12 | |
iut_cachan01 | 0:f462be86dc4a | 13 | bool Codeurs::test() |
iut_cachan01 | 0:f462be86dc4a | 14 | { |
iut_cachan01 | 0:f462be86dc4a | 15 | char data = 5; |
iut_cachan01 | 0:f462be86dc4a | 16 | _i2c.write(_address, &data, 1); |
iut_cachan01 | 0:f462be86dc4a | 17 | _i2c.read(_address, &data, 1); |
iut_cachan01 | 0:f462be86dc4a | 18 | return (data == 0x1F); |
iut_cachan01 | 0:f462be86dc4a | 19 | } |
iut_cachan01 | 0:f462be86dc4a | 20 | |
iut_cachan01 | 0:f462be86dc4a | 21 | void Codeurs::read16(int16_t &gauche, int16_t &droit) |
iut_cachan01 | 0:f462be86dc4a | 22 | { |
iut_cachan01 | 0:f462be86dc4a | 23 | char data[4]; |
iut_cachan01 | 0:f462be86dc4a | 24 | data[0] = 1; |
iut_cachan01 | 0:f462be86dc4a | 25 | _i2c.write(_address, data, 1); |
iut_cachan01 | 0:f462be86dc4a | 26 | _i2c.read(_address, data, 4); |
iut_cachan01 | 0:f462be86dc4a | 27 | gauche = data[0]<<8 | (data[1]&0xFF); |
iut_cachan01 | 0:f462be86dc4a | 28 | droit = data[2]<<8 | (data[3]&0xFF);; |
iut_cachan01 | 0:f462be86dc4a | 29 | } |
iut_cachan01 | 0:f462be86dc4a | 30 | |
iut_cachan01 | 0:f462be86dc4a | 31 | void Codeurs::reset() |
iut_cachan01 | 0:f462be86dc4a | 32 | { |
iut_cachan01 | 0:f462be86dc4a | 33 | char data[2] = {0, 1}; |
iut_cachan01 | 0:f462be86dc4a | 34 | _i2c.write(_address, data, 2); |
iut_cachan01 | 0:f462be86dc4a | 35 | } |
iut_cachan01 | 0:f462be86dc4a | 36 | |
iut_cachan01 | 0:f462be86dc4a | 37 | void Codeurs::read(int32_t &gauche, int32_t &droit) |
iut_cachan01 | 0:f462be86dc4a | 38 | { |
iut_cachan01 | 0:f462be86dc4a | 39 | int16_t ng, nd; |
iut_cachan01 | 0:f462be86dc4a | 40 | read16(ng, nd); |
iut_cachan01 | 0:f462be86dc4a | 41 | if ((ng > MAX) && (_g16 < -MAX)) { |
iut_cachan01 | 0:f462be86dc4a | 42 | _gauche = _gauche - _g16 + ng - 65536; |
iut_cachan01 | 0:f462be86dc4a | 43 | } else if ((ng < -MAX) && (_g16 > MAX)) { |
iut_cachan01 | 0:f462be86dc4a | 44 | _gauche = _gauche - _g16 + ng + 65536; |
iut_cachan01 | 0:f462be86dc4a | 45 | } else { |
iut_cachan01 | 0:f462be86dc4a | 46 | _gauche = _gauche - _g16 + ng; |
iut_cachan01 | 0:f462be86dc4a | 47 | } |
iut_cachan01 | 0:f462be86dc4a | 48 | _g16 = ng; |
iut_cachan01 | 0:f462be86dc4a | 49 | if ((nd > MAX) && (_d16 < -MAX)) { |
iut_cachan01 | 0:f462be86dc4a | 50 | _droit = _droit - _d16 + nd - 65536; |
iut_cachan01 | 0:f462be86dc4a | 51 | } else if ((nd < -MAX) && (_d16 > MAX)) { |
iut_cachan01 | 0:f462be86dc4a | 52 | _droit = _droit - _d16 + nd + 65536; |
iut_cachan01 | 0:f462be86dc4a | 53 | } else { |
iut_cachan01 | 0:f462be86dc4a | 54 | _droit = _droit - _d16 + nd; |
iut_cachan01 | 0:f462be86dc4a | 55 | } |
iut_cachan01 | 0:f462be86dc4a | 56 | _d16 = nd; |
iut_cachan01 | 0:f462be86dc4a | 57 | gauche = _gauche; |
iut_cachan01 | 0:f462be86dc4a | 58 | droit = _droit; |
iut_cachan01 | 0:f462be86dc4a | 59 | } |
iut_cachan01 | 0:f462be86dc4a | 60 |