Duncan Haldane / mbed_BLDC_driver_KL25Z

Initial commit of motor driver firmware

Dependencies:   mbed-src

Fork of mbed_AS5047P_driver by Duncan Haldane

enc.cpp@1:d68c51a0a706, 2016-09-28 (annotated)

Committer:
dhaldane
Date:
Wed Sep 28 16:59:46 2016 +0000
Revision:
1:d68c51a0a706
Parent:
0:00c56a9c6dee 
Initial commit;

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