曾 宗圓 / Mbed 2 deprecated LSM9DS1_project

Implement sensor fusion algorithm based on LSM9DS1 IMU.

Dependencies:   mbed

Fork of LSM9DS1_project by 曾 宗圓

AS5145.cpp@2:c889fecf9afe, 2018-04-24 (annotated)

Committer:
middleyuan
Date:
Tue Apr 24 15:13:58 2018 +0000
Revision:
2:c889fecf9afe
Parent:
1:b1562831bbaf 
SensorFusion

Who changed what in which revision?

UserRevisionLine numberNew contents of line
middleyuan 1:b1562831bbaf 1 #include"AS5145.h"
middleyuan 1:b1562831bbaf 2
middleyuan 1:b1562831bbaf 3 SPI spi(PB_15,PB_14,PB_13);
middleyuan 1:b1562831bbaf 4 SPI spi2(PC_12,PC_11,PC_10);
middleyuan 1:b1562831bbaf 5 DigitalOut CS(PB_1);
middleyuan 2:c889fecf9afe 6 DigitalOut CS2(PA_13);
middleyuan 1:b1562831bbaf 7
middleyuan 1:b1562831bbaf 8 int position[2] = {0,0};
middleyuan 1:b1562831bbaf 9 int position_old[2] = {0,0};
middleyuan 1:b1562831bbaf 10 short low_byte = 0x00;
middleyuan 1:b1562831bbaf 11 short high_byte = 0x00;
middleyuan 1:b1562831bbaf 12
middleyuan 1:b1562831bbaf 13 bool AS5145_begin(void)
middleyuan 1:b1562831bbaf 14 {
middleyuan 1:b1562831bbaf 15 spi.format(8,3);
middleyuan 1:b1562831bbaf 16 spi.frequency(1000000);
middleyuan 1:b1562831bbaf 17 CS = 1;
middleyuan 1:b1562831bbaf 18 spi2.format(8,3);
middleyuan 1:b1562831bbaf 19 spi2.frequency(1000000);
middleyuan 1:b1562831bbaf 20 CS2 = 1;
middleyuan 1:b1562831bbaf 21 return true;
middleyuan 1:b1562831bbaf 22 }
middleyuan 1:b1562831bbaf 23 void ReadValue(void)
middleyuan 1:b1562831bbaf 24 {
middleyuan 1:b1562831bbaf 25 CS = 0;
middleyuan 1:b1562831bbaf 26 low_byte = spi.write(0x00);
middleyuan 1:b1562831bbaf 27 high_byte = spi.write(0x00);
middleyuan 1:b1562831bbaf 28 wait_us(1);
middleyuan 1:b1562831bbaf 29 CS = 1;
middleyuan 1:b1562831bbaf 30 position[0] = (low_byte << 8 | high_byte) >> 3;
middleyuan 1:b1562831bbaf 31 CS2 = 0;
middleyuan 1:b1562831bbaf 32 low_byte = spi2.write(0x00);
middleyuan 1:b1562831bbaf 33 high_byte = spi2.write(0x00);
middleyuan 1:b1562831bbaf 34 wait_us(1);
middleyuan 1:b1562831bbaf 35 CS2 = 1;
middleyuan 1:b1562831bbaf 36 position[1] = (low_byte << 8 | high_byte) >> 3;
middleyuan 1:b1562831bbaf 37 if(position[0] > 4096)
middleyuan 1:b1562831bbaf 38 position[0] = position_old[0];
middleyuan 1:b1562831bbaf 39 position_old[0] = position[0];
middleyuan 1:b1562831bbaf 40 if(position[1] > 4096)
middleyuan 1:b1562831bbaf 41 position[1] = position_old[1];
middleyuan 1:b1562831bbaf 42 position_old[1] = position[1];
middleyuan 1:b1562831bbaf 43 }