By pOOPOO
Fork of LSM9DS1_project_5_zerotorque by
encoder.cpp
- Committer:
- open4416
- Date:
- 2018-11-05
- Revision:
- 10:6a9de32601b1
- Parent:
- 0:c23e915f255b
File content as of revision 10:6a9de32601b1:
#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;
}
SHENG-HEN HSIEH
