Implement sensor fusion algorithm based on LSM9DS1 IMU.

Dependencies:   mbed

Fork of LSM9DS1_project by 曾 宗圓

AS5145.cpp

Committer:
middleyuan
Date:
2018-04-24
Revision:
2:c889fecf9afe
Parent:
1:b1562831bbaf

File content as of revision 2:c889fecf9afe:

#include"AS5145.h"

SPI spi(PB_15,PB_14,PB_13);
SPI spi2(PC_12,PC_11,PC_10);
DigitalOut CS(PB_1);
DigitalOut CS2(PA_13);

int position[2] = {0,0};
int position_old[2] = {0,0};
short low_byte = 0x00;
short high_byte = 0x00;

bool AS5145_begin(void)
{
    spi.format(8,3);
    spi.frequency(1000000);
    CS = 1;
    spi2.format(8,3);
    spi2.frequency(1000000);
    CS2 = 1;
    return true;
}
void ReadValue(void)
{
    CS = 0;
    low_byte = spi.write(0x00);
    high_byte = spi.write(0x00);
    wait_us(1);
    CS = 1;
    position[0] = (low_byte << 8 | high_byte) >> 3;
    CS2 = 0;
    low_byte = spi2.write(0x00);
    high_byte = spi2.write(0x00);
    wait_us(1);
    CS2 = 1;
    position[1] = (low_byte << 8 | high_byte) >> 3;
    if(position[0] > 4096)
        position[0] = position_old[0];
    position_old[0] = position[0];
    if(position[1] > 4096)
        position[1] = position_old[1];
    position_old[1] = position[1];      
}