Austin Buchan / mbed_BLDC_driver_KL25Z

BLDC control for jumping robot.

Dependencies:   CRC MODSERIAL mbed-dev mbed-rtos

Fork of mbed_BLDC_driver_KL25Z by Duncan Haldane

enc.cpp@0:00c56a9c6dee, 2016-03-05 (annotated)

Committer:
dhaldane
Date:
Sat Mar 05 00:09:45 2016 +0000
Revision:
0:00c56a9c6dee
Child:
1:d68c51a0a706
Driver codes;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dhaldane 0:00c56a9c6dee 1 #include "enc.h"
dhaldane 0:00c56a9c6dee 2
dhaldane 0:00c56a9c6dee 3 enc::enc(PinName mosi, PinName miso, PinName sck, PinName cs) : m_spi(mosi, miso, sck), csl(cs){
dhaldane 0:00c56a9c6dee 4 ushort zpos = 2228; // est. 8/6/2015 1773
dhaldane 0:00c56a9c6dee 5 ushort zhold;
dhaldane 0:00c56a9c6dee 6 m_spi.format(16, 1);
dhaldane 0:00c56a9c6dee 7 m_spi.frequency(50000);
dhaldane 0:00c56a9c6dee 8 csl=1;
dhaldane 0:00c56a9c6dee 9 wait(0.05);
dhaldane 0:00c56a9c6dee 10
dhaldane 0:00c56a9c6dee 11
dhaldane 0:00c56a9c6dee 12 write_spi(0x0019, 0); //SETTINGS2
dhaldane 0:00c56a9c6dee 13 write_spi(0x001D, 0);
dhaldane 0:00c56a9c6dee 14 write_spi(0x0018, 0); //SETTINGS1
dhaldane 0:00c56a9c6dee 15 write_spi(0x0018, 0);
dhaldane 0:00c56a9c6dee 16
dhaldane 0:00c56a9c6dee 17 zhold = zpos >> 6; //8MSB of zero position
dhaldane 0:00c56a9c6dee 18 write_spi(0x0016, 0);
dhaldane 0:00c56a9c6dee 19 write_spi(zhold, 0);
dhaldane 0:00c56a9c6dee 20
dhaldane 0:00c56a9c6dee 21 zhold = zpos & 0x003F; //6 LSB of zero position
dhaldane 0:00c56a9c6dee 22 write_spi(0x0017, 0);
dhaldane 0:00c56a9c6dee 23 write_spi(zhold, 0);
dhaldane 0:00c56a9c6dee 24
dhaldane 0:00c56a9c6dee 25 enc_pos.pos = 0;
dhaldane 0:00c56a9c6dee 26 enc_pos.oticks = 0;
dhaldane 0:00c56a9c6dee 27 enc_pos.offset = 0;
dhaldane 0:00c56a9c6dee 28 enc_pos.calibPos = 0;
dhaldane 0:00c56a9c6dee 29 }
dhaldane 0:00c56a9c6dee 30
dhaldane 0:00c56a9c6dee 31 enc::~enc(){
dhaldane 0:00c56a9c6dee 32
dhaldane 0:00c56a9c6dee 33 }
dhaldane 0:00c56a9c6dee 34
dhaldane 0:00c56a9c6dee 35 unsigned int enc::cal_state(){
dhaldane 0:00c56a9c6dee 36 return enc_pos.pos;
dhaldane 0:00c56a9c6dee 37 }
dhaldane 0:00c56a9c6dee 38
dhaldane 0:00c56a9c6dee 39 void enc::set_offset(){
dhaldane 0:00c56a9c6dee 40 enc_pos.offset = ams_read();
dhaldane 0:00c56a9c6dee 41 }
dhaldane 0:00c56a9c6dee 42
dhaldane 0:00c56a9c6dee 43 ushort enc::write_spi(ushort reg, u8 rw){
dhaldane 0:00c56a9c6dee 44 ushort data;
dhaldane 0:00c56a9c6dee 45 reg = reg | rw << 14;
dhaldane 0:00c56a9c6dee 46 reg = reg | par(reg);
dhaldane 0:00c56a9c6dee 47 csl=0;
dhaldane 0:00c56a9c6dee 48 data = m_spi.write(reg);
dhaldane 0:00c56a9c6dee 49 csl=1;
dhaldane 0:00c56a9c6dee 50 wait(0.00001);
dhaldane 0:00c56a9c6dee 51 return data;
dhaldane 0:00c56a9c6dee 52
dhaldane 0:00c56a9c6dee 53 }
dhaldane 0:00c56a9c6dee 54
dhaldane 0:00c56a9c6dee 55 ushort enc::read_spi(ushort reg){
dhaldane 0:00c56a9c6dee 56 ushort data;
dhaldane 0:00c56a9c6dee 57 reg = reg | 1 << 14;
dhaldane 0:00c56a9c6dee 58 reg = reg | par(reg);
dhaldane 0:00c56a9c6dee 59 write_spi(reg, 1);
dhaldane 0:00c56a9c6dee 60 data = write_spi(0x0000, 1);
dhaldane 0:00c56a9c6dee 61 return data;
dhaldane 0:00c56a9c6dee 62 }
dhaldane 0:00c56a9c6dee 63
dhaldane 0:00c56a9c6dee 64 /*!
dhaldane 0:00c56a9c6dee 65 *****************************************************************************
dhaldane 0:00c56a9c6dee 66 * Calculate even parity of a 16 bit unsigned integer
dhaldane 0:00c56a9c6dee 67 *
dhaldane 0:00c56a9c6dee 68 * This function is used by the SPI interface to calculate the even parity
dhaldane 0:00c56a9c6dee 69 * of the data which will be sent via SPI to the encoder.
dhaldane 0:00c56a9c6dee 70 *
dhaldane 0:00c56a9c6dee 71 * \param[in] value : 16 bit unsigned integer whose parity shall be calculated
dhaldane 0:00c56a9c6dee 72 *
dhaldane 0:00c56a9c6dee 73 * \return : Even parity
dhaldane 0:00c56a9c6dee 74 *
dhaldane 0:00c56a9c6dee 75 *****************************************************************************
dhaldane 0:00c56a9c6dee 76 */
dhaldane 0:00c56a9c6dee 77 ushort enc::par(ushort value)
dhaldane 0:00c56a9c6dee 78 {
dhaldane 0:00c56a9c6dee 79 u8 cnt = 0;
dhaldane 0:00c56a9c6dee 80 u8 i;
dhaldane 0:00c56a9c6dee 81
dhaldane 0:00c56a9c6dee 82 for (i = 0; i < 16; i++)
dhaldane 0:00c56a9c6dee 83 {
dhaldane 0:00c56a9c6dee 84 if (value & 0x1)
dhaldane 0:00c56a9c6dee 85 {
dhaldane 0:00c56a9c6dee 86 cnt++;
dhaldane 0:00c56a9c6dee 87 }
dhaldane 0:00c56a9c6dee 88 value >>= 1;
dhaldane 0:00c56a9c6dee 89 }
dhaldane 0:00c56a9c6dee 90 cnt = cnt & 0x1;
dhaldane 0:00c56a9c6dee 91 return cnt << 15;
dhaldane 0:00c56a9c6dee 92 }
dhaldane 0:00c56a9c6dee 93
dhaldane 0:00c56a9c6dee 94 void enc::update_pos(){
dhaldane 0:00c56a9c6dee 95 enc_pos.pos = ams_read();
dhaldane 0:00c56a9c6dee 96 }
dhaldane 0:00c56a9c6dee 97
dhaldane 0:00c56a9c6dee 98 unsigned int enc::ams_read() {
dhaldane 0:00c56a9c6dee 99 unsigned int enc_data;
dhaldane 0:00c56a9c6dee 100
dhaldane 0:00c56a9c6dee 101 enc_data = read_spi(0x3FFE);
dhaldane 0:00c56a9c6dee 102 read_spi(0x0001);
dhaldane 0:00c56a9c6dee 103
dhaldane 0:00c56a9c6dee 104 return enc_data & 0x3FFF;
dhaldane 0:00c56a9c6dee 105 }