a4960.cpp

00001 #include "a4960.h"
00002 
00003 a4960::a4960() : SPI(NRF_SPI1), _cs_pin(P0_10), _blink_pin(P0_4), _PWM_pin(P0_15)
00004 {
00005     PWM_freq = 20000.0;
00006     PWM_duty = 1;
00007     motor_started = false;
00008 
00009     _config[0] = (0x0 << 10) | (0x8 << 6) | (0x14); // 50 us comm blank time, 2.4 us blank time, 200 ns deadtime
00010     _config[1] = (0x7 << 6) | (0x0); // Vri = 100% Vref, Vdsth = 800mV
00011     _config[2] = (0x10); // 35.6 us current control time
00012     _config[3] = (0x0 << 8) | (0x5 << 4) | (0x4); // current limited, 50% current for hold, 18ms hold time
00013     _config[4] =  (0x15 << 4) | (0x4); // 0.8ms min comm time, 24 ms start comm time
00014     _config[5] = (0x0 << 8) | (0x8 << 4) | (0x0); // 16.875deg phase adv, 100% ramp current, 0.4ms ramp rate
00015     _config[6] = (0x0); // fault detection all on
00016     _config[7] = (0x0 << 10) | (0x4 << 7) | (0x0 << 6) | (0x0 << 4) | (0x1 << 3) | (0x0 << 2) | (0x0 << 1) | (0x0);
00017     // auto BEMF hyst, 3.2us zx det window, no stop on fail, DIAG pin = fault, restart on loss of sync, brake off, forward, coast
00018     
00019     SPI_init();
00020 }
00021 
00022 void a4960::SPI_init()
00023 {
00024     // initialize pins
00025     _cs_pin = 1;
00026     _blink_pin = 1;
00027     _PWM_pin.period(1/PWM_freq);
00028     _PWM_pin.write(PWM_duty);
00029     // initialize SPI connection
00030     SPI.begin(P0_8, P0_9, P0_11);//SCK, MOSI, MOSI
00031     // set registers to default config
00032     for (int i = 0; i < 8; i++) {
00033         write_to_a4960(i, _config[i]);
00034     }
00035     // turn off pin to indicate initialization complete
00036     _blink_pin = 0;
00037 }
00038 void a4960::write_to_a4960(uint8_t addr, uint16_t msg)
00039 {
00040     // split 16-bit message to a4960 into two 8-bit messages
00041     uint8_t ms_half;
00042     uint8_t ls_half;
00043 
00044     // ---- 1st message (bits 15 - 8)
00045     // 3 bit address
00046     ms_half = addr << 5;
00047     // 12 bit message
00048     // shift message to get rid of 8 LSB on the end and OR
00049     // the write bit and the
00050     // 4 remaining bits into the first-half msg
00051     ms_half = ms_half | 1 << 4 | (uint8_t)(msg>>8);
00052     // ---- 2nd message (bits 7 - 0)
00053     ls_half = (uint8_t)(msg);
00054 
00055     // pull to active low
00056     _cs_pin = 0;
00057     wait_us(200);
00058 
00059     // transfer the two messages halves, MSB first
00060     SPI.transfer(ms_half);
00061     SPI.transfer(ls_half);
00062 
00063     // wait and pull CS line back to inactive high
00064     wait_us(200);
00065     _cs_pin = 1;
00066 }
00067 
00068 void a4960::write_run(void)
00069 {
00070     write_to_a4960(7, _config[7] | 1);
00071 }
00072 
00073 void a4960::write_brake(void)
00074 {
00075     write_to_a4960(7, _config[7]);
00076 }
00077 
00078 
00079 
00080 
00081 
00082