test upload STMicroelectronics L6470

Dependents:   test L6470exaple

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);            
+}
+