LPS JS / lib_L6470DC_v2

LPS mod

lib_L6470DC.cpp@7:2ff9788e2827, 2022-05-02 (annotated)

Committer:
jsanchez
Date:
Mon May 02 13:07:52 2022 +0000
Revision:
7:2ff9788e2827
Parent:
6:7ca2eef852fd 
added status enum

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jsanchez 6:7ca2eef852fd 1 #include "mbed.h"
jsanchez 6:7ca2eef852fd 2 #include "lib_L6470DC.h"
jsanchez 6:7ca2eef852fd 3
jsanchez 6:7ca2eef852fd 4 ////////////////////////////////////////////////////////////////////////////////
jsanchez 6:7ca2eef852fd 5 // CONSTRUCTOR //
jsanchez 6:7ca2eef852fd 6 ////////////////////////////////////////////////////////////////////////////////
jsanchez 6:7ca2eef852fd 7
jsanchez 6:7ca2eef852fd 8 L6470DC::L6470DC(int nMax, PinName mosi, PinName miso, PinName sclk, PinName cs):m_spi(mosi, miso, sclk),m_cs(cs), nMotMax(nMax)
jsanchez 6:7ca2eef852fd 9 {
jsanchez 6:7ca2eef852fd 10 m_cs = 1;
jsanchez 6:7ca2eef852fd 11 m_spi.format(8,3);
jsanchez 6:7ca2eef852fd 12 m_spi.frequency(5000000);
jsanchez 6:7ca2eef852fd 13 }
jsanchez 6:7ca2eef852fd 14
jsanchez 6:7ca2eef852fd 15 ////////////////////////////////////////////////////////////////////////////////
jsanchez 6:7ca2eef852fd 16 // BASICS_FUNC //
jsanchez 6:7ca2eef852fd 17 ////////////////////////////////////////////////////////////////////////////////
jsanchez 6:7ca2eef852fd 18
jsanchez 6:7ca2eef852fd 19 unsigned char L6470DC::send(unsigned char nMOT,unsigned char temp)
jsanchez 6:7ca2eef852fd 20 {
jsanchez 6:7ca2eef852fd 21 unsigned char temp0;
jsanchez 6:7ca2eef852fd 22 int i;
jsanchez 6:7ca2eef852fd 23 m_cs = 0;
jsanchez 6:7ca2eef852fd 24 for (i = nMotMax - 1; i >= 0; i--) {
jsanchez 6:7ca2eef852fd 25 if(i==nMOT) temp =(unsigned char) m_spi.write(temp);
jsanchez 6:7ca2eef852fd 26 else temp0 =(unsigned char) m_spi.write(0x00);
jsanchez 6:7ca2eef852fd 27 }
jsanchez 6:7ca2eef852fd 28
jsanchez 6:7ca2eef852fd 29 m_cs = 1;
jsanchez 6:7ca2eef852fd 30 wait_us(1);
jsanchez 6:7ca2eef852fd 31 return(temp);
jsanchez 6:7ca2eef852fd 32 }
jsanchez 6:7ca2eef852fd 33
jsanchez 6:7ca2eef852fd 34 void L6470DC::send_bytes(unsigned char nMOT,unsigned char temp[],int i)
jsanchez 6:7ca2eef852fd 35 {
jsanchez 6:7ca2eef852fd 36 while(0 < i--) {
jsanchez 6:7ca2eef852fd 37 temp[i] = send(nMOT,temp[i]);
jsanchez 6:7ca2eef852fd 38 }
jsanchez 6:7ca2eef852fd 39 }
jsanchez 6:7ca2eef852fd 40
jsanchez 6:7ca2eef852fd 41 void L6470DC::NOP(unsigned char nMOT)
jsanchez 6:7ca2eef852fd 42 {
jsanchez 6:7ca2eef852fd 43 send(nMOT,0x00);
jsanchez 6:7ca2eef852fd 44 }
jsanchez 6:7ca2eef852fd 45
jsanchez 6:7ca2eef852fd 46 void L6470DC::SetParam(unsigned char nMOT,L6470_Register param,int value)
jsanchez 6:7ca2eef852fd 47 {
jsanchez 6:7ca2eef852fd 48 int length,address;
jsanchez 6:7ca2eef852fd 49
jsanchez 6:7ca2eef852fd 50 switch(param) {
jsanchez 6:7ca2eef852fd 51 case ABS_POS:
jsanchez 6:7ca2eef852fd 52 length = LEN_ABS_POS;
jsanchez 6:7ca2eef852fd 53 address = ADD_ABS_POS;
jsanchez 6:7ca2eef852fd 54 break;
jsanchez 6:7ca2eef852fd 55 case EL_POS:
jsanchez 6:7ca2eef852fd 56 length = LEN_EL_POS;
jsanchez 6:7ca2eef852fd 57 address = ADD_EL_POS;
jsanchez 6:7ca2eef852fd 58 break;
jsanchez 6:7ca2eef852fd 59 case MARK:
jsanchez 6:7ca2eef852fd 60 length = LEN_MARK;
jsanchez 6:7ca2eef852fd 61 address = ADD_MARK;
jsanchez 6:7ca2eef852fd 62 break;
jsanchez 6:7ca2eef852fd 63 case SPEED:
jsanchez 6:7ca2eef852fd 64 length = LEN_SPEED;
jsanchez 6:7ca2eef852fd 65 address = ADD_SPEED;
jsanchez 6:7ca2eef852fd 66 break;
jsanchez 6:7ca2eef852fd 67 case ACC:
jsanchez 6:7ca2eef852fd 68 length = LEN_ACC;
jsanchez 6:7ca2eef852fd 69 address = ADD_ACC;
jsanchez 6:7ca2eef852fd 70 break;
jsanchez 6:7ca2eef852fd 71 case DEC:
jsanchez 6:7ca2eef852fd 72 length = LEN_DEC;
jsanchez 6:7ca2eef852fd 73 address = ADD_DEC;
jsanchez 6:7ca2eef852fd 74 break;
jsanchez 6:7ca2eef852fd 75 case MAX_SPEED:
jsanchez 6:7ca2eef852fd 76 length = LEN_MAX_SPEED;
jsanchez 6:7ca2eef852fd 77 address = ADD_MAX_SPEED;
jsanchez 6:7ca2eef852fd 78 break;
jsanchez 6:7ca2eef852fd 79 case MIN_SPEED:
jsanchez 6:7ca2eef852fd 80 length = LEN_MIN_SPEED;
jsanchez 6:7ca2eef852fd 81 address = ADD_MIN_SPEED;
jsanchez 6:7ca2eef852fd 82 break;
jsanchez 6:7ca2eef852fd 83 case KVAL_HOLD:
jsanchez 6:7ca2eef852fd 84 length = LEN_KVAL_HOLD;
jsanchez 6:7ca2eef852fd 85 address = ADD_KVAL_HOLD;
jsanchez 6:7ca2eef852fd 86 break;
jsanchez 6:7ca2eef852fd 87 case KVAL_RUN:
jsanchez 6:7ca2eef852fd 88 length = LEN_KVAL_RUN;
jsanchez 6:7ca2eef852fd 89 address = ADD_KVAL_RUN;
jsanchez 6:7ca2eef852fd 90 break;
jsanchez 6:7ca2eef852fd 91 case KVAL_ACC:
jsanchez 6:7ca2eef852fd 92 length = LEN_KVAL_ACC;
jsanchez 6:7ca2eef852fd 93 address = ADD_KVAL_ACC;
jsanchez 6:7ca2eef852fd 94 break;
jsanchez 6:7ca2eef852fd 95 case KVAL_DEC:
jsanchez 6:7ca2eef852fd 96 length = LEN_KVAL_DEC;
jsanchez 6:7ca2eef852fd 97 address = ADD_KVAL_DEC;
jsanchez 6:7ca2eef852fd 98 break;
jsanchez 6:7ca2eef852fd 99 case INT_SPD:
jsanchez 6:7ca2eef852fd 100 length = LEN_INT_SPD;
jsanchez 6:7ca2eef852fd 101 address = ADD_INT_SPD;
jsanchez 6:7ca2eef852fd 102 break;
jsanchez 6:7ca2eef852fd 103 case ST_SLP:
jsanchez 6:7ca2eef852fd 104 length = LEN_ST_SLP;
jsanchez 6:7ca2eef852fd 105 address = ADD_ST_SLP;
jsanchez 6:7ca2eef852fd 106 break;
jsanchez 6:7ca2eef852fd 107 case FN_SLP_ACC:
jsanchez 6:7ca2eef852fd 108 length = LEN_FN_SLP_ACC;
jsanchez 6:7ca2eef852fd 109 address = ADD_FN_SLP_ACC;
jsanchez 6:7ca2eef852fd 110 break;
jsanchez 6:7ca2eef852fd 111 case FN_SLP_DEC:
jsanchez 6:7ca2eef852fd 112 length = LEN_FN_SLP_DEC;
jsanchez 6:7ca2eef852fd 113 address = ADD_FN_SLP_DEC;
jsanchez 6:7ca2eef852fd 114 break;
jsanchez 6:7ca2eef852fd 115 case K_THERA:
jsanchez 6:7ca2eef852fd 116 length = LEN_K_THERA;
jsanchez 6:7ca2eef852fd 117 address = ADD_K_THERA;
jsanchez 6:7ca2eef852fd 118 break;
jsanchez 6:7ca2eef852fd 119 case ADC_OUT:
jsanchez 6:7ca2eef852fd 120 length = LEN_ADC_OUT;
jsanchez 6:7ca2eef852fd 121 address = ADD_ADC_OUT;
jsanchez 6:7ca2eef852fd 122 break;
jsanchez 6:7ca2eef852fd 123 case OCR_TH:
jsanchez 6:7ca2eef852fd 124 length = LEN_OCR_TH;
jsanchez 6:7ca2eef852fd 125 address = ADD_OCR_TH;
jsanchez 6:7ca2eef852fd 126 break;
jsanchez 6:7ca2eef852fd 127 case STALL_TH:
jsanchez 6:7ca2eef852fd 128 length = LEN_STALL_TH;
jsanchez 6:7ca2eef852fd 129 address = ADD_STALL_TH;
jsanchez 6:7ca2eef852fd 130 break;
jsanchez 6:7ca2eef852fd 131 case FS_SPD:
jsanchez 6:7ca2eef852fd 132 length = LEN_FS_SPD;
jsanchez 6:7ca2eef852fd 133 address = ADD_FS_SPD;
jsanchez 6:7ca2eef852fd 134 break;
jsanchez 6:7ca2eef852fd 135 case STEP_MODE:
jsanchez 6:7ca2eef852fd 136 length = LEN_STEP_MODE;
jsanchez 6:7ca2eef852fd 137 address = ADD_STEP_MODE;
jsanchez 6:7ca2eef852fd 138 break;
jsanchez 6:7ca2eef852fd 139 case ARARM_FN:
jsanchez 6:7ca2eef852fd 140 length = LEN_ARARM_FN;
jsanchez 6:7ca2eef852fd 141 address = ADD_ARARM_FN;
jsanchez 6:7ca2eef852fd 142 break;
jsanchez 6:7ca2eef852fd 143 case CONFIG:
jsanchez 6:7ca2eef852fd 144 length = LEN_CONFIG;
jsanchez 6:7ca2eef852fd 145 address = ADD_CONFIG;
jsanchez 6:7ca2eef852fd 146 break;
jsanchez 6:7ca2eef852fd 147 case STATUS:
jsanchez 6:7ca2eef852fd 148 length = LEN_STATUS;
jsanchez 6:7ca2eef852fd 149 address = ADD_STATUS;
jsanchez 6:7ca2eef852fd 150 break;
jsanchez 6:7ca2eef852fd 151 default:
jsanchez 6:7ca2eef852fd 152 length = 0;
jsanchez 6:7ca2eef852fd 153 address = 0;
jsanchez 6:7ca2eef852fd 154 break;
jsanchez 6:7ca2eef852fd 155
jsanchez 6:7ca2eef852fd 156 }
jsanchez 6:7ca2eef852fd 157
jsanchez 6:7ca2eef852fd 158 int n = length/8;
jsanchez 6:7ca2eef852fd 159 int m = length%8;
jsanchez 6:7ca2eef852fd 160 if(m==0) {
jsanchez 6:7ca2eef852fd 161 unsigned char temp[n+1];
jsanchez 6:7ca2eef852fd 162 temp[n] = 0x00|(unsigned char)(address);
jsanchez 6:7ca2eef852fd 163 while(0 < n--) {
jsanchez 6:7ca2eef852fd 164 temp[n]=(unsigned char) (value >> 8*n)&0xFF;
jsanchez 6:7ca2eef852fd 165 }
jsanchez 6:7ca2eef852fd 166 send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
jsanchez 6:7ca2eef852fd 167 } else {
jsanchez 6:7ca2eef852fd 168 unsigned char temp[n+2];
jsanchez 6:7ca2eef852fd 169 temp[n+1] = 0x00|(unsigned char)(address);
jsanchez 6:7ca2eef852fd 170 temp[n] =(unsigned char) (value >> 8*n)&~(0xff<<m);
jsanchez 6:7ca2eef852fd 171 while(0 < n--) {
jsanchez 6:7ca2eef852fd 172 temp[n]=(unsigned char) (value >> 8*n)&0xFF;
jsanchez 6:7ca2eef852fd 173 }
jsanchez 6:7ca2eef852fd 174 send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
jsanchez 6:7ca2eef852fd 175 }
jsanchez 6:7ca2eef852fd 176 }
jsanchez 6:7ca2eef852fd 177
jsanchez 6:7ca2eef852fd 178 int L6470DC::GetParam(unsigned char nMOT,L6470_Register param)
jsanchez 6:7ca2eef852fd 179 {
jsanchez 6:7ca2eef852fd 180 int value = 0;
jsanchez 6:7ca2eef852fd 181 int length,address;
jsanchez 6:7ca2eef852fd 182 switch(param) {
jsanchez 6:7ca2eef852fd 183 case ABS_POS:
jsanchez 6:7ca2eef852fd 184 length = LEN_ABS_POS;
jsanchez 6:7ca2eef852fd 185 address = ADD_ABS_POS;
jsanchez 6:7ca2eef852fd 186 break;
jsanchez 6:7ca2eef852fd 187 case EL_POS:
jsanchez 6:7ca2eef852fd 188 length = LEN_EL_POS;
jsanchez 6:7ca2eef852fd 189 address = ADD_EL_POS;
jsanchez 6:7ca2eef852fd 190 break;
jsanchez 6:7ca2eef852fd 191 case MARK:
jsanchez 6:7ca2eef852fd 192 length = LEN_MARK;
jsanchez 6:7ca2eef852fd 193 address = ADD_MARK;
jsanchez 6:7ca2eef852fd 194 break;
jsanchez 6:7ca2eef852fd 195 case SPEED:
jsanchez 6:7ca2eef852fd 196 length = LEN_SPEED;
jsanchez 6:7ca2eef852fd 197 address = ADD_SPEED;
jsanchez 6:7ca2eef852fd 198 break;
jsanchez 6:7ca2eef852fd 199 case ACC:
jsanchez 6:7ca2eef852fd 200 length = LEN_ACC;
jsanchez 6:7ca2eef852fd 201 address = ADD_ACC;
jsanchez 6:7ca2eef852fd 202 break;
jsanchez 6:7ca2eef852fd 203 case DEC:
jsanchez 6:7ca2eef852fd 204 length = LEN_DEC;
jsanchez 6:7ca2eef852fd 205 address = ADD_DEC;
jsanchez 6:7ca2eef852fd 206 break;
jsanchez 6:7ca2eef852fd 207 case MAX_SPEED:
jsanchez 6:7ca2eef852fd 208 length = LEN_MAX_SPEED;
jsanchez 6:7ca2eef852fd 209 address = ADD_MAX_SPEED;
jsanchez 6:7ca2eef852fd 210 break;
jsanchez 6:7ca2eef852fd 211 case MIN_SPEED:
jsanchez 6:7ca2eef852fd 212 length = LEN_MIN_SPEED;
jsanchez 6:7ca2eef852fd 213 address = ADD_MIN_SPEED;
jsanchez 6:7ca2eef852fd 214 break;
jsanchez 6:7ca2eef852fd 215 case KVAL_HOLD:
jsanchez 6:7ca2eef852fd 216 length = LEN_KVAL_HOLD;
jsanchez 6:7ca2eef852fd 217 address = ADD_KVAL_HOLD;
jsanchez 6:7ca2eef852fd 218 break;
jsanchez 6:7ca2eef852fd 219 case KVAL_RUN:
jsanchez 6:7ca2eef852fd 220 length = LEN_KVAL_RUN;
jsanchez 6:7ca2eef852fd 221 address = ADD_KVAL_RUN;
jsanchez 6:7ca2eef852fd 222 break;
jsanchez 6:7ca2eef852fd 223 case KVAL_ACC:
jsanchez 6:7ca2eef852fd 224 length = LEN_KVAL_ACC;
jsanchez 6:7ca2eef852fd 225 address = ADD_KVAL_ACC;
jsanchez 6:7ca2eef852fd 226 break;
jsanchez 6:7ca2eef852fd 227 case KVAL_DEC:
jsanchez 6:7ca2eef852fd 228 length = LEN_KVAL_DEC;
jsanchez 6:7ca2eef852fd 229 address = ADD_KVAL_DEC;
jsanchez 6:7ca2eef852fd 230 break;
jsanchez 6:7ca2eef852fd 231 case INT_SPD:
jsanchez 6:7ca2eef852fd 232 length = LEN_INT_SPD;
jsanchez 6:7ca2eef852fd 233 address = ADD_INT_SPD;
jsanchez 6:7ca2eef852fd 234 break;
jsanchez 6:7ca2eef852fd 235 case ST_SLP:
jsanchez 6:7ca2eef852fd 236 length = LEN_ST_SLP;
jsanchez 6:7ca2eef852fd 237 address = ADD_ST_SLP;
jsanchez 6:7ca2eef852fd 238 break;
jsanchez 6:7ca2eef852fd 239 case FN_SLP_ACC:
jsanchez 6:7ca2eef852fd 240 length = LEN_FN_SLP_ACC;
jsanchez 6:7ca2eef852fd 241 address = ADD_FN_SLP_ACC;
jsanchez 6:7ca2eef852fd 242 break;
jsanchez 6:7ca2eef852fd 243 case FN_SLP_DEC:
jsanchez 6:7ca2eef852fd 244 length = LEN_FN_SLP_DEC;
jsanchez 6:7ca2eef852fd 245 address = ADD_FN_SLP_DEC;
jsanchez 6:7ca2eef852fd 246 break;
jsanchez 6:7ca2eef852fd 247 case K_THERA:
jsanchez 6:7ca2eef852fd 248 length = LEN_K_THERA;
jsanchez 6:7ca2eef852fd 249 address = ADD_K_THERA;
jsanchez 6:7ca2eef852fd 250 break;
jsanchez 6:7ca2eef852fd 251 case ADC_OUT:
jsanchez 6:7ca2eef852fd 252 length = LEN_ADC_OUT;
jsanchez 6:7ca2eef852fd 253 address = ADD_ADC_OUT;
jsanchez 6:7ca2eef852fd 254 break;
jsanchez 6:7ca2eef852fd 255 case OCR_TH:
jsanchez 6:7ca2eef852fd 256 length = LEN_OCR_TH;
jsanchez 6:7ca2eef852fd 257 address = ADD_OCR_TH;
jsanchez 6:7ca2eef852fd 258 break;
jsanchez 6:7ca2eef852fd 259 case STALL_TH:
jsanchez 6:7ca2eef852fd 260 length = LEN_STALL_TH;
jsanchez 6:7ca2eef852fd 261 address = ADD_STALL_TH;
jsanchez 6:7ca2eef852fd 262 break;
jsanchez 6:7ca2eef852fd 263 case FS_SPD:
jsanchez 6:7ca2eef852fd 264 length = LEN_FS_SPD;
jsanchez 6:7ca2eef852fd 265 address = ADD_FS_SPD;
jsanchez 6:7ca2eef852fd 266 break;
jsanchez 6:7ca2eef852fd 267 case STEP_MODE:
jsanchez 6:7ca2eef852fd 268 length = LEN_STEP_MODE;
jsanchez 6:7ca2eef852fd 269 address = ADD_STEP_MODE;
jsanchez 6:7ca2eef852fd 270 break;
jsanchez 6:7ca2eef852fd 271 case ARARM_FN:
jsanchez 6:7ca2eef852fd 272 length = LEN_ARARM_FN;
jsanchez 6:7ca2eef852fd 273 address = ADD_ARARM_FN;
jsanchez 6:7ca2eef852fd 274 break;
jsanchez 6:7ca2eef852fd 275 case CONFIG:
jsanchez 6:7ca2eef852fd 276 length = LEN_CONFIG;
jsanchez 6:7ca2eef852fd 277 address = ADD_CONFIG;
jsanchez 6:7ca2eef852fd 278 break;
jsanchez 6:7ca2eef852fd 279 case STATUS:
jsanchez 6:7ca2eef852fd 280 length = LEN_STATUS;
jsanchez 6:7ca2eef852fd 281 address = ADD_STATUS;
jsanchez 6:7ca2eef852fd 282 break;
jsanchez 6:7ca2eef852fd 283 default:
jsanchez 6:7ca2eef852fd 284 length = 0;
jsanchez 6:7ca2eef852fd 285 address = 0;
jsanchez 6:7ca2eef852fd 286 break;
jsanchez 6:7ca2eef852fd 287
jsanchez 6:7ca2eef852fd 288 }
jsanchez 6:7ca2eef852fd 289
jsanchez 6:7ca2eef852fd 290 int n = length/8;
jsanchez 6:7ca2eef852fd 291 int m = length%8;
jsanchez 6:7ca2eef852fd 292 if(m==0) {
jsanchez 6:7ca2eef852fd 293 unsigned char temp[n+1];
jsanchez 6:7ca2eef852fd 294 for(int i = 0; i < n+1; i++) {
jsanchez 6:7ca2eef852fd 295 temp[i]=0;
jsanchez 6:7ca2eef852fd 296 }
jsanchez 6:7ca2eef852fd 297 temp[n] = 0x20|(unsigned char)(address);
jsanchez 6:7ca2eef852fd 298 send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
jsanchez 6:7ca2eef852fd 299 while(0 < n--) {
jsanchez 6:7ca2eef852fd 300 value |= (int)temp[n] << 8*n;
jsanchez 6:7ca2eef852fd 301 }
jsanchez 6:7ca2eef852fd 302 } else {
jsanchez 6:7ca2eef852fd 303 n++;
jsanchez 6:7ca2eef852fd 304 unsigned char temp[n+1];
jsanchez 6:7ca2eef852fd 305 for(int i = 0; i < n+2; i++) {
jsanchez 6:7ca2eef852fd 306 temp[i]=0;
jsanchez 6:7ca2eef852fd 307 }
jsanchez 6:7ca2eef852fd 308 temp[n] = 0x20|(unsigned char)(address);
jsanchez 6:7ca2eef852fd 309 send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
jsanchez 6:7ca2eef852fd 310 while(0 < n--) {
jsanchez 6:7ca2eef852fd 311 value |= (int)temp[n] << 8*n;
jsanchez 6:7ca2eef852fd 312 }
jsanchez 6:7ca2eef852fd 313 }
jsanchez 6:7ca2eef852fd 314
jsanchez 6:7ca2eef852fd 315 return(value);
jsanchez 6:7ca2eef852fd 316 }
jsanchez 6:7ca2eef852fd 317
jsanchez 6:7ca2eef852fd 318
jsanchez 6:7ca2eef852fd 319
jsanchez 7:2ff9788e2827 320 void L6470DC::Run(unsigned char nMOT,unsigned char dir,float spd)
jsanchez 6:7ca2eef852fd 321 {
jsanchez 6:7ca2eef852fd 322 unsigned char temp[4];
jsanchez 7:2ff9788e2827 323 int tmp_spd = L6470_Step_s_2_Speed(spd);
jsanchez 6:7ca2eef852fd 324 temp[3] = 0x50|dir;
jsanchez 7:2ff9788e2827 325 temp[2] = (unsigned char) (tmp_spd >> 16)&0x0F;
jsanchez 7:2ff9788e2827 326 temp[1] = (unsigned char) (tmp_spd >> 8)&0xFF;
jsanchez 7:2ff9788e2827 327 temp[0] = (unsigned char) (tmp_spd >> 0)&0xFF;
jsanchez 6:7ca2eef852fd 328 send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
jsanchez 6:7ca2eef852fd 329 }
jsanchez 6:7ca2eef852fd 330
jsanchez 6:7ca2eef852fd 331 void L6470DC::StepClock(unsigned char nMOT,unsigned char dir)
jsanchez 6:7ca2eef852fd 332 {
jsanchez 6:7ca2eef852fd 333 send(nMOT,0x58|dir);
jsanchez 6:7ca2eef852fd 334 }
jsanchez 6:7ca2eef852fd 335
jsanchez 6:7ca2eef852fd 336 void L6470DC::Move(unsigned char nMOT,unsigned char dir,int n_step)
jsanchez 6:7ca2eef852fd 337 {
jsanchez 6:7ca2eef852fd 338 unsigned char temp[4];
jsanchez 6:7ca2eef852fd 339 temp[3] = 0x40|dir;
jsanchez 6:7ca2eef852fd 340 temp[2] = (unsigned char) (n_step >> 16)&0x3F;
jsanchez 6:7ca2eef852fd 341 temp[1] = (unsigned char) (n_step >> 8)&0xFF;
jsanchez 6:7ca2eef852fd 342 temp[0] = (unsigned char) (n_step >> 0)&0xFF;
jsanchez 6:7ca2eef852fd 343 send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
jsanchez 6:7ca2eef852fd 344 }
jsanchez 6:7ca2eef852fd 345
jsanchez 6:7ca2eef852fd 346 void L6470DC::GoTo(unsigned char nMOT,int abs_pos)
jsanchez 6:7ca2eef852fd 347 {
jsanchez 6:7ca2eef852fd 348 unsigned char temp[4];
jsanchez 6:7ca2eef852fd 349 temp[3] = 0x60;
jsanchez 6:7ca2eef852fd 350 temp[2] = (unsigned char) (abs_pos >> 16)&0x3F;
jsanchez 6:7ca2eef852fd 351 temp[1] = (unsigned char) (abs_pos >> 8)&0xFF;
jsanchez 6:7ca2eef852fd 352 temp[0] = (unsigned char) (abs_pos >> 0)&0xFF;
jsanchez 6:7ca2eef852fd 353 send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
jsanchez 6:7ca2eef852fd 354 }
jsanchez 6:7ca2eef852fd 355
jsanchez 6:7ca2eef852fd 356 void L6470DC::GoTo_DIR(unsigned char nMOT,unsigned char dir,int abs_pos)
jsanchez 6:7ca2eef852fd 357 {
jsanchez 6:7ca2eef852fd 358 unsigned char temp[4];
jsanchez 6:7ca2eef852fd 359 temp[3] = 0x68|dir;
jsanchez 6:7ca2eef852fd 360 temp[2] = (unsigned char) (abs_pos >> 16)&0x3F;
jsanchez 6:7ca2eef852fd 361 temp[1] = (unsigned char) (abs_pos >> 8)&0xFF;
jsanchez 6:7ca2eef852fd 362 temp[0] = (unsigned char) (abs_pos >> 0)&0xFF;
jsanchez 6:7ca2eef852fd 363 send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
jsanchez 6:7ca2eef852fd 364 }
jsanchez 6:7ca2eef852fd 365
jsanchez 6:7ca2eef852fd 366 void L6470DC::GoUntil(unsigned char nMOT,unsigned char act,unsigned char dir,int spd)
jsanchez 6:7ca2eef852fd 367 {
jsanchez 6:7ca2eef852fd 368 unsigned char temp[4];
jsanchez 6:7ca2eef852fd 369 temp[3] = 0x82|(act << 3)|dir;
jsanchez 6:7ca2eef852fd 370 temp[2] = (unsigned char) (spd >> 16)&0x0F;
jsanchez 6:7ca2eef852fd 371 temp[1] = (unsigned char) (spd >> 8)&0xFF;
jsanchez 6:7ca2eef852fd 372 temp[0] = (unsigned char) (spd >> 0)&0xFF;
jsanchez 6:7ca2eef852fd 373 send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
jsanchez 6:7ca2eef852fd 374 }
jsanchez 6:7ca2eef852fd 375
jsanchez 6:7ca2eef852fd 376 void L6470DC::ReleaseSW(unsigned char nMOT,unsigned char act,unsigned char dir)
jsanchez 6:7ca2eef852fd 377 {
jsanchez 6:7ca2eef852fd 378 send(nMOT,0x92|(act << 3)|dir);
jsanchez 6:7ca2eef852fd 379 }
jsanchez 6:7ca2eef852fd 380
jsanchez 6:7ca2eef852fd 381 void L6470DC::GoHome(unsigned char nMOT)
jsanchez 6:7ca2eef852fd 382 {
jsanchez 6:7ca2eef852fd 383 send(nMOT,0x70);
jsanchez 6:7ca2eef852fd 384 }
jsanchez 6:7ca2eef852fd 385
jsanchez 6:7ca2eef852fd 386 void L6470DC::GoMark(unsigned char nMOT)
jsanchez 6:7ca2eef852fd 387 {
jsanchez 6:7ca2eef852fd 388 send(nMOT,0x78);
jsanchez 6:7ca2eef852fd 389 }
jsanchez 6:7ca2eef852fd 390
jsanchez 6:7ca2eef852fd 391 void L6470DC::ResetPos(unsigned char nMOT)
jsanchez 6:7ca2eef852fd 392 {
jsanchez 6:7ca2eef852fd 393 send(nMOT,0xD8);
jsanchez 6:7ca2eef852fd 394 }
jsanchez 6:7ca2eef852fd 395
jsanchez 6:7ca2eef852fd 396 void L6470DC::ResetDevice(unsigned char nMOT)
jsanchez 6:7ca2eef852fd 397 {
jsanchez 6:7ca2eef852fd 398 send(nMOT,0xC0);
jsanchez 6:7ca2eef852fd 399 }
jsanchez 6:7ca2eef852fd 400
jsanchez 6:7ca2eef852fd 401 void L6470DC::SoftStop(unsigned char nMOT)
jsanchez 6:7ca2eef852fd 402 {
jsanchez 6:7ca2eef852fd 403 send(nMOT,0xB0);
jsanchez 6:7ca2eef852fd 404 }
jsanchez 6:7ca2eef852fd 405
jsanchez 6:7ca2eef852fd 406 void L6470DC::HardStop(unsigned char nMOT)
jsanchez 6:7ca2eef852fd 407 {
jsanchez 6:7ca2eef852fd 408 send(nMOT,0xB8);
jsanchez 6:7ca2eef852fd 409 }
jsanchez 6:7ca2eef852fd 410
jsanchez 6:7ca2eef852fd 411
jsanchez 6:7ca2eef852fd 412 void L6470DC::SoftHiZ(unsigned char nMOT)
jsanchez 6:7ca2eef852fd 413 {
jsanchez 6:7ca2eef852fd 414 send(nMOT,0xA0);
jsanchez 6:7ca2eef852fd 415 }
jsanchez 6:7ca2eef852fd 416
jsanchez 6:7ca2eef852fd 417 void L6470DC::HardHiZ(unsigned char nMOT)
jsanchez 6:7ca2eef852fd 418 {
jsanchez 6:7ca2eef852fd 419 send(nMOT,0xA8);
jsanchez 6:7ca2eef852fd 420 }
jsanchez 6:7ca2eef852fd 421
jsanchez 6:7ca2eef852fd 422 ////////////////////////////////////////////////////////////////////////////////
jsanchez 6:7ca2eef852fd 423 // ADVANCED_FUNC //
jsanchez 6:7ca2eef852fd 424 ////////////////////////////////////////////////////////////////////////////////
jsanchez 6:7ca2eef852fd 425
jsanchez 6:7ca2eef852fd 426 void L6470DC::Config(unsigned char nMOT, L6470_MotorConf *conf)
jsanchez 6:7ca2eef852fd 427 {
jsanchez 6:7ca2eef852fd 428 L6470_MotorConf *MotorParameterData = (L6470_MotorConf *) conf;
jsanchez 6:7ca2eef852fd 429 StepperMotorRegister MotorRegister;
jsanchez 7:2ff9788e2827 430
jsanchez 6:7ca2eef852fd 431 ResetDevice(nMOT);
jsanchez 6:7ca2eef852fd 432
jsanchez 6:7ca2eef852fd 433 MotorRegister.ACC = L6470_Step_s2_2_Acc(MotorParameterData->acc);
jsanchez 6:7ca2eef852fd 434 MotorRegister.DEC = L6470_Step_s2_2_Dec(MotorParameterData->dec);
jsanchez 6:7ca2eef852fd 435 MotorRegister.MAX_SPEED = L6470_Step_s_2_MaxSpeed(MotorParameterData->maxspeed);
jsanchez 6:7ca2eef852fd 436 MotorRegister.MIN_SPEED = L6470_Step_s_2_MinSpeed(MotorParameterData->minspeed);
jsanchez 6:7ca2eef852fd 437 MotorRegister.FS_SPD = L6470_Step_s_2_FsSpd(MotorParameterData->fsspd);
jsanchez 6:7ca2eef852fd 438 MotorRegister.KVAL_HOLD = (uint8_t)((float)((float)(MotorParameterData->kvalhold * 256) / (MotorParameterData->motorvoltage)));
jsanchez 6:7ca2eef852fd 439 MotorRegister.KVAL_RUN = (uint8_t)((float)((float)(MotorParameterData->kvalrun * 256) / (MotorParameterData->motorvoltage)));
jsanchez 6:7ca2eef852fd 440 MotorRegister.KVAL_ACC = (uint8_t)((float)((float)(MotorParameterData->kvalacc * 256) / (MotorParameterData->motorvoltage)));
jsanchez 6:7ca2eef852fd 441 MotorRegister.KVAL_DEC = (uint8_t)((float)((float)(MotorParameterData->kvaldec * 256) / (MotorParameterData->motorvoltage)));
jsanchez 6:7ca2eef852fd 442 MotorRegister.INT_SPEED = L6470_Step_s_2_IntSpeed(MotorParameterData->intspeed);
jsanchez 6:7ca2eef852fd 443 MotorRegister.ST_SLP = L6470_s_Step_2_StSlp(MotorParameterData->stslp);
jsanchez 6:7ca2eef852fd 444 MotorRegister.FN_SLP_ACC = L6470_s_Step_2_FnSlpAcc(MotorParameterData->fnslpacc);
jsanchez 6:7ca2eef852fd 445 MotorRegister.FN_SLP_DEC = L6470_s_Step_2_FnSlpDec(MotorParameterData->fnslpdec);
jsanchez 6:7ca2eef852fd 446 MotorRegister.K_THERM = MotorParameterData->kterm;
jsanchez 6:7ca2eef852fd 447 MotorRegister.OCD_TH = L6470_mA_2_OcdTh(MotorParameterData->ocdth);
jsanchez 6:7ca2eef852fd 448 MotorRegister.STALL_TH = L6470_mA_2_StallTh(MotorParameterData->stallth);
jsanchez 6:7ca2eef852fd 449 MotorRegister.ALARM_EN = MotorParameterData->alarmen;
jsanchez 6:7ca2eef852fd 450 MotorRegister.CONFIG = MotorParameterData->config;
jsanchez 6:7ca2eef852fd 451 MotorRegister.STEP_MODE = MotorParameterData->step_sel;
jsanchez 6:7ca2eef852fd 452
jsanchez 6:7ca2eef852fd 453 /* Write the L6470 registers with the prepared data */
jsanchez 6:7ca2eef852fd 454 SetParam(nMOT, ACC, MotorRegister.ACC);
jsanchez 6:7ca2eef852fd 455 SetParam(nMOT, DEC, MotorRegister.DEC);
jsanchez 6:7ca2eef852fd 456 SetParam(nMOT, MAX_SPEED, MotorRegister.MAX_SPEED);
jsanchez 6:7ca2eef852fd 457 SetParam(nMOT, MIN_SPEED, MotorRegister.MIN_SPEED);
jsanchez 6:7ca2eef852fd 458 SetParam(nMOT, FS_SPD, MotorRegister.FS_SPD);
jsanchez 6:7ca2eef852fd 459 SetParam(nMOT, KVAL_HOLD, MotorRegister.KVAL_HOLD);
jsanchez 6:7ca2eef852fd 460 SetParam(nMOT, KVAL_RUN, MotorRegister.KVAL_RUN);
jsanchez 6:7ca2eef852fd 461 SetParam(nMOT, KVAL_ACC, MotorRegister.KVAL_ACC);
jsanchez 6:7ca2eef852fd 462 SetParam(nMOT, KVAL_DEC, MotorRegister.KVAL_DEC);
jsanchez 6:7ca2eef852fd 463 SetParam(nMOT, INT_SPD, MotorRegister.INT_SPEED);
jsanchez 6:7ca2eef852fd 464 SetParam(nMOT, ST_SLP, MotorRegister.ST_SLP);
jsanchez 6:7ca2eef852fd 465 SetParam(nMOT, FN_SLP_ACC, MotorRegister.FN_SLP_ACC);
jsanchez 6:7ca2eef852fd 466 SetParam(nMOT, FN_SLP_DEC, MotorRegister.FN_SLP_DEC);
jsanchez 6:7ca2eef852fd 467 SetParam(nMOT, K_THERA, MotorRegister.K_THERM);
jsanchez 6:7ca2eef852fd 468 SetParam(nMOT, OCR_TH, MotorRegister.OCD_TH);
jsanchez 6:7ca2eef852fd 469 SetParam(nMOT, STALL_TH, MotorRegister.STALL_TH);
jsanchez 6:7ca2eef852fd 470 SetParam(nMOT, STEP_MODE, MotorRegister.STEP_MODE);
jsanchez 6:7ca2eef852fd 471 SetParam(nMOT, ARARM_FN, MotorRegister.ALARM_EN);
jsanchez 6:7ca2eef852fd 472 SetParam(nMOT, CONFIG, MotorRegister.CONFIG);
jsanchez 6:7ca2eef852fd 473 }
jsanchez 6:7ca2eef852fd 474
jsanchez 6:7ca2eef852fd 475 ////////////////////////////////////////////////////////////////////////////////
jsanchez 6:7ca2eef852fd 476
jsanchez 6:7ca2eef852fd 477 int L6470DC::GetPosition(int nMOT)
jsanchez 6:7ca2eef852fd 478 {
jsanchez 6:7ca2eef852fd 479 return L6470_AbsPos_2_Position(GetParam(nMOT, ABS_POS));
jsanchez 6:7ca2eef852fd 480 }
jsanchez 6:7ca2eef852fd 481
jsanchez 6:7ca2eef852fd 482 int L6470DC::GetMark(int nMOT)
jsanchez 6:7ca2eef852fd 483 {
jsanchez 6:7ca2eef852fd 484 return L6470_AbsPos_2_Position(GetParam(nMOT, MARK));
jsanchez 6:7ca2eef852fd 485 }
jsanchez 6:7ca2eef852fd 486
jsanchez 6:7ca2eef852fd 487 int L6470DC::GetSpeed(int nMOT)
jsanchez 6:7ca2eef852fd 488 {
jsanchez 7:2ff9788e2827 489 return L6470_Speed_2_Step_s(GetParam(nMOT, SPEED));
jsanchez 7:2ff9788e2827 490 }
jsanchez 7:2ff9788e2827 491
jsanchez 7:2ff9788e2827 492 int L6470DC::GetStatus(int nMOT, FlagId_t FlagId)
jsanchez 7:2ff9788e2827 493 {
jsanchez 7:2ff9788e2827 494 int status = GetParam(nMOT, STATUS);
jsanchez 7:2ff9788e2827 495 int ack = -1;
jsanchez 7:2ff9788e2827 496
jsanchez 7:2ff9788e2827 497 switch(FlagId) {
jsanchez 7:2ff9788e2827 498 case MOT_STATUS_ID:
jsanchez 7:2ff9788e2827 499 ack = status & (3 << MOT_STATUS_ID);
jsanchez 7:2ff9788e2827 500 return ack >> MOT_STATUS_ID;
jsanchez 7:2ff9788e2827 501 break;
jsanchez 7:2ff9788e2827 502
jsanchez 7:2ff9788e2827 503 default :
jsanchez 7:2ff9788e2827 504 ack = status & (1 << FlagId);
jsanchez 7:2ff9788e2827 505 return ack >> FlagId;
jsanchez 7:2ff9788e2827 506 break;
jsanchez 7:2ff9788e2827 507 }
jsanchez 6:7ca2eef852fd 508 }
jsanchez 6:7ca2eef852fd 509
jsanchez 6:7ca2eef852fd 510 void L6470DC::SetHome(int nMOT)
jsanchez 6:7ca2eef852fd 511 {
jsanchez 6:7ca2eef852fd 512 ResetPos(nMOT);
jsanchez 6:7ca2eef852fd 513 }
jsanchez 6:7ca2eef852fd 514
jsanchez 6:7ca2eef852fd 515 void L6470DC::SetMaxSpeed(int nMOT, int speed)
jsanchez 6:7ca2eef852fd 516 {
jsanchez 7:2ff9788e2827 517 SetParam(nMOT, MAX_SPEED, L6470_Step_s_2_MaxSpeed(speed));
jsanchez 6:7ca2eef852fd 518 }
jsanchez 6:7ca2eef852fd 519
jsanchez 6:7ca2eef852fd 520 void L6470DC::SetMark(int nMOT, int mark)
jsanchez 6:7ca2eef852fd 521 {
jsanchez 6:7ca2eef852fd 522 SetParam(nMOT, MARK, L6470_Position_2_AbsPos(mark));
jsanchez 6:7ca2eef852fd 523 }
jsanchez 6:7ca2eef852fd 524
jsanchez 6:7ca2eef852fd 525
jsanchez 6:7ca2eef852fd 526 ////////////////////////////////////////////////////////////////////////////////
jsanchez 6:7ca2eef852fd 527 // CONVERSIONS //
jsanchez 6:7ca2eef852fd 528 ////////////////////////////////////////////////////////////////////////////////
jsanchez 6:7ca2eef852fd 529
jsanchez 6:7ca2eef852fd 530 int32_t L6470DC::L6470_AbsPos_2_Position(uint32_t AbsPos)
jsanchez 6:7ca2eef852fd 531 {
jsanchez 7:2ff9788e2827 532
jsanchez 6:7ca2eef852fd 533 if (AbsPos > L6470_MAX_POSITION)
jsanchez 6:7ca2eef852fd 534 return (AbsPos - (L6470_POSITION_RANGE + 1));
jsanchez 6:7ca2eef852fd 535 else
jsanchez 7:2ff9788e2827 536
jsanchez 7:2ff9788e2827 537 return AbsPos;
jsanchez 6:7ca2eef852fd 538 }
jsanchez 6:7ca2eef852fd 539
jsanchez 6:7ca2eef852fd 540 uint32_t L6470DC::L6470_Position_2_AbsPos(int32_t Position)
jsanchez 6:7ca2eef852fd 541 {
jsanchez 7:2ff9788e2827 542 if ((Position >= 0) && (Position <= L6470_MAX_POSITION))
jsanchez 7:2ff9788e2827 543 return Position;
jsanchez 7:2ff9788e2827 544 else {
jsanchez 7:2ff9788e2827 545 if ((Position >= L6470_MIN_POSITION) && (Position < 0))
jsanchez 7:2ff9788e2827 546 return (Position + (L6470_POSITION_RANGE + 1));
jsanchez 7:2ff9788e2827 547 else
jsanchez 7:2ff9788e2827 548 return (L6470_POSITION_RANGE + 1); // OVF
jsanchez 7:2ff9788e2827 549 }
jsanchez 6:7ca2eef852fd 550 }
jsanchez 6:7ca2eef852fd 551
jsanchez 6:7ca2eef852fd 552 float L6470DC::L6470_Speed_2_Step_s(uint32_t Speed)
jsanchez 6:7ca2eef852fd 553 {
jsanchez 6:7ca2eef852fd 554 return (Speed * ((float)14.9012e-3));
jsanchez 6:7ca2eef852fd 555 }
jsanchez 6:7ca2eef852fd 556
jsanchez 6:7ca2eef852fd 557 uint32_t L6470DC::L6470_Step_s_2_Speed(float Step_s)
jsanchez 6:7ca2eef852fd 558 {
jsanchez 7:2ff9788e2827 559 //if (Step_s <= (L6470_MAX_SPEED * ((float)14.9012e-3)))
jsanchez 7:2ff9788e2827 560 return (uint32_t)(Step_s / ((float)14.9012e-3));
jsanchez 7:2ff9788e2827 561 //else
jsanchez 7:2ff9788e2827 562 //return 0;
jsanchez 6:7ca2eef852fd 563 }
jsanchez 6:7ca2eef852fd 564
jsanchez 6:7ca2eef852fd 565 float L6470DC::L6470_Acc_2_Step_s2(uint16_t Acc)
jsanchez 6:7ca2eef852fd 566 {
jsanchez 6:7ca2eef852fd 567 if (Acc <= L6470_MAX_ACC)
jsanchez 6:7ca2eef852fd 568 return (Acc * ((float)1.4552e1));
jsanchez 6:7ca2eef852fd 569 else
jsanchez 6:7ca2eef852fd 570 return 0;
jsanchez 6:7ca2eef852fd 571 }
jsanchez 6:7ca2eef852fd 572
jsanchez 6:7ca2eef852fd 573 uint16_t L6470DC::L6470_Step_s2_2_Acc(float Step_s2)
jsanchez 6:7ca2eef852fd 574 {
jsanchez 6:7ca2eef852fd 575 if (Step_s2 <= (L6470_MAX_ACC * ((float)1.4552e1)))
jsanchez 6:7ca2eef852fd 576 return (uint16_t)(Step_s2 / ((float)1.4552e1));
jsanchez 6:7ca2eef852fd 577 else
jsanchez 6:7ca2eef852fd 578 return 0;
jsanchez 6:7ca2eef852fd 579 }
jsanchez 6:7ca2eef852fd 580
jsanchez 6:7ca2eef852fd 581 float L6470DC::L6470_Dec_2_Step_s2(uint16_t Dec)
jsanchez 6:7ca2eef852fd 582 {
jsanchez 6:7ca2eef852fd 583 if (Dec <= L6470_MAX_DEC)
jsanchez 6:7ca2eef852fd 584 return (Dec * ((float)1.4552e1));
jsanchez 6:7ca2eef852fd 585 else
jsanchez 6:7ca2eef852fd 586 return 0;
jsanchez 6:7ca2eef852fd 587 }
jsanchez 6:7ca2eef852fd 588
jsanchez 6:7ca2eef852fd 589 uint16_t L6470DC::L6470_Step_s2_2_Dec(float Step_s2)
jsanchez 6:7ca2eef852fd 590 {
jsanchez 6:7ca2eef852fd 591 if (Step_s2 <= (L6470_MAX_DEC * ((float)1.4552e1)))
jsanchez 6:7ca2eef852fd 592 return (uint16_t)(Step_s2 / ((float)1.4552e1));
jsanchez 6:7ca2eef852fd 593 else
jsanchez 6:7ca2eef852fd 594 return 0;
jsanchez 6:7ca2eef852fd 595 }
jsanchez 6:7ca2eef852fd 596
jsanchez 6:7ca2eef852fd 597 float L6470DC::L6470_MaxSpeed_2_Step_s(uint16_t MaxSpeed)
jsanchez 6:7ca2eef852fd 598 {
jsanchez 6:7ca2eef852fd 599 if (MaxSpeed <= L6470_MAX_MAX_SPEED)
jsanchez 6:7ca2eef852fd 600 return (MaxSpeed * ((float)15.2588));
jsanchez 6:7ca2eef852fd 601 else
jsanchez 6:7ca2eef852fd 602 return 0;
jsanchez 6:7ca2eef852fd 603 }
jsanchez 6:7ca2eef852fd 604
jsanchez 6:7ca2eef852fd 605 uint16_t L6470DC::L6470_Step_s_2_MaxSpeed(float Step_s)
jsanchez 6:7ca2eef852fd 606 {
jsanchez 6:7ca2eef852fd 607 if (Step_s <= (L6470_MAX_MAX_SPEED * ((float)15.2588)))
jsanchez 6:7ca2eef852fd 608 return (uint16_t)(Step_s / ((float)15.2588));
jsanchez 6:7ca2eef852fd 609 else
jsanchez 6:7ca2eef852fd 610 return 0;
jsanchez 6:7ca2eef852fd 611 }
jsanchez 6:7ca2eef852fd 612
jsanchez 6:7ca2eef852fd 613 float L6470DC::L6470_MinSpeed_2_Step_s(uint16_t MinSpeed)
jsanchez 6:7ca2eef852fd 614 {
jsanchez 6:7ca2eef852fd 615 if (MinSpeed <= L6470_MAX_MIN_SPEED)
jsanchez 6:7ca2eef852fd 616 return (MinSpeed * ((float)238.4186e-3));
jsanchez 6:7ca2eef852fd 617 else
jsanchez 6:7ca2eef852fd 618 return 0;
jsanchez 6:7ca2eef852fd 619 }
jsanchez 6:7ca2eef852fd 620
jsanchez 6:7ca2eef852fd 621 uint16_t L6470DC::L6470_Step_s_2_MinSpeed(float Step_s)
jsanchez 6:7ca2eef852fd 622 {
jsanchez 6:7ca2eef852fd 623 if (Step_s <= (L6470_MAX_MIN_SPEED * ((float)238.4186e-3)))
jsanchez 6:7ca2eef852fd 624 return (uint16_t)(Step_s / ((float)238.4186e-3));
jsanchez 6:7ca2eef852fd 625 else
jsanchez 6:7ca2eef852fd 626 return 0;
jsanchez 6:7ca2eef852fd 627 }
jsanchez 6:7ca2eef852fd 628
jsanchez 6:7ca2eef852fd 629 float L6470DC::L6470_FsSpd_2_Step_s(uint16_t FsSpd)
jsanchez 6:7ca2eef852fd 630 {
jsanchez 6:7ca2eef852fd 631 if (FsSpd <= L6470_MAX_FS_SPD)
jsanchez 6:7ca2eef852fd 632 return ((FsSpd+0.5) * ((float)15.25));
jsanchez 6:7ca2eef852fd 633 else
jsanchez 6:7ca2eef852fd 634 return 0;
jsanchez 6:7ca2eef852fd 635 }
jsanchez 6:7ca2eef852fd 636
jsanchez 6:7ca2eef852fd 637 uint16_t L6470DC::L6470_Step_s_2_FsSpd(float Step_s)
jsanchez 6:7ca2eef852fd 638 {
jsanchez 6:7ca2eef852fd 639 if (Step_s <= ((L6470_MAX_FS_SPD+0.5) * ((float)15.25)))
jsanchez 6:7ca2eef852fd 640 return (uint16_t)((float)(Step_s / ((float)15.25)) - (float)0.5);
jsanchez 6:7ca2eef852fd 641 else
jsanchez 6:7ca2eef852fd 642 return 0;
jsanchez 6:7ca2eef852fd 643 }
jsanchez 6:7ca2eef852fd 644
jsanchez 6:7ca2eef852fd 645 float L6470DC::L6470_IntSpeed_2_Step_s(uint16_t IntSpeed)
jsanchez 6:7ca2eef852fd 646 {
jsanchez 6:7ca2eef852fd 647 if (IntSpeed <= L6470_MAX_INT_SPEED)
jsanchez 6:7ca2eef852fd 648 return (IntSpeed * ((float)59.6046e-3));
jsanchez 6:7ca2eef852fd 649 else
jsanchez 6:7ca2eef852fd 650 return 0;
jsanchez 6:7ca2eef852fd 651 }
jsanchez 6:7ca2eef852fd 652
jsanchez 6:7ca2eef852fd 653 uint16_t L6470DC::L6470_Step_s_2_IntSpeed(float Step_s)
jsanchez 6:7ca2eef852fd 654 {
jsanchez 6:7ca2eef852fd 655 if (Step_s <= (L6470_MAX_INT_SPEED * ((float)59.6046e-3)))
jsanchez 6:7ca2eef852fd 656 return (uint16_t)(Step_s / ((float)59.6046e-3));
jsanchez 6:7ca2eef852fd 657 else
jsanchez 6:7ca2eef852fd 658 return 0;
jsanchez 6:7ca2eef852fd 659 }
jsanchez 6:7ca2eef852fd 660
jsanchez 6:7ca2eef852fd 661 float L6470DC::L6470_StSlp_2_s_Step(uint8_t StSlp)
jsanchez 6:7ca2eef852fd 662 {
jsanchez 6:7ca2eef852fd 663 return (StSlp * ((float)1.5686e-5));
jsanchez 6:7ca2eef852fd 664 }
jsanchez 6:7ca2eef852fd 665
jsanchez 6:7ca2eef852fd 666 uint8_t L6470DC::L6470_s_Step_2_StSlp(float s_Step)
jsanchez 6:7ca2eef852fd 667 {
jsanchez 6:7ca2eef852fd 668 if (s_Step <= (L6470_MAX_ST_SLP * ((float)1.5686e-5)))
jsanchez 6:7ca2eef852fd 669 return (uint8_t)(s_Step / ((float)1.5686e-5));
jsanchez 6:7ca2eef852fd 670 else
jsanchez 6:7ca2eef852fd 671 return 0;
jsanchez 6:7ca2eef852fd 672 }
jsanchez 6:7ca2eef852fd 673
jsanchez 6:7ca2eef852fd 674 float L6470DC::L6470_FnSlpAcc_2_s_Step(uint8_t FnSlpAcc)
jsanchez 6:7ca2eef852fd 675 {
jsanchez 6:7ca2eef852fd 676 return (FnSlpAcc * ((float)1.5686e-5));
jsanchez 6:7ca2eef852fd 677 }
jsanchez 6:7ca2eef852fd 678
jsanchez 6:7ca2eef852fd 679 uint8_t L6470DC::L6470_s_Step_2_FnSlpAcc(float s_Step)
jsanchez 6:7ca2eef852fd 680 {
jsanchez 6:7ca2eef852fd 681 if (s_Step <= (L6470_MAX_FN_SLP_ACC * ((float)1.5686e-5)))
jsanchez 6:7ca2eef852fd 682 return (uint8_t)(s_Step / ((float)1.5686e-5));
jsanchez 6:7ca2eef852fd 683 else
jsanchez 6:7ca2eef852fd 684 return 0;
jsanchez 6:7ca2eef852fd 685 }
jsanchez 6:7ca2eef852fd 686
jsanchez 6:7ca2eef852fd 687 float L6470DC::L6470_FnSlpDec_2_s_Step(uint8_t FnSlpDec)
jsanchez 6:7ca2eef852fd 688 {
jsanchez 6:7ca2eef852fd 689 return (FnSlpDec * ((float)1.5686e-5));
jsanchez 6:7ca2eef852fd 690 }
jsanchez 6:7ca2eef852fd 691
jsanchez 6:7ca2eef852fd 692
jsanchez 6:7ca2eef852fd 693 uint8_t L6470DC::L6470_s_Step_2_FnSlpDec(float s_Step)
jsanchez 6:7ca2eef852fd 694 {
jsanchez 6:7ca2eef852fd 695 if (s_Step <= (L6470_MAX_FN_SLP_DEC * ((float)1.5686e-5)))
jsanchez 6:7ca2eef852fd 696 return (uint8_t)(s_Step / ((float)1.5686e-5));
jsanchez 6:7ca2eef852fd 697 else
jsanchez 6:7ca2eef852fd 698 return 0;
jsanchez 6:7ca2eef852fd 699 }
jsanchez 6:7ca2eef852fd 700
jsanchez 6:7ca2eef852fd 701 float L6470DC::L6470_OcdTh_2_mA(uint8_t OcdTh)
jsanchez 6:7ca2eef852fd 702 {
jsanchez 6:7ca2eef852fd 703 if (OcdTh <= L6470_MAX_OCD_TH)
jsanchez 6:7ca2eef852fd 704 return ((OcdTh+1) * ((float)375));
jsanchez 6:7ca2eef852fd 705 else
jsanchez 6:7ca2eef852fd 706 return 0;
jsanchez 6:7ca2eef852fd 707 }
jsanchez 6:7ca2eef852fd 708
jsanchez 6:7ca2eef852fd 709 uint8_t L6470DC::L6470_mA_2_OcdTh(float mA)
jsanchez 6:7ca2eef852fd 710 {
jsanchez 6:7ca2eef852fd 711 float result, decimal;
jsanchez 6:7ca2eef852fd 712
jsanchez 6:7ca2eef852fd 713 if (mA <= ((L6470_MAX_OCD_TH+1) * ((float)375))) {
jsanchez 6:7ca2eef852fd 714 result = (mA / ((float)375));
jsanchez 6:7ca2eef852fd 715 decimal = result - (uint8_t)result;
jsanchez 6:7ca2eef852fd 716
jsanchez 6:7ca2eef852fd 717 if (decimal < (float)0.5)
jsanchez 6:7ca2eef852fd 718 return ((uint8_t)result - 1);
jsanchez 6:7ca2eef852fd 719 else
jsanchez 6:7ca2eef852fd 720 return ((uint8_t)result);
jsanchez 6:7ca2eef852fd 721 } else
jsanchez 6:7ca2eef852fd 722 return 0;
jsanchez 6:7ca2eef852fd 723 }
jsanchez 6:7ca2eef852fd 724
jsanchez 6:7ca2eef852fd 725 float L6470DC::L6470_StallTh_2_mA(uint8_t StallTh)
jsanchez 6:7ca2eef852fd 726 {
jsanchez 6:7ca2eef852fd 727 if (StallTh <= L6470_MAX_STALL_TH)
jsanchez 6:7ca2eef852fd 728 return ((StallTh+1) * ((float)31.25));
jsanchez 6:7ca2eef852fd 729 else
jsanchez 6:7ca2eef852fd 730 return 0; // Warning
jsanchez 6:7ca2eef852fd 731 }
jsanchez 6:7ca2eef852fd 732
jsanchez 6:7ca2eef852fd 733 uint8_t L6470DC::L6470_mA_2_StallTh(float mA)
jsanchez 6:7ca2eef852fd 734 {
jsanchez 6:7ca2eef852fd 735 float result, decimal;
jsanchez 6:7ca2eef852fd 736
jsanchez 6:7ca2eef852fd 737 if (mA <= ((L6470_MAX_STALL_TH+1) * ((float)31.25))) {
jsanchez 6:7ca2eef852fd 738 result = (mA / ((float)31.25));
jsanchez 6:7ca2eef852fd 739 decimal = result - (uint8_t)result;
jsanchez 6:7ca2eef852fd 740
jsanchez 6:7ca2eef852fd 741 if (decimal < (float)0.5)
jsanchez 6:7ca2eef852fd 742 return ((uint8_t)result - 1);
jsanchez 6:7ca2eef852fd 743 else
jsanchez 6:7ca2eef852fd 744 return ((uint8_t)result);
jsanchez 6:7ca2eef852fd 745 } else
jsanchez 6:7ca2eef852fd 746 return 0;
jsanchez 6:7ca2eef852fd 747 }
jsanchez 6:7ca2eef852fd 748