AS5600.cpp

00001 // AS5600 Verson 1.3
00002 // 20/03/2015  GR : fonction limitant la dynamique autour de zero à -max , +max
00003 #include "AS5600.h"
00004 //#include "mbed.h"
00005 
00006 
00007 
00008 AS5600::AS5600(PinName sda, PinName scl) : i2c(sda,scl)
00009 {
00010     addresse=0x36;
00011     i2c.frequency(400000);
00012 }
00013 int AS5600::isMagnetPresent()
00014 {
00015     char cmd=0x0B,data=0,value=0;
00016     data=this->read(cmd);
00017     value=(char) (data & 0x20) >> 5 ;
00018 
00019     return value;
00020 }
00021 
00022 
00023 int AS5600::getAngleAbsolute()
00024 {
00025     char cmd1=0x0E,cmd2=0x0F;
00026     int data=0;
00027 
00028     data=this->read(cmd1) << 8;
00029     data=data+this->read(cmd2);
00030 
00031     return data;
00032 }
00033 
00034 int AS5600::getAngleRelative()
00035 {
00036     return ((this->getAngleAbsolute() + (2047 - relative_zero)) % 4096) - 2047;
00037 }
00038 
00039 
00040 float AS5600::getAngleDegrees()
00041 {
00042     return ((float)this->getAngleRelative() * 180) / 2048 ;
00043 }
00044 
00045 void AS5600::setZero()
00046 {
00047     relative_zero=0;
00048     relative_zero=this->getAngleAbsolute();
00049 }
00050 
00051 
00052 char AS5600::read(char address)
00053 {
00054     char retval;
00055     i2c.write(addresse * 2, &address, 1);
00056     i2c.read(addresse * 2, &retval, 1);
00057     return retval;
00058 }
00059 
00060 void AS5600::init()
00061 {
00062 
00063     for (int i=0; i<100; i++)
00064         this->setZero();
00065 
00066 }
00067 
00068 float AS5600::getAngleMinMax(float angleMax)
00069 {
00070     static unsigned char etat=0;           // etat de l'automate
00071     static int anglePrec=0;                // angle precedent
00072     float angle=0;                         // angle courant
00073     float angleX=0;                        //angle en sortie borne
00074     //
00075     angle=this->getAngleDegrees();         // lecture de l'angle courant
00076     switch(etat) {
00077         case 0 :    // angle compris entre min et max
00078             angleX = angle;
00079             if(angle > angleMax)  etat = 2;
00080             if(angle < -angleMax) etat = 1;
00081             break;
00082         case 1 :    // angle inferieur a -max
00083             angleX = -angleMax;
00084             if((anglePrec <= -angleMax) && (angle >= -angleMax) && (angle < 0)) etat = 0;
00085             break;
00086         case 2 :
00087             angleX = angleMax;
00088             if((anglePrec >=  angleMax) && (angle <= angleMax) && (angle > 0))etat = 0;
00089             break;
00090         default :
00091             break;
00092     }
00093     anglePrec=angle;
00094     return angleX;
00095 }