Александр Селифонов / AS5600

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?

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