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