ver1

Dependencies:   MX28 mbed

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;
}