Chen Wei Ting
/
system_ID
ver1
encoder.cpp
- Committer:
- JJting
- Date:
- 2018-08-29
- Revision:
- 0:762ec6dc5696
File content as of revision 0:762ec6dc5696:
#include "mbed.h" #include "encoder.h" DigitalOut encoder_cs(D9); SPI spi_encoder(D11, D12, D13); // mosi, miso, sclk unsigned short encoder_value; unsigned short encoder_value_1; unsigned short encoder_value_2; unsigned short angle; unsigned short angle_old; int angle_dif; int a_dif; int Angle = 0; unsigned short k = 0; void init_SPI_encoder() { spi_encoder.format(8,3); spi_encoder.frequency(1200000); // 1.125MHz clock rate } void init_encoder() { encoder_cs = 1; // high:disable the device } void angle_measure() { encoder_cs = 0; // Select the device by seting chip select low encoder_value_1 = spi_encoder.write(0x00); encoder_value_2 = spi_encoder.write(0x00); encoder_value = (encoder_value_1 << 8) | encoder_value_2; angle = encoder_value >> 3; encoder_cs = 1; // Deselect the device // encoder_value = spi_encoder.write(0x00); // angle = encoder_value >> 3; // encoder_cs = 1; // Deselect the device if (k == 0) { Angle = 0; angle_old = angle; k++; } else { angle_dif = angle_count(angle, angle_old); Angle = Angle + angle_dif; angle_old = angle; } } int angle_count(unsigned short now,unsigned short old) { a_dif = now - old; if (a_dif > 4096/2) a_dif = -(4096 - a_dif); else if (a_dif < -4096/2) a_dif = 4096 + a_dif; else a_dif = a_dif; return a_dif; }