Daoyu_Sofiane Yao_Belouka / codeur

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

codeurs.cpp@0:a1e0f644e2cb, 2021-04-16 (annotated)

Committer:
daoyu_sofiane
Date:
Fri Apr 16 09:27:25 2021 +0000
Revision:
0:a1e0f644e2cb
Gyropode Avancement

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daoyu_sofiane 0:a1e0f644e2cb 1 #include "codeurs.h"
daoyu_sofiane 0:a1e0f644e2cb 2
daoyu_sofiane 0:a1e0f644e2cb 3 static const int16_t MAX = 16384;
daoyu_sofiane 0:a1e0f644e2cb 4
daoyu_sofiane 0:a1e0f644e2cb 5 Codeurs::Codeurs(PinName sda, PinName scl, int address) : _i2c(sda, scl), _address(address)
daoyu_sofiane 0:a1e0f644e2cb 6 {
daoyu_sofiane 0:a1e0f644e2cb 7 _gauche = 0;
daoyu_sofiane 0:a1e0f644e2cb 8 _droit = 0;
daoyu_sofiane 0:a1e0f644e2cb 9 _g16 = 0;
daoyu_sofiane 0:a1e0f644e2cb 10 _d16 = 0;
daoyu_sofiane 0:a1e0f644e2cb 11 }
daoyu_sofiane 0:a1e0f644e2cb 12
daoyu_sofiane 0:a1e0f644e2cb 13 bool Codeurs::test()
daoyu_sofiane 0:a1e0f644e2cb 14 {
daoyu_sofiane 0:a1e0f644e2cb 15 char data = 5;
daoyu_sofiane 0:a1e0f644e2cb 16 _i2c.write(_address, &data, 1);
daoyu_sofiane 0:a1e0f644e2cb 17 _i2c.read(_address, &data, 1);
daoyu_sofiane 0:a1e0f644e2cb 18 return (data == 0x1F);
daoyu_sofiane 0:a1e0f644e2cb 19 }
daoyu_sofiane 0:a1e0f644e2cb 20
daoyu_sofiane 0:a1e0f644e2cb 21 void Codeurs::read16(int16_t &gauche, int16_t &droit)
daoyu_sofiane 0:a1e0f644e2cb 22 {
daoyu_sofiane 0:a1e0f644e2cb 23 char data[4];
daoyu_sofiane 0:a1e0f644e2cb 24 data[0] = 1;
daoyu_sofiane 0:a1e0f644e2cb 25 _i2c.write(_address, data, 1);
daoyu_sofiane 0:a1e0f644e2cb 26 _i2c.read(_address, data, 4);
daoyu_sofiane 0:a1e0f644e2cb 27 gauche = data[0]<<8 | (data[1]&0xFF);
daoyu_sofiane 0:a1e0f644e2cb 28 droit = data[2]<<8 | (data[3]&0xFF);;
daoyu_sofiane 0:a1e0f644e2cb 29 }
daoyu_sofiane 0:a1e0f644e2cb 30
daoyu_sofiane 0:a1e0f644e2cb 31 void Codeurs::reset()
daoyu_sofiane 0:a1e0f644e2cb 32 {
daoyu_sofiane 0:a1e0f644e2cb 33 char data[2] = {0, 1};
daoyu_sofiane 0:a1e0f644e2cb 34 _i2c.write(_address, data, 2);
daoyu_sofiane 0:a1e0f644e2cb 35 _gauche = 0;
daoyu_sofiane 0:a1e0f644e2cb 36 _droit = 0;
daoyu_sofiane 0:a1e0f644e2cb 37 _g16 = 0;
daoyu_sofiane 0:a1e0f644e2cb 38 _d16 = 0;
daoyu_sofiane 0:a1e0f644e2cb 39 }
daoyu_sofiane 0:a1e0f644e2cb 40
daoyu_sofiane 0:a1e0f644e2cb 41 void Codeurs::read(int32_t &gauche, int32_t &droit)
daoyu_sofiane 0:a1e0f644e2cb 42 {
daoyu_sofiane 0:a1e0f644e2cb 43 int16_t ng, nd;
daoyu_sofiane 0:a1e0f644e2cb 44 read16(ng, nd);
daoyu_sofiane 0:a1e0f644e2cb 45 if ((ng > MAX) && (_g16 < -MAX)) {
daoyu_sofiane 0:a1e0f644e2cb 46 _gauche = _gauche - _g16 + ng - 65536;
daoyu_sofiane 0:a1e0f644e2cb 47 } else if ((ng < -MAX) && (_g16 > MAX)) {
daoyu_sofiane 0:a1e0f644e2cb 48 _gauche = _gauche - _g16 + ng + 65536;
daoyu_sofiane 0:a1e0f644e2cb 49 } else {
daoyu_sofiane 0:a1e0f644e2cb 50 _gauche = _gauche - _g16 + ng;
daoyu_sofiane 0:a1e0f644e2cb 51 }
daoyu_sofiane 0:a1e0f644e2cb 52 _g16 = ng;
daoyu_sofiane 0:a1e0f644e2cb 53 if ((nd > MAX) && (_d16 < -MAX)) {
daoyu_sofiane 0:a1e0f644e2cb 54 _droit = _droit - _d16 + nd - 65536;
daoyu_sofiane 0:a1e0f644e2cb 55 } else if ((nd < -MAX) && (_d16 > MAX)) {
daoyu_sofiane 0:a1e0f644e2cb 56 _droit = _droit - _d16 + nd + 65536;
daoyu_sofiane 0:a1e0f644e2cb 57 } else {
daoyu_sofiane 0:a1e0f644e2cb 58 _droit = _droit - _d16 + nd;
daoyu_sofiane 0:a1e0f644e2cb 59 }
daoyu_sofiane 0:a1e0f644e2cb 60 _d16 = nd;
daoyu_sofiane 0:a1e0f644e2cb 61 gauche = _gauche;
daoyu_sofiane 0:a1e0f644e2cb 62 droit = _droit;
daoyu_sofiane 0:a1e0f644e2cb 63 }
daoyu_sofiane 0:a1e0f644e2cb 64