IUT CACHAN / Mbed 2 deprecated codeursMecatro

Codeurs MECATRO

Dependencies:   mbed

codeurs.cpp@1:ac5eb2de660e, 2021-03-25 (annotated)

Committer:
iut_cachan01
Date:
Thu Mar 25 07:12:17 2021 +0000
Revision:
1:ac5eb2de660e
Parent:
0:f462be86dc4a 
Correction methode reset. V1.1

Who changed what in which revision?

UserRevisionLine numberNew 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 1:ac5eb2de660e 35 _gauche = 0;
iut_cachan01 1:ac5eb2de660e 36 _droit = 0;
iut_cachan01 1:ac5eb2de660e 37 _g16 = 0;
iut_cachan01 1:ac5eb2de660e 38 _d16 = 0;
iut_cachan01 0:f462be86dc4a 39 }
iut_cachan01 0:f462be86dc4a 40
iut_cachan01 0:f462be86dc4a 41 void Codeurs::read(int32_t &gauche, int32_t &droit)
iut_cachan01 0:f462be86dc4a 42 {
iut_cachan01 0:f462be86dc4a 43 int16_t ng, nd;
iut_cachan01 0:f462be86dc4a 44 read16(ng, nd);
iut_cachan01 0:f462be86dc4a 45 if ((ng > MAX) && (_g16 < -MAX)) {
iut_cachan01 0:f462be86dc4a 46 _gauche = _gauche - _g16 + ng - 65536;
iut_cachan01 0:f462be86dc4a 47 } else if ((ng < -MAX) && (_g16 > MAX)) {
iut_cachan01 0:f462be86dc4a 48 _gauche = _gauche - _g16 + ng + 65536;
iut_cachan01 0:f462be86dc4a 49 } else {
iut_cachan01 0:f462be86dc4a 50 _gauche = _gauche - _g16 + ng;
iut_cachan01 0:f462be86dc4a 51 }
iut_cachan01 0:f462be86dc4a 52 _g16 = ng;
iut_cachan01 0:f462be86dc4a 53 if ((nd > MAX) && (_d16 < -MAX)) {
iut_cachan01 0:f462be86dc4a 54 _droit = _droit - _d16 + nd - 65536;
iut_cachan01 0:f462be86dc4a 55 } else if ((nd < -MAX) && (_d16 > MAX)) {
iut_cachan01 0:f462be86dc4a 56 _droit = _droit - _d16 + nd + 65536;
iut_cachan01 0:f462be86dc4a 57 } else {
iut_cachan01 0:f462be86dc4a 58 _droit = _droit - _d16 + nd;
iut_cachan01 0:f462be86dc4a 59 }
iut_cachan01 0:f462be86dc4a 60 _d16 = nd;
iut_cachan01 0:f462be86dc4a 61 gauche = _gauche;
iut_cachan01 0:f462be86dc4a 62 droit = _droit;
iut_cachan01 0:f462be86dc4a 63 }
iut_cachan01 0:f462be86dc4a 64