Chen Wei Ting / Mbed 2 deprecated LSM9DS1_project_3

read encoder

Dependencies:   mbed

encoder.cpp@1:2f2a74337b77, 2018-07-31 (annotated)

Committer:
JJting
Date:
Tue Jul 31 13:24:56 2018 +0000
Revision:
1:2f2a74337b77
Parent:
0:f2657b94ea70
Child:
2:33062fc279ca
20180731ver2;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JJting 1:2f2a74337b77 1 #include "mbed.h"
JJting 0:f2657b94ea70 2 #include "encoder.h"
JJting 0:f2657b94ea70 3
JJting 1:2f2a74337b77 4 DigitalOut encoder_cs(D9);
JJting 1:2f2a74337b77 5 SPI spi_encoder(D11, D12, D13); // mosi, miso, sclk
JJting 1:2f2a74337b77 6
JJting 1:2f2a74337b77 7 unsigned short encoder_value = 0;
JJting 1:2f2a74337b77 8 unsigned short angle = 0;
JJting 1:2f2a74337b77 9 unsigned short angle_old;
JJting 1:2f2a74337b77 10 int angle_dif;
JJting 1:2f2a74337b77 11 int a_dif;
JJting 1:2f2a74337b77 12 int Angle;
JJting 1:2f2a74337b77 13 unsigned short k = 0;
JJting 1:2f2a74337b77 14 unsigned short angle_init;
JJting 1:2f2a74337b77 15
JJting 1:2f2a74337b77 16 void init_encoder()
JJting 1:2f2a74337b77 17 {
JJting 1:2f2a74337b77 18 encoder_cs = 0; // Select the device by seting chip select low
JJting 1:2f2a74337b77 19 encoder_value = spi_encoder.write(0x00);
JJting 1:2f2a74337b77 20 angle_init = encoder_value >> 3;
JJting 1:2f2a74337b77 21 encoder_cs = 1; // Deselect the device
JJting 1:2f2a74337b77 22 }
JJting 1:2f2a74337b77 23
JJting 1:2f2a74337b77 24 void init_SPI_encoder()
JJting 1:2f2a74337b77 25 {
JJting 1:2f2a74337b77 26 spi_encoder.format(16,3);
JJting 1:2f2a74337b77 27 spi_encoder.frequency(1000000); // 1MHz clock rate
JJting 1:2f2a74337b77 28 }
JJting 1:2f2a74337b77 29
JJting 1:2f2a74337b77 30 void angle_measure()
JJting 1:2f2a74337b77 31 {
JJting 1:2f2a74337b77 32 encoder_cs = 0; // Select the device by seting chip select low
JJting 1:2f2a74337b77 33 encoder_value = spi_encoder.write(0x00);
JJting 1:2f2a74337b77 34 angle = encoder_value >> 3;
JJting 1:2f2a74337b77 35 encoder_cs = 1; // Deselect the device
JJting 1:2f2a74337b77 36
JJting 1:2f2a74337b77 37 if (k == 0)
JJting 1:2f2a74337b77 38 {
JJting 1:2f2a74337b77 39 angle_dif = angle_count(angle, angle_init);
JJting 1:2f2a74337b77 40 Angle = angle_init + angle_dif;
JJting 1:2f2a74337b77 41 angle_old = angle;
JJting 1:2f2a74337b77 42 k++;
JJting 1:2f2a74337b77 43 }
JJting 1:2f2a74337b77 44 else
JJting 1:2f2a74337b77 45 {
JJting 1:2f2a74337b77 46 angle_dif = angle_count(angle, angle_old);
JJting 1:2f2a74337b77 47 Angle = Angle + angle_dif;
JJting 1:2f2a74337b77 48 angle_old = angle;
JJting 1:2f2a74337b77 49 }
JJting 1:2f2a74337b77 50 }
JJting 1:2f2a74337b77 51
JJting 1:2f2a74337b77 52 int angle_count(unsigned short now,unsigned short old)
JJting 1:2f2a74337b77 53 {
JJting 1:2f2a74337b77 54 a_dif = now - old;
JJting 1:2f2a74337b77 55 if (a_dif > 4096/2)
JJting 1:2f2a74337b77 56 a_dif = -(4096 - a_dif);
JJting 1:2f2a74337b77 57 else if (a_dif < -4096/2)
JJting 1:2f2a74337b77 58 a_dif = 4096 + a_dif;
JJting 1:2f2a74337b77 59 else
JJting 1:2f2a74337b77 60 a_dif = a_dif;
JJting 1:2f2a74337b77 61
JJting 1:2f2a74337b77 62 return a_dif;
JJting 1:2f2a74337b77 63 }