test upload STMicroelectronics L6470
Diff: L6470.cpp
- Revision:
- 0:26699d53cd2a
- Child:
- 1:bb4bb05904cb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/L6470.cpp Tue Dec 25 02:39:30 2012 +0000 @@ -0,0 +1,211 @@ + +#include "mbed.h" +#include "L6470.h" + +L6470::L6470(PinName mosi, PinName miso, PinName sclk, PinName cs) +:m_spi(mosi, miso, sclk) +,m_cs(cs) { + m_cs = 1; + m_spi.format(8,3); + m_spi.frequency(1000000); + //wait(1); + //pc.printf("reset now\n"); +} + + +unsigned char L6470::send(unsigned char temp){ + m_cs = 0; + //printf("send: %2x",temp); + temp =(unsigned char) m_spi.write(temp); + //printf(" %2x\n",temp); + //wait(0.5); + m_cs = 1; + wait_us(1); + return(temp); +} + +void L6470::send_bytes(unsigned char temp[],int i){ + while(0 < i--){ + temp[i] = send(temp[i]); + } +} + +void L6470::NOP(){ + send(0x00); +} + +void L6470::SetParam(int param,int value){ + int n = (param>>8)/8; + int m = (param>>8)%8; +// pc.printf("palam = %x\n",param); +// pc.printf("n = %x\n",n); +// pc.printf("m = %x\n",m); + if(m==0){ + unsigned char temp[n+1]; + temp[n] = 0x00|(unsigned char)(param&0xFF); + while(0 < n--){ + temp[n]=(unsigned char) (value >> 8*n)&0xFF; + } + send_bytes(temp,sizeof temp/sizeof temp[0]); + }else{ + unsigned char temp[n+2]; + temp[n+1] = 0x00|(unsigned char)(param&0xFF); + temp[n] =(unsigned char) (value >> 8*n)&~(0xff<<m); + while(0 < n--){ + temp[n]=(unsigned char) (value >> 8*n)&0xFF; + } + send_bytes(temp,sizeof temp/sizeof temp[0]); + } +} + + +int L6470::GetParam(int param){ + int value = 0; + int n = (param>>8)/8; + int m = (param>>8)%8; +// pc.printf("palam = %x\n",param); +// pc.printf("n = %x\n",n); +// pc.printf("m = %x\n",m); + if(m==0){ + unsigned char temp[n+1]; + for(int i = 0; i < n+1; i++){ + temp[i]=0; + } + temp[n] = 0x20|(unsigned char)(param&0xFF); + send_bytes(temp,sizeof temp/sizeof temp[0]); + while(0 < n--){ + value |= (int)temp[n] << 8*n; + } + }else{ + n++; + unsigned char temp[n+1]; + for(int i = 0; i < n+2; i++){ + temp[i]=0; + } + temp[n] = 0x20|(unsigned char)(param&0xFF); + send_bytes(temp,sizeof temp/sizeof temp[0]); + while(0 < n--){ + value |= (int)temp[n] << 8*n; + } + } +return(value); +} + + + +void L6470::Run(unsigned char dir,int spd){ + unsigned char temp[4]; + temp[3] = 0x50|dir; + temp[2] = (unsigned char) (spd >> 16)&0x0F; + temp[1] = (unsigned char) (spd >> 8)&0xFF; + temp[0] = (unsigned char) (spd >> 0)&0xFF; + send_bytes(temp,sizeof temp/sizeof temp[0]); +} + +void L6470::StepClock(unsigned char dir){ + send(0x58|dir); +} + + +void L6470::Move(unsigned char dir,int n_step){ + unsigned char temp[4]; + temp[3] = 0x40|dir; + temp[2] = (unsigned char) (n_step >> 16)&0x3F; + temp[1] = (unsigned char) (n_step >> 8)&0xFF; + temp[0] = (unsigned char) (n_step >> 0)&0xFF; + send_bytes(temp,sizeof temp/sizeof temp[0]); +} + +void L6470::GoTo(int abs_pos){ + unsigned char temp[4]; + temp[3] = 0x60; + temp[2] = (unsigned char) (abs_pos >> 16)&0x3F; + temp[1] = (unsigned char) (abs_pos >> 8)&0xFF; + temp[0] = (unsigned char) (abs_pos >> 0)&0xFF; + send_bytes(temp,sizeof temp/sizeof temp[0]); +} + + +void L6470::GoTo_DIR(unsigned char dir,int abs_pos){ + unsigned char temp[4]; + temp[3] = 0x68|dir; + temp[2] = (unsigned char) (abs_pos >> 16)&0x3F; + temp[1] = (unsigned char) (abs_pos >> 8)&0xFF; + temp[0] = (unsigned char) (abs_pos >> 0)&0xFF; + send_bytes(temp,sizeof temp/sizeof temp[0]); +} + + +void L6470::GoUntil(unsigned char act,unsigned char dir,int spd){ + unsigned char temp[4]; + temp[3] = 0x82|(act << 3)|dir; + temp[2] = (unsigned char) (spd >> 16)&0x0F; + temp[1] = (unsigned char) (spd >> 8)&0xFF; + temp[0] = (unsigned char) (spd >> 0)&0xFF; + send_bytes(temp,sizeof temp/sizeof temp[0]); +} + +void L6470::ReleaseSW(unsigned char act,unsigned char dir){ + send(0x92|(act << 3)|dir); +} + +void L6470::GoHome(){ + send(0x70); +} + +void L6470::GoMark(){ + send(0x78); +} + +void L6470::ResetPos(){ + send(0xD8); +} + +void L6470::ResetDevice(){ + send(0xC0); +} + +void L6470::SoftStop(){ + send(0xB0); +} + +void L6470::HardStop(){ + send(0xB8); +} + + +void L6470::SoftHiZ(){ + send(0xA0); +} + +void L6470::HardHiZ(){ + send(0xA8); +} + +void L6470::Resets(){ + SoftStop(); + SetParam(ABS_POS,RH_ABS_POS); + SetParam(EL_POS,RH_EL_POS); + SetParam(MARK,RH_MARK); + SetParam(SPEED,RH_SPEED); + SetParam(ACC,RH_ACC); + SetParam(DEC,RH_DEC); + SetParam(MAX_SPEED,RH_MAX_SPEED); + SetParam(MIN_SPEED,RH_MIN_SPEED); + SetParam(KVAL_HOLD,RH_KVAL_HOLD); + SetParam(KVAL_RUN,RH_KVAL_RUN); + SetParam(KVAL_ACC,RH_KVAL_ACC); + SetParam(KVAL_DEC,RH_KVAL_DEC); + SetParam(INT_SPD,RH_INT_SPD); + SetParam(ST_SLP,RH_ST_SLP); + SetParam(FN_SLP_ACC,RH_FN_SLP_ACC); + SetParam(FN_SLP_DEC,RH_FN_SLP_DEC); + SetParam(K_THERA,RH_K_THERA); + SetParam(OCR_TH,RH_OCR_TH); + SetParam(STALL_TH,RH_STALL_TH); + SetParam(FS_SPD,RH_FS_SPD); + SetParam(STEP_MODE,RH_STEP_MODE); + SetParam(ARARM_FN,RH_ARARM_FN); + SetParam(CONFIG,RH_CONFIG); +} +