Chen Wei Ting
/
LSM9DS1_project_4_torque
read encoder and Dynamixel angle
encoder.cpp
- Committer:
- JJting
- Date:
- 2018-08-04
- Revision:
- 0:2dbbe7d28fab
File content as of revision 0:2dbbe7d28fab:
#include "mbed.h" #include "encoder.h" DigitalOut encoder_cs(D9); //SPI spi_encoder(D11, D12, D13); // mosi, miso, sclk SPI spi_encoder(D4, D5, D3); // mosi, miso, sclk unsigned short encoder_value; unsigned short angle = 0; unsigned short angle_old; int angle_dif; int a_dif; int Angle = 0; unsigned short k = 0; void init_SPI_encoder() { spi_encoder.format(16,3); spi_encoder.frequency(1000000); // 1MHz 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 = 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; }