324
AS5600.cpp@1:e4b72c71ca47, 2020-05-21 (annotated)
- Committer:
- turumputum
- Date:
- Thu May 21 19:09:28 2020 +0000
- Revision:
- 1:e4b72c71ca47
- Parent:
- 0:ec69fa9832e5
223131
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gr66 | 0:ec69fa9832e5 | 1 | // AS5600 Verson 1.3 |
gr66 | 0:ec69fa9832e5 | 2 | // 20/03/2015 GR : fonction limitant la dynamique autour de zero à -max , +max |
gr66 | 0:ec69fa9832e5 | 3 | #include "AS5600.h" |
gr66 | 0:ec69fa9832e5 | 4 | //#include "mbed.h" |
gr66 | 0:ec69fa9832e5 | 5 | |
gr66 | 0:ec69fa9832e5 | 6 | |
gr66 | 0:ec69fa9832e5 | 7 | |
gr66 | 0:ec69fa9832e5 | 8 | AS5600::AS5600(PinName sda, PinName scl) : i2c(sda,scl) |
gr66 | 0:ec69fa9832e5 | 9 | { |
gr66 | 0:ec69fa9832e5 | 10 | addresse=0x36; |
turumputum | 1:e4b72c71ca47 | 11 | i2c.frequency(40000); |
gr66 | 0:ec69fa9832e5 | 12 | } |
turumputum | 1:e4b72c71ca47 | 13 | |
turumputum | 1:e4b72c71ca47 | 14 | int AS5600::sensorState() |
turumputum | 1:e4b72c71ca47 | 15 | { |
turumputum | 1:e4b72c71ca47 | 16 | char cmd=0x0B,data=0,value=0; |
turumputum | 1:e4b72c71ca47 | 17 | data=this->read(cmd); |
turumputum | 1:e4b72c71ca47 | 18 | //value=(char) (data & 0x20) >> 5 ; |
turumputum | 1:e4b72c71ca47 | 19 | |
turumputum | 1:e4b72c71ca47 | 20 | return data; |
turumputum | 1:e4b72c71ca47 | 21 | } |
turumputum | 1:e4b72c71ca47 | 22 | |
turumputum | 1:e4b72c71ca47 | 23 | |
gr66 | 0:ec69fa9832e5 | 24 | int AS5600::isMagnetPresent() |
gr66 | 0:ec69fa9832e5 | 25 | { |
gr66 | 0:ec69fa9832e5 | 26 | char cmd=0x0B,data=0,value=0; |
gr66 | 0:ec69fa9832e5 | 27 | data=this->read(cmd); |
gr66 | 0:ec69fa9832e5 | 28 | value=(char) (data & 0x20) >> 5 ; |
gr66 | 0:ec69fa9832e5 | 29 | |
gr66 | 0:ec69fa9832e5 | 30 | return value; |
gr66 | 0:ec69fa9832e5 | 31 | } |
gr66 | 0:ec69fa9832e5 | 32 | |
gr66 | 0:ec69fa9832e5 | 33 | |
gr66 | 0:ec69fa9832e5 | 34 | int AS5600::getAngleAbsolute() |
gr66 | 0:ec69fa9832e5 | 35 | { |
gr66 | 0:ec69fa9832e5 | 36 | char cmd1=0x0E,cmd2=0x0F; |
gr66 | 0:ec69fa9832e5 | 37 | int data=0; |
gr66 | 0:ec69fa9832e5 | 38 | |
gr66 | 0:ec69fa9832e5 | 39 | data=this->read(cmd1) << 8; |
gr66 | 0:ec69fa9832e5 | 40 | data=data+this->read(cmd2); |
gr66 | 0:ec69fa9832e5 | 41 | |
gr66 | 0:ec69fa9832e5 | 42 | return data; |
gr66 | 0:ec69fa9832e5 | 43 | } |
gr66 | 0:ec69fa9832e5 | 44 | |
turumputum | 1:e4b72c71ca47 | 45 | int AS5600::getMagnitude() |
turumputum | 1:e4b72c71ca47 | 46 | { |
turumputum | 1:e4b72c71ca47 | 47 | char cmd1=0x1B,cmd2=0x1C; |
turumputum | 1:e4b72c71ca47 | 48 | int data=0; |
turumputum | 1:e4b72c71ca47 | 49 | |
turumputum | 1:e4b72c71ca47 | 50 | data=this->read(cmd1) << 8; |
turumputum | 1:e4b72c71ca47 | 51 | data=data+this->read(cmd2); |
turumputum | 1:e4b72c71ca47 | 52 | |
turumputum | 1:e4b72c71ca47 | 53 | return data; |
turumputum | 1:e4b72c71ca47 | 54 | } |
turumputum | 1:e4b72c71ca47 | 55 | |
gr66 | 0:ec69fa9832e5 | 56 | int AS5600::getAngleRelative() |
gr66 | 0:ec69fa9832e5 | 57 | { |
gr66 | 0:ec69fa9832e5 | 58 | return ((this->getAngleAbsolute() + (2047 - relative_zero)) % 4096) - 2047; |
gr66 | 0:ec69fa9832e5 | 59 | } |
gr66 | 0:ec69fa9832e5 | 60 | |
gr66 | 0:ec69fa9832e5 | 61 | |
gr66 | 0:ec69fa9832e5 | 62 | float AS5600::getAngleDegrees() |
gr66 | 0:ec69fa9832e5 | 63 | { |
gr66 | 0:ec69fa9832e5 | 64 | return ((float)this->getAngleRelative() * 180) / 2048 ; |
gr66 | 0:ec69fa9832e5 | 65 | } |
gr66 | 0:ec69fa9832e5 | 66 | |
gr66 | 0:ec69fa9832e5 | 67 | void AS5600::setZero() |
gr66 | 0:ec69fa9832e5 | 68 | { |
gr66 | 0:ec69fa9832e5 | 69 | relative_zero=0; |
gr66 | 0:ec69fa9832e5 | 70 | relative_zero=this->getAngleAbsolute(); |
gr66 | 0:ec69fa9832e5 | 71 | } |
gr66 | 0:ec69fa9832e5 | 72 | |
gr66 | 0:ec69fa9832e5 | 73 | |
gr66 | 0:ec69fa9832e5 | 74 | char AS5600::read(char address) |
gr66 | 0:ec69fa9832e5 | 75 | { |
gr66 | 0:ec69fa9832e5 | 76 | char retval; |
gr66 | 0:ec69fa9832e5 | 77 | i2c.write(addresse * 2, &address, 1); |
gr66 | 0:ec69fa9832e5 | 78 | i2c.read(addresse * 2, &retval, 1); |
gr66 | 0:ec69fa9832e5 | 79 | return retval; |
gr66 | 0:ec69fa9832e5 | 80 | } |
gr66 | 0:ec69fa9832e5 | 81 | |
gr66 | 0:ec69fa9832e5 | 82 | void AS5600::init() |
gr66 | 0:ec69fa9832e5 | 83 | { |
gr66 | 0:ec69fa9832e5 | 84 | |
gr66 | 0:ec69fa9832e5 | 85 | for (int i=0; i<100; i++) |
gr66 | 0:ec69fa9832e5 | 86 | this->setZero(); |
gr66 | 0:ec69fa9832e5 | 87 | |
gr66 | 0:ec69fa9832e5 | 88 | } |
gr66 | 0:ec69fa9832e5 | 89 | |
gr66 | 0:ec69fa9832e5 | 90 | float AS5600::getAngleMinMax(float angleMax) |
gr66 | 0:ec69fa9832e5 | 91 | { |
gr66 | 0:ec69fa9832e5 | 92 | static unsigned char etat=0; // etat de l'automate |
gr66 | 0:ec69fa9832e5 | 93 | static int anglePrec=0; // angle precedent |
gr66 | 0:ec69fa9832e5 | 94 | float angle=0; // angle courant |
gr66 | 0:ec69fa9832e5 | 95 | float angleX=0; //angle en sortie borne |
gr66 | 0:ec69fa9832e5 | 96 | // |
gr66 | 0:ec69fa9832e5 | 97 | angle=this->getAngleDegrees(); // lecture de l'angle courant |
gr66 | 0:ec69fa9832e5 | 98 | switch(etat) { |
gr66 | 0:ec69fa9832e5 | 99 | case 0 : // angle compris entre min et max |
gr66 | 0:ec69fa9832e5 | 100 | angleX = angle; |
gr66 | 0:ec69fa9832e5 | 101 | if(angle > angleMax) etat = 2; |
gr66 | 0:ec69fa9832e5 | 102 | if(angle < -angleMax) etat = 1; |
gr66 | 0:ec69fa9832e5 | 103 | break; |
gr66 | 0:ec69fa9832e5 | 104 | case 1 : // angle inferieur a -max |
gr66 | 0:ec69fa9832e5 | 105 | angleX = -angleMax; |
gr66 | 0:ec69fa9832e5 | 106 | if((anglePrec <= -angleMax) && (angle >= -angleMax) && (angle < 0)) etat = 0; |
gr66 | 0:ec69fa9832e5 | 107 | break; |
gr66 | 0:ec69fa9832e5 | 108 | case 2 : |
gr66 | 0:ec69fa9832e5 | 109 | angleX = angleMax; |
gr66 | 0:ec69fa9832e5 | 110 | if((anglePrec >= angleMax) && (angle <= angleMax) && (angle > 0))etat = 0; |
gr66 | 0:ec69fa9832e5 | 111 | break; |
gr66 | 0:ec69fa9832e5 | 112 | default : |
gr66 | 0:ec69fa9832e5 | 113 | break; |
gr66 | 0:ec69fa9832e5 | 114 | } |
gr66 | 0:ec69fa9832e5 | 115 | anglePrec=angle; |
gr66 | 0:ec69fa9832e5 | 116 | return angleX; |
gr66 | 0:ec69fa9832e5 | 117 | } |