Revision 0:26699d53cd2a, committed 2012-12-25

Comitter:

Brid

Date:

Tue Dec 25 02:39:30 2012 +0000

Child:

1:bb4bb05904cb

Commit message:

test upload

Changed in this revision

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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L6470.h	Tue Dec 25 02:39:30 2012 +0000
@@ -0,0 +1,99 @@
+#ifndef MBED_L6470
+#define MBED_L6470
+ 
+#include "mbed.h"
+
+#define FORWARD     1
+#define REVERSE     0
+
+// name         (length<<2)+address
+#define ABS_POS      ((22<<8)+0x01)    //Current position         
+#define EL_POS        ((9<<8)+0x02)    //Electrical position
+#define MARK         ((22<<8)+0x03)    //Mark position
+#define SPEED        ((20<<8)+0x04)    //Current speed
+#define ACC          ((12<<8)+0x05)    //Acceleration
+#define DEC          ((12<<8)+0x06)    //Deceleration
+#define MAX_SPEED    ((10<<8)+0x07)    //Maximum speed
+#define MIN_SPEED    ((13<<8)+0x08)    //Minimum speed
+#define KVAL_HOLD     ((8<<8)+0x09)    //Full step speed
+#define KVAL_RUN      ((8<<8)+0x0A)    //
+#define KVAL_ACC      ((8<<8)+0x0B)    //
+#define KVAL_DEC      ((8<<8)+0x0C)    //
+#define INT_SPD      ((14<<8)+0x0D)    //
+#define ST_SLP        ((8<<8)+0x0E)    //
+#define FN_SLP_ACC    ((8<<8)+0x0F)    //
+#define FN_SLP_DEC    ((8<<8)+0x10)    //
+#define K_THERA       ((4<<8)+0x11)    //
+#define ADC_OUT       ((5<<8)+0x12)    //
+#define OCR_TH        ((4<<8)+0x13)    //
+#define STALL_TH      ((7<<8)+0x14)    //
+#define FS_SPD       ((10<<8)+0x15)    //
+#define STEP_MODE     ((8<<8)+0x16)    //
+#define ARARM_FN      ((8<<8)+0x17)    //
+#define CONFIG       ((16<<8)+0x18)    //
+#define STATUS       ((16<<8)+0x19)    //
+
+
+
+//RH = ResetHex
+#define RH_ABS_POS     0    //Current position         
+#define RH_EL_POS      0    //Electrical position
+#define RH_MARK        0    //Mark position
+#define RH_SPEED       0    //Current speed
+#define RH_ACC         0x8A    //Acceleration
+#define RH_DEC         0x8A    //Deceleration
+#define RH_MAX_SPEED   0x20    //Maximum speed
+#define RH_MIN_SPEED   0    //Minimum speed
+#define RH_KVAL_HOLD   0xFF    //Full step speed
+#define RH_KVAL_RUN    0xFF    //
+#define RH_KVAL_ACC    0xFF    //
+#define RH_KVAL_DEC    0xFF    //
+#define RH_INT_SPD     0x408   //
+#define RH_ST_SLP      0x19    //
+#define RH_FN_SLP_ACC  0x29    //
+#define RH_FN_SLP_DEC  0x29    //
+#define RH_K_THERA     0x0    //
+//         ADC_OUT     ReadOnly
+#define RH_OCR_TH      0xF      
+#define RH_STALL_TH    0x7F
+#define RH_FS_SPD      0x27
+#define RH_STEP_MODE   0x7
+#define RH_ARARM_FN    0xFF
+#define RH_CONFIG      0x2E88    
+//      RH_STATUS      ReadOnly
+
+
+
+class L6470{
+public:
+    L6470(PinName mosi, PinName miso, PinName sclk, PinName cs);
+
+    unsigned char send(unsigned char temp);    
+    void send_bytes(unsigned char temp[],int i);
+    void NOP();
+    void SetParam(int param,int value);
+    int GetParam(int param);
+    void Run(unsigned char dir,int spd);
+    void StepClock(unsigned char dir);
+    void Move(unsigned char dir,int n_step);
+    void GoTo(int abs_pos);
+    void GoTo_DIR(unsigned char dir,int abs_pos);
+    void GoUntil(unsigned char act,unsigned char dir,int spd);
+    void ReleaseSW(unsigned char act,unsigned char dir);
+    void GoHome();
+    void GoMark();
+    void ResetPos();
+    void ResetDevice();
+    void SoftStop();
+    void HardStop();
+    void SoftHiZ();
+    void HardHiZ();
+    void Resets();
+
+private:
+    SPI m_spi;
+    DigitalOut m_cs;
+};
+
+ 
+#endif
\ No newline at end of file