test upload STMicroelectronics L6470
L6470.cpp
- Committer:
- Brid
- Date:
- 2014-03-05
- Revision:
- 2:97c203ed65da
- Parent:
- 1:bb4bb05904cb
File content as of revision 2:97c203ed65da:
//This 'L6470' libraly maked by Brid // // #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(L6470_Register param,int value){ int length,address; switch(param){ case ABS_POS: length = LEN_ABS_POS; address = ADD_ABS_POS; break; case EL_POS: length = LEN_EL_POS; address = ADD_EL_POS; break; case MARK: length = LEN_MARK; address = ADD_MARK; break; case SPEED: length = LEN_SPEED; address = ADD_SPEED; break; case ACC: length = LEN_ACC; address = ADD_ACC; break; case DEC: length = LEN_DEC; address = ADD_DEC; break; case MAX_SPEED: length = LEN_MAX_SPEED; address = ADD_MAX_SPEED; break; case MIN_SPEED: length = LEN_MIN_SPEED; address = ADD_MIN_SPEED; break; case KVAL_HOLD: length = LEN_KVAL_HOLD; address = ADD_KVAL_HOLD; break; case KVAL_RUN: length = LEN_KVAL_RUN; address = ADD_KVAL_RUN; break; case KVAL_ACC: length = LEN_KVAL_ACC; address = ADD_KVAL_ACC; break; case KVAL_DEC: length = LEN_KVAL_DEC; address = ADD_KVAL_DEC; break; case INT_SPD: length = LEN_INT_SPD; address = ADD_INT_SPD; break; case ST_SLP: length = LEN_ST_SLP; address = ADD_ST_SLP; break; case FN_SLP_ACC: length = LEN_FN_SLP_ACC; address = ADD_FN_SLP_ACC; break; case FN_SLP_DEC: length = LEN_FN_SLP_DEC; address = ADD_FN_SLP_DEC; break; case K_THERA: length = LEN_K_THERA; address = ADD_K_THERA; break; case ADC_OUT: length = LEN_ADC_OUT; address = ADD_ADC_OUT; break; case OCR_TH: length = LEN_OCR_TH; address = ADD_OCR_TH; break; case STALL_TH: length = LEN_STALL_TH; address = ADD_STALL_TH; break; case FS_SPD: length = LEN_FS_SPD; address = ADD_FS_SPD; break; case STEP_MODE: length = LEN_STEP_MODE; address = ADD_STEP_MODE; break; case ARARM_FN: length = LEN_ARARM_FN; address = ADD_ARARM_FN; break; case CONFIG: length = LEN_CONFIG; address = ADD_CONFIG; break; case STATUS: length = LEN_STATUS; address = ADD_STATUS; break; default: length = 0; address = 0; break; } int n = length/8; int m = length%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)(address); 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)(address); 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(L6470_Register param){ int value = 0; int length,address; switch(param){ case ABS_POS: length = LEN_ABS_POS; address = ADD_ABS_POS; break; case EL_POS: length = LEN_EL_POS; address = ADD_EL_POS; break; case MARK: length = LEN_MARK; address = ADD_MARK; break; case SPEED: length = LEN_SPEED; address = ADD_SPEED; break; case ACC: length = LEN_ACC; address = ADD_ACC; break; case DEC: length = LEN_DEC; address = ADD_DEC; break; case MAX_SPEED: length = LEN_MAX_SPEED; address = ADD_MAX_SPEED; break; case MIN_SPEED: length = LEN_MIN_SPEED; address = ADD_MIN_SPEED; break; case KVAL_HOLD: length = LEN_KVAL_HOLD; address = ADD_KVAL_HOLD; break; case KVAL_RUN: length = LEN_KVAL_RUN; address = ADD_KVAL_RUN; break; case KVAL_ACC: length = LEN_KVAL_ACC; address = ADD_KVAL_ACC; break; case KVAL_DEC: length = LEN_KVAL_DEC; address = ADD_KVAL_DEC; break; case INT_SPD: length = LEN_INT_SPD; address = ADD_INT_SPD; break; case ST_SLP: length = LEN_ST_SLP; address = ADD_ST_SLP; break; case FN_SLP_ACC: length = LEN_FN_SLP_ACC; address = ADD_FN_SLP_ACC; break; case FN_SLP_DEC: length = LEN_FN_SLP_DEC; address = ADD_FN_SLP_DEC; break; case K_THERA: length = LEN_K_THERA; address = ADD_K_THERA; break; case ADC_OUT: length = LEN_ADC_OUT; address = ADD_ADC_OUT; break; case OCR_TH: length = LEN_OCR_TH; address = ADD_OCR_TH; break; case STALL_TH: length = LEN_STALL_TH; address = ADD_STALL_TH; break; case FS_SPD: length = LEN_FS_SPD; address = ADD_FS_SPD; break; case STEP_MODE: length = LEN_STEP_MODE; address = ADD_STEP_MODE; break; case ARARM_FN: length = LEN_ARARM_FN; address = ADD_ARARM_FN; break; case CONFIG: length = LEN_CONFIG; address = ADD_CONFIG; break; case STATUS: length = LEN_STATUS; address = ADD_STATUS; break; default: length = 0; address = 0; break; } int n = length/8; int m = length%8; if(m==0){ unsigned char temp[n+1]; for(int i = 0; i < n+1; i++){ temp[i]=0; } temp[n] = 0x20|(unsigned char)(address); 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)(address); 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(); ResetDevice(); SetParam(ABS_POS,INI_ABS_POS); SetParam(EL_POS,INI_EL_POS); SetParam(MARK,INI_MARK); SetParam(SPEED,INI_SPEED); SetParam(ACC,INI_ACC); SetParam(DEC,INI_DEC); SetParam(MAX_SPEED,INI_MAX_SPEED); SetParam(MIN_SPEED,INI_MIN_SPEED); SetParam(KVAL_HOLD,INI_KVAL_HOLD); SetParam(KVAL_RUN,INI_KVAL_RUN); SetParam(KVAL_ACC,INI_KVAL_ACC); SetParam(KVAL_DEC,INI_KVAL_DEC); SetParam(INT_SPD,INI_INT_SPD); SetParam(ST_SLP,INI_ST_SLP); SetParam(FN_SLP_ACC,INI_FN_SLP_ACC); SetParam(FN_SLP_DEC,INI_FN_SLP_DEC); SetParam(K_THERA,INI_K_THERA); SetParam(OCR_TH,INI_OCR_TH); SetParam(STALL_TH,INI_STALL_TH); SetParam(FS_SPD,INI_FS_SPD); SetParam(STEP_MODE,INI_STEP_MODE); SetParam(ARARM_FN,INI_ARARM_FN); SetParam(CONFIG,INI_CONFIG); }