Brid . / L6470

test upload STMicroelectronics L6470

Dependents:   test L6470exaple

L6470.cpp@3:f0e29d94b863, 2014-03-05 (annotated)

Committer:
Brid
Date:
Wed Mar 05 06:22:03 2014 +0000
Revision:
3:f0e29d94b863
Parent:
1:bb4bb05904cb 
ver1.1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Brid 1:bb4bb05904cb 1 //This 'L6470' libraly maked by Brid
Brid 1:bb4bb05904cb 2 //
Brid 1:bb4bb05904cb 3 //
Brid 1:bb4bb05904cb 4
Brid 0:26699d53cd2a 5
Brid 0:26699d53cd2a 6 #include "mbed.h"
Brid 0:26699d53cd2a 7 #include "L6470.h"
Brid 0:26699d53cd2a 8
Brid 0:26699d53cd2a 9 L6470::L6470(PinName mosi, PinName miso, PinName sclk, PinName cs)
Brid 0:26699d53cd2a 10 :m_spi(mosi, miso, sclk)
Brid 0:26699d53cd2a 11 ,m_cs(cs) {
Brid 0:26699d53cd2a 12 m_cs = 1;
Brid 0:26699d53cd2a 13 m_spi.format(8,3);
Brid 0:26699d53cd2a 14 m_spi.frequency(1000000);
Brid 0:26699d53cd2a 15 //wait(1);
Brid 0:26699d53cd2a 16 //pc.printf("reset now\n");
Brid 0:26699d53cd2a 17 }
Brid 0:26699d53cd2a 18
Brid 0:26699d53cd2a 19
Brid 0:26699d53cd2a 20 unsigned char L6470::send(unsigned char temp){
Brid 0:26699d53cd2a 21 m_cs = 0;
Brid 0:26699d53cd2a 22 //printf("send: %2x",temp);
Brid 0:26699d53cd2a 23 temp =(unsigned char) m_spi.write(temp);
Brid 0:26699d53cd2a 24 //printf(" %2x\n",temp);
Brid 0:26699d53cd2a 25 //wait(0.5);
Brid 0:26699d53cd2a 26 m_cs = 1;
Brid 0:26699d53cd2a 27 wait_us(1);
Brid 0:26699d53cd2a 28 return(temp);
Brid 0:26699d53cd2a 29 }
Brid 0:26699d53cd2a 30
Brid 0:26699d53cd2a 31 void L6470::send_bytes(unsigned char temp[],int i){
Brid 0:26699d53cd2a 32 while(0 < i--){
Brid 0:26699d53cd2a 33 temp[i] = send(temp[i]);
Brid 0:26699d53cd2a 34 }
Brid 0:26699d53cd2a 35 }
Brid 0:26699d53cd2a 36
Brid 0:26699d53cd2a 37 void L6470::NOP(){
Brid 0:26699d53cd2a 38 send(0x00);
Brid 0:26699d53cd2a 39 }
Brid 0:26699d53cd2a 40
Brid 0:26699d53cd2a 41 void L6470::SetParam(int param,int value){
Brid 0:26699d53cd2a 42 int n = (param>>8)/8;
Brid 0:26699d53cd2a 43 int m = (param>>8)%8;
Brid 0:26699d53cd2a 44 // pc.printf("palam = %x\n",param);
Brid 0:26699d53cd2a 45 // pc.printf("n = %x\n",n);
Brid 0:26699d53cd2a 46 // pc.printf("m = %x\n",m);
Brid 0:26699d53cd2a 47 if(m==0){
Brid 0:26699d53cd2a 48 unsigned char temp[n+1];
Brid 0:26699d53cd2a 49 temp[n] = 0x00|(unsigned char)(param&0xFF);
Brid 0:26699d53cd2a 50 while(0 < n--){
Brid 0:26699d53cd2a 51 temp[n]=(unsigned char) (value >> 8*n)&0xFF;
Brid 0:26699d53cd2a 52 }
Brid 0:26699d53cd2a 53 send_bytes(temp,sizeof temp/sizeof temp[0]);
Brid 0:26699d53cd2a 54 }else{
Brid 0:26699d53cd2a 55 unsigned char temp[n+2];
Brid 0:26699d53cd2a 56 temp[n+1] = 0x00|(unsigned char)(param&0xFF);
Brid 0:26699d53cd2a 57 temp[n] =(unsigned char) (value >> 8*n)&~(0xff<<m);
Brid 0:26699d53cd2a 58 while(0 < n--){
Brid 0:26699d53cd2a 59 temp[n]=(unsigned char) (value >> 8*n)&0xFF;
Brid 0:26699d53cd2a 60 }
Brid 0:26699d53cd2a 61 send_bytes(temp,sizeof temp/sizeof temp[0]);
Brid 0:26699d53cd2a 62 }
Brid 0:26699d53cd2a 63 }
Brid 0:26699d53cd2a 64
Brid 0:26699d53cd2a 65
Brid 3:f0e29d94b863 66
Brid 3:f0e29d94b863 67
Brid 0:26699d53cd2a 68 int L6470::GetParam(int param){
Brid 0:26699d53cd2a 69 int value = 0;
Brid 0:26699d53cd2a 70 int n = (param>>8)/8;
Brid 0:26699d53cd2a 71 int m = (param>>8)%8;
Brid 0:26699d53cd2a 72 if(m==0){
Brid 0:26699d53cd2a 73 unsigned char temp[n+1];
Brid 0:26699d53cd2a 74 for(int i = 0; i < n+1; i++){
Brid 0:26699d53cd2a 75 temp[i]=0;
Brid 0:26699d53cd2a 76 }
Brid 0:26699d53cd2a 77 temp[n] = 0x20|(unsigned char)(param&0xFF);
Brid 0:26699d53cd2a 78 send_bytes(temp,sizeof temp/sizeof temp[0]);
Brid 0:26699d53cd2a 79 while(0 < n--){
Brid 0:26699d53cd2a 80 value |= (int)temp[n] << 8*n;
Brid 0:26699d53cd2a 81 }
Brid 0:26699d53cd2a 82 }else{
Brid 0:26699d53cd2a 83 n++;
Brid 0:26699d53cd2a 84 unsigned char temp[n+1];
Brid 0:26699d53cd2a 85 for(int i = 0; i < n+2; i++){
Brid 0:26699d53cd2a 86 temp[i]=0;
Brid 0:26699d53cd2a 87 }
Brid 0:26699d53cd2a 88 temp[n] = 0x20|(unsigned char)(param&0xFF);
Brid 0:26699d53cd2a 89 send_bytes(temp,sizeof temp/sizeof temp[0]);
Brid 0:26699d53cd2a 90 while(0 < n--){
Brid 0:26699d53cd2a 91 value |= (int)temp[n] << 8*n;
Brid 0:26699d53cd2a 92 }
Brid 0:26699d53cd2a 93 }
Brid 3:f0e29d94b863 94
Brid 0:26699d53cd2a 95 return(value);
Brid 0:26699d53cd2a 96 }
Brid 0:26699d53cd2a 97
Brid 0:26699d53cd2a 98
Brid 0:26699d53cd2a 99
Brid 0:26699d53cd2a 100 void L6470::Run(unsigned char dir,int spd){
Brid 0:26699d53cd2a 101 unsigned char temp[4];
Brid 0:26699d53cd2a 102 temp[3] = 0x50|dir;
Brid 0:26699d53cd2a 103 temp[2] = (unsigned char) (spd >> 16)&0x0F;
Brid 0:26699d53cd2a 104 temp[1] = (unsigned char) (spd >> 8)&0xFF;
Brid 0:26699d53cd2a 105 temp[0] = (unsigned char) (spd >> 0)&0xFF;
Brid 0:26699d53cd2a 106 send_bytes(temp,sizeof temp/sizeof temp[0]);
Brid 0:26699d53cd2a 107 }
Brid 0:26699d53cd2a 108
Brid 0:26699d53cd2a 109 void L6470::StepClock(unsigned char dir){
Brid 0:26699d53cd2a 110 send(0x58|dir);
Brid 0:26699d53cd2a 111 }
Brid 0:26699d53cd2a 112
Brid 0:26699d53cd2a 113
Brid 0:26699d53cd2a 114 void L6470::Move(unsigned char dir,int n_step){
Brid 0:26699d53cd2a 115 unsigned char temp[4];
Brid 0:26699d53cd2a 116 temp[3] = 0x40|dir;
Brid 0:26699d53cd2a 117 temp[2] = (unsigned char) (n_step >> 16)&0x3F;
Brid 0:26699d53cd2a 118 temp[1] = (unsigned char) (n_step >> 8)&0xFF;
Brid 0:26699d53cd2a 119 temp[0] = (unsigned char) (n_step >> 0)&0xFF;
Brid 0:26699d53cd2a 120 send_bytes(temp,sizeof temp/sizeof temp[0]);
Brid 0:26699d53cd2a 121 }
Brid 0:26699d53cd2a 122
Brid 0:26699d53cd2a 123 void L6470::GoTo(int abs_pos){
Brid 0:26699d53cd2a 124 unsigned char temp[4];
Brid 0:26699d53cd2a 125 temp[3] = 0x60;
Brid 0:26699d53cd2a 126 temp[2] = (unsigned char) (abs_pos >> 16)&0x3F;
Brid 0:26699d53cd2a 127 temp[1] = (unsigned char) (abs_pos >> 8)&0xFF;
Brid 0:26699d53cd2a 128 temp[0] = (unsigned char) (abs_pos >> 0)&0xFF;
Brid 0:26699d53cd2a 129 send_bytes(temp,sizeof temp/sizeof temp[0]);
Brid 0:26699d53cd2a 130 }
Brid 0:26699d53cd2a 131
Brid 0:26699d53cd2a 132
Brid 0:26699d53cd2a 133 void L6470::GoTo_DIR(unsigned char dir,int abs_pos){
Brid 0:26699d53cd2a 134 unsigned char temp[4];
Brid 0:26699d53cd2a 135 temp[3] = 0x68|dir;
Brid 0:26699d53cd2a 136 temp[2] = (unsigned char) (abs_pos >> 16)&0x3F;
Brid 0:26699d53cd2a 137 temp[1] = (unsigned char) (abs_pos >> 8)&0xFF;
Brid 0:26699d53cd2a 138 temp[0] = (unsigned char) (abs_pos >> 0)&0xFF;
Brid 0:26699d53cd2a 139 send_bytes(temp,sizeof temp/sizeof temp[0]);
Brid 0:26699d53cd2a 140 }
Brid 0:26699d53cd2a 141
Brid 0:26699d53cd2a 142
Brid 0:26699d53cd2a 143 void L6470::GoUntil(unsigned char act,unsigned char dir,int spd){
Brid 0:26699d53cd2a 144 unsigned char temp[4];
Brid 0:26699d53cd2a 145 temp[3] = 0x82|(act << 3)|dir;
Brid 0:26699d53cd2a 146 temp[2] = (unsigned char) (spd >> 16)&0x0F;
Brid 0:26699d53cd2a 147 temp[1] = (unsigned char) (spd >> 8)&0xFF;
Brid 0:26699d53cd2a 148 temp[0] = (unsigned char) (spd >> 0)&0xFF;
Brid 0:26699d53cd2a 149 send_bytes(temp,sizeof temp/sizeof temp[0]);
Brid 0:26699d53cd2a 150 }
Brid 0:26699d53cd2a 151
Brid 0:26699d53cd2a 152 void L6470::ReleaseSW(unsigned char act,unsigned char dir){
Brid 0:26699d53cd2a 153 send(0x92|(act << 3)|dir);
Brid 0:26699d53cd2a 154 }
Brid 0:26699d53cd2a 155
Brid 0:26699d53cd2a 156 void L6470::GoHome(){
Brid 0:26699d53cd2a 157 send(0x70);
Brid 0:26699d53cd2a 158 }
Brid 0:26699d53cd2a 159
Brid 0:26699d53cd2a 160 void L6470::GoMark(){
Brid 0:26699d53cd2a 161 send(0x78);
Brid 0:26699d53cd2a 162 }
Brid 0:26699d53cd2a 163
Brid 0:26699d53cd2a 164 void L6470::ResetPos(){
Brid 0:26699d53cd2a 165 send(0xD8);
Brid 0:26699d53cd2a 166 }
Brid 0:26699d53cd2a 167
Brid 0:26699d53cd2a 168 void L6470::ResetDevice(){
Brid 0:26699d53cd2a 169 send(0xC0);
Brid 0:26699d53cd2a 170 }
Brid 0:26699d53cd2a 171
Brid 0:26699d53cd2a 172 void L6470::SoftStop(){
Brid 0:26699d53cd2a 173 send(0xB0);
Brid 0:26699d53cd2a 174 }
Brid 0:26699d53cd2a 175
Brid 0:26699d53cd2a 176 void L6470::HardStop(){
Brid 0:26699d53cd2a 177 send(0xB8);
Brid 0:26699d53cd2a 178 }
Brid 0:26699d53cd2a 179
Brid 0:26699d53cd2a 180
Brid 0:26699d53cd2a 181 void L6470::SoftHiZ(){
Brid 0:26699d53cd2a 182 send(0xA0);
Brid 0:26699d53cd2a 183 }
Brid 0:26699d53cd2a 184
Brid 0:26699d53cd2a 185 void L6470::HardHiZ(){
Brid 0:26699d53cd2a 186 send(0xA8);
Brid 0:26699d53cd2a 187 }
Brid 0:26699d53cd2a 188
Brid 0:26699d53cd2a 189 void L6470::Resets(){
Brid 0:26699d53cd2a 190 SoftStop();
Brid 3:f0e29d94b863 191 ResetDevice();
Brid 3:f0e29d94b863 192 SetParam(ABS_POS,INI_ABS_POS);
Brid 3:f0e29d94b863 193 SetParam(EL_POS,INI_EL_POS);
Brid 3:f0e29d94b863 194 SetParam(MARK,INI_MARK);
Brid 3:f0e29d94b863 195 SetParam(SPEED,INI_SPEED);
Brid 3:f0e29d94b863 196 SetParam(ACC,INI_ACC);
Brid 3:f0e29d94b863 197 SetParam(DEC,INI_DEC);
Brid 3:f0e29d94b863 198 SetParam(MAX_SPEED,INI_MAX_SPEED);
Brid 3:f0e29d94b863 199 SetParam(MIN_SPEED,INI_MIN_SPEED);
Brid 3:f0e29d94b863 200 SetParam(KVAL_HOLD,INI_KVAL_HOLD);
Brid 3:f0e29d94b863 201 SetParam(KVAL_RUN,INI_KVAL_RUN);
Brid 3:f0e29d94b863 202 SetParam(KVAL_ACC,INI_KVAL_ACC);
Brid 3:f0e29d94b863 203 SetParam(KVAL_DEC,INI_KVAL_DEC);
Brid 3:f0e29d94b863 204 SetParam(INT_SPD,INI_INT_SPD);
Brid 3:f0e29d94b863 205 SetParam(ST_SLP,INI_ST_SLP);
Brid 3:f0e29d94b863 206 SetParam(FN_SLP_ACC,INI_FN_SLP_ACC);
Brid 3:f0e29d94b863 207 SetParam(FN_SLP_DEC,INI_FN_SLP_DEC);
Brid 3:f0e29d94b863 208 SetParam(K_THERA,INI_K_THERA);
Brid 3:f0e29d94b863 209 SetParam(OCR_TH,INI_OCR_TH);
Brid 3:f0e29d94b863 210 SetParam(STALL_TH,INI_STALL_TH);
Brid 3:f0e29d94b863 211 SetParam(FS_SPD,INI_FS_SPD);
Brid 3:f0e29d94b863 212 SetParam(STEP_MODE,INI_STEP_MODE);
Brid 3:f0e29d94b863 213 SetParam(ARARM_FN,INI_ARARM_FN);
Brid 3:f0e29d94b863 214 SetParam(CONFIG,INI_CONFIG);
Brid 0:26699d53cd2a 215 }
Brid 0:26699d53cd2a 216