Brid .
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);
}