test upload STMicroelectronics L6470
Diff: L6470.cpp
- Revision:
- 2:97c203ed65da
- Parent:
- 1:bb4bb05904cb
--- a/L6470.cpp Tue Dec 25 02:46:32 2012 +0000 +++ b/L6470.cpp Wed Mar 05 06:17:55 2014 +0000 @@ -38,22 +38,136 @@ send(0x00); } -void L6470::SetParam(int param,int value){ - int n = (param>>8)/8; - int m = (param>>8)%8; +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)(param&0xFF); + 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)(param&0xFF); + 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; @@ -63,19 +177,127 @@ } -int L6470::GetParam(int param){ + + +int L6470::GetParam(L6470_Register 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); + 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)(param&0xFF); + temp[n] = 0x20|(unsigned char)(address); send_bytes(temp,sizeof temp/sizeof temp[0]); while(0 < n--){ value |= (int)temp[n] << 8*n; @@ -86,12 +308,13 @@ for(int i = 0; i < n+2; i++){ temp[i]=0; } - temp[n] = 0x20|(unsigned char)(param&0xFF); + 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); } @@ -188,28 +411,29 @@ 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); + 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); }