test upload STMicroelectronics L6470
L6470.cpp@2:97c203ed65da, 2014-03-05 (annotated)
- Committer:
- Brid
- Date:
- Wed Mar 05 06:17:55 2014 +0000
- Revision:
- 2:97c203ed65da
- Parent:
- 1:bb4bb05904cb
ver1.01
Who changed what in which revision?
User | Revision | Line number | New 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 | 2:97c203ed65da | 41 | void L6470::SetParam(L6470_Register param,int value){ |
Brid | 2:97c203ed65da | 42 | int length,address; |
Brid | 2:97c203ed65da | 43 | |
Brid | 2:97c203ed65da | 44 | switch(param){ |
Brid | 2:97c203ed65da | 45 | case ABS_POS: |
Brid | 2:97c203ed65da | 46 | length = LEN_ABS_POS; |
Brid | 2:97c203ed65da | 47 | address = ADD_ABS_POS; |
Brid | 2:97c203ed65da | 48 | break; |
Brid | 2:97c203ed65da | 49 | case EL_POS: |
Brid | 2:97c203ed65da | 50 | length = LEN_EL_POS; |
Brid | 2:97c203ed65da | 51 | address = ADD_EL_POS; |
Brid | 2:97c203ed65da | 52 | break; |
Brid | 2:97c203ed65da | 53 | case MARK: |
Brid | 2:97c203ed65da | 54 | length = LEN_MARK; |
Brid | 2:97c203ed65da | 55 | address = ADD_MARK; |
Brid | 2:97c203ed65da | 56 | break; |
Brid | 2:97c203ed65da | 57 | case SPEED: |
Brid | 2:97c203ed65da | 58 | length = LEN_SPEED; |
Brid | 2:97c203ed65da | 59 | address = ADD_SPEED; |
Brid | 2:97c203ed65da | 60 | break; |
Brid | 2:97c203ed65da | 61 | case ACC: |
Brid | 2:97c203ed65da | 62 | length = LEN_ACC; |
Brid | 2:97c203ed65da | 63 | address = ADD_ACC; |
Brid | 2:97c203ed65da | 64 | break; |
Brid | 2:97c203ed65da | 65 | case DEC: |
Brid | 2:97c203ed65da | 66 | length = LEN_DEC; |
Brid | 2:97c203ed65da | 67 | address = ADD_DEC; |
Brid | 2:97c203ed65da | 68 | break; |
Brid | 2:97c203ed65da | 69 | case MAX_SPEED: |
Brid | 2:97c203ed65da | 70 | length = LEN_MAX_SPEED; |
Brid | 2:97c203ed65da | 71 | address = ADD_MAX_SPEED; |
Brid | 2:97c203ed65da | 72 | break; |
Brid | 2:97c203ed65da | 73 | case MIN_SPEED: |
Brid | 2:97c203ed65da | 74 | length = LEN_MIN_SPEED; |
Brid | 2:97c203ed65da | 75 | address = ADD_MIN_SPEED; |
Brid | 2:97c203ed65da | 76 | break; |
Brid | 2:97c203ed65da | 77 | case KVAL_HOLD: |
Brid | 2:97c203ed65da | 78 | length = LEN_KVAL_HOLD; |
Brid | 2:97c203ed65da | 79 | address = ADD_KVAL_HOLD; |
Brid | 2:97c203ed65da | 80 | break; |
Brid | 2:97c203ed65da | 81 | case KVAL_RUN: |
Brid | 2:97c203ed65da | 82 | length = LEN_KVAL_RUN; |
Brid | 2:97c203ed65da | 83 | address = ADD_KVAL_RUN; |
Brid | 2:97c203ed65da | 84 | break; |
Brid | 2:97c203ed65da | 85 | case KVAL_ACC: |
Brid | 2:97c203ed65da | 86 | length = LEN_KVAL_ACC; |
Brid | 2:97c203ed65da | 87 | address = ADD_KVAL_ACC; |
Brid | 2:97c203ed65da | 88 | break; |
Brid | 2:97c203ed65da | 89 | case KVAL_DEC: |
Brid | 2:97c203ed65da | 90 | length = LEN_KVAL_DEC; |
Brid | 2:97c203ed65da | 91 | address = ADD_KVAL_DEC; |
Brid | 2:97c203ed65da | 92 | break; |
Brid | 2:97c203ed65da | 93 | case INT_SPD: |
Brid | 2:97c203ed65da | 94 | length = LEN_INT_SPD; |
Brid | 2:97c203ed65da | 95 | address = ADD_INT_SPD; |
Brid | 2:97c203ed65da | 96 | break; |
Brid | 2:97c203ed65da | 97 | case ST_SLP: |
Brid | 2:97c203ed65da | 98 | length = LEN_ST_SLP; |
Brid | 2:97c203ed65da | 99 | address = ADD_ST_SLP; |
Brid | 2:97c203ed65da | 100 | break; |
Brid | 2:97c203ed65da | 101 | case FN_SLP_ACC: |
Brid | 2:97c203ed65da | 102 | length = LEN_FN_SLP_ACC; |
Brid | 2:97c203ed65da | 103 | address = ADD_FN_SLP_ACC; |
Brid | 2:97c203ed65da | 104 | break; |
Brid | 2:97c203ed65da | 105 | case FN_SLP_DEC: |
Brid | 2:97c203ed65da | 106 | length = LEN_FN_SLP_DEC; |
Brid | 2:97c203ed65da | 107 | address = ADD_FN_SLP_DEC; |
Brid | 2:97c203ed65da | 108 | break; |
Brid | 2:97c203ed65da | 109 | case K_THERA: |
Brid | 2:97c203ed65da | 110 | length = LEN_K_THERA; |
Brid | 2:97c203ed65da | 111 | address = ADD_K_THERA; |
Brid | 2:97c203ed65da | 112 | break; |
Brid | 2:97c203ed65da | 113 | case ADC_OUT: |
Brid | 2:97c203ed65da | 114 | length = LEN_ADC_OUT; |
Brid | 2:97c203ed65da | 115 | address = ADD_ADC_OUT; |
Brid | 2:97c203ed65da | 116 | break; |
Brid | 2:97c203ed65da | 117 | case OCR_TH: |
Brid | 2:97c203ed65da | 118 | length = LEN_OCR_TH; |
Brid | 2:97c203ed65da | 119 | address = ADD_OCR_TH; |
Brid | 2:97c203ed65da | 120 | break; |
Brid | 2:97c203ed65da | 121 | case STALL_TH: |
Brid | 2:97c203ed65da | 122 | length = LEN_STALL_TH; |
Brid | 2:97c203ed65da | 123 | address = ADD_STALL_TH; |
Brid | 2:97c203ed65da | 124 | break; |
Brid | 2:97c203ed65da | 125 | case FS_SPD: |
Brid | 2:97c203ed65da | 126 | length = LEN_FS_SPD; |
Brid | 2:97c203ed65da | 127 | address = ADD_FS_SPD; |
Brid | 2:97c203ed65da | 128 | break; |
Brid | 2:97c203ed65da | 129 | case STEP_MODE: |
Brid | 2:97c203ed65da | 130 | length = LEN_STEP_MODE; |
Brid | 2:97c203ed65da | 131 | address = ADD_STEP_MODE; |
Brid | 2:97c203ed65da | 132 | break; |
Brid | 2:97c203ed65da | 133 | case ARARM_FN: |
Brid | 2:97c203ed65da | 134 | length = LEN_ARARM_FN; |
Brid | 2:97c203ed65da | 135 | address = ADD_ARARM_FN; |
Brid | 2:97c203ed65da | 136 | break; |
Brid | 2:97c203ed65da | 137 | case CONFIG: |
Brid | 2:97c203ed65da | 138 | length = LEN_CONFIG; |
Brid | 2:97c203ed65da | 139 | address = ADD_CONFIG; |
Brid | 2:97c203ed65da | 140 | break; |
Brid | 2:97c203ed65da | 141 | case STATUS: |
Brid | 2:97c203ed65da | 142 | length = LEN_STATUS; |
Brid | 2:97c203ed65da | 143 | address = ADD_STATUS; |
Brid | 2:97c203ed65da | 144 | break; |
Brid | 2:97c203ed65da | 145 | default: |
Brid | 2:97c203ed65da | 146 | length = 0; |
Brid | 2:97c203ed65da | 147 | address = 0; |
Brid | 2:97c203ed65da | 148 | break; |
Brid | 2:97c203ed65da | 149 | |
Brid | 2:97c203ed65da | 150 | } |
Brid | 2:97c203ed65da | 151 | |
Brid | 2:97c203ed65da | 152 | |
Brid | 2:97c203ed65da | 153 | int n = length/8; |
Brid | 2:97c203ed65da | 154 | int m = length%8; |
Brid | 2:97c203ed65da | 155 | |
Brid | 2:97c203ed65da | 156 | |
Brid | 2:97c203ed65da | 157 | |
Brid | 0:26699d53cd2a | 158 | // pc.printf("palam = %x\n",param); |
Brid | 0:26699d53cd2a | 159 | // pc.printf("n = %x\n",n); |
Brid | 0:26699d53cd2a | 160 | // pc.printf("m = %x\n",m); |
Brid | 0:26699d53cd2a | 161 | if(m==0){ |
Brid | 0:26699d53cd2a | 162 | unsigned char temp[n+1]; |
Brid | 2:97c203ed65da | 163 | temp[n] = 0x00|(unsigned char)(address); |
Brid | 0:26699d53cd2a | 164 | while(0 < n--){ |
Brid | 0:26699d53cd2a | 165 | temp[n]=(unsigned char) (value >> 8*n)&0xFF; |
Brid | 0:26699d53cd2a | 166 | } |
Brid | 0:26699d53cd2a | 167 | send_bytes(temp,sizeof temp/sizeof temp[0]); |
Brid | 0:26699d53cd2a | 168 | }else{ |
Brid | 0:26699d53cd2a | 169 | unsigned char temp[n+2]; |
Brid | 2:97c203ed65da | 170 | temp[n+1] = 0x00|(unsigned char)(address); |
Brid | 0:26699d53cd2a | 171 | temp[n] =(unsigned char) (value >> 8*n)&~(0xff<<m); |
Brid | 0:26699d53cd2a | 172 | while(0 < n--){ |
Brid | 0:26699d53cd2a | 173 | temp[n]=(unsigned char) (value >> 8*n)&0xFF; |
Brid | 0:26699d53cd2a | 174 | } |
Brid | 0:26699d53cd2a | 175 | send_bytes(temp,sizeof temp/sizeof temp[0]); |
Brid | 0:26699d53cd2a | 176 | } |
Brid | 0:26699d53cd2a | 177 | } |
Brid | 0:26699d53cd2a | 178 | |
Brid | 0:26699d53cd2a | 179 | |
Brid | 2:97c203ed65da | 180 | |
Brid | 2:97c203ed65da | 181 | |
Brid | 2:97c203ed65da | 182 | int L6470::GetParam(L6470_Register param){ |
Brid | 0:26699d53cd2a | 183 | int value = 0; |
Brid | 2:97c203ed65da | 184 | int length,address; |
Brid | 2:97c203ed65da | 185 | switch(param){ |
Brid | 2:97c203ed65da | 186 | case ABS_POS: |
Brid | 2:97c203ed65da | 187 | length = LEN_ABS_POS; |
Brid | 2:97c203ed65da | 188 | address = ADD_ABS_POS; |
Brid | 2:97c203ed65da | 189 | break; |
Brid | 2:97c203ed65da | 190 | case EL_POS: |
Brid | 2:97c203ed65da | 191 | length = LEN_EL_POS; |
Brid | 2:97c203ed65da | 192 | address = ADD_EL_POS; |
Brid | 2:97c203ed65da | 193 | break; |
Brid | 2:97c203ed65da | 194 | case MARK: |
Brid | 2:97c203ed65da | 195 | length = LEN_MARK; |
Brid | 2:97c203ed65da | 196 | address = ADD_MARK; |
Brid | 2:97c203ed65da | 197 | break; |
Brid | 2:97c203ed65da | 198 | case SPEED: |
Brid | 2:97c203ed65da | 199 | length = LEN_SPEED; |
Brid | 2:97c203ed65da | 200 | address = ADD_SPEED; |
Brid | 2:97c203ed65da | 201 | break; |
Brid | 2:97c203ed65da | 202 | case ACC: |
Brid | 2:97c203ed65da | 203 | length = LEN_ACC; |
Brid | 2:97c203ed65da | 204 | address = ADD_ACC; |
Brid | 2:97c203ed65da | 205 | break; |
Brid | 2:97c203ed65da | 206 | case DEC: |
Brid | 2:97c203ed65da | 207 | length = LEN_DEC; |
Brid | 2:97c203ed65da | 208 | address = ADD_DEC; |
Brid | 2:97c203ed65da | 209 | break; |
Brid | 2:97c203ed65da | 210 | case MAX_SPEED: |
Brid | 2:97c203ed65da | 211 | length = LEN_MAX_SPEED; |
Brid | 2:97c203ed65da | 212 | address = ADD_MAX_SPEED; |
Brid | 2:97c203ed65da | 213 | break; |
Brid | 2:97c203ed65da | 214 | case MIN_SPEED: |
Brid | 2:97c203ed65da | 215 | length = LEN_MIN_SPEED; |
Brid | 2:97c203ed65da | 216 | address = ADD_MIN_SPEED; |
Brid | 2:97c203ed65da | 217 | break; |
Brid | 2:97c203ed65da | 218 | case KVAL_HOLD: |
Brid | 2:97c203ed65da | 219 | length = LEN_KVAL_HOLD; |
Brid | 2:97c203ed65da | 220 | address = ADD_KVAL_HOLD; |
Brid | 2:97c203ed65da | 221 | break; |
Brid | 2:97c203ed65da | 222 | case KVAL_RUN: |
Brid | 2:97c203ed65da | 223 | length = LEN_KVAL_RUN; |
Brid | 2:97c203ed65da | 224 | address = ADD_KVAL_RUN; |
Brid | 2:97c203ed65da | 225 | break; |
Brid | 2:97c203ed65da | 226 | case KVAL_ACC: |
Brid | 2:97c203ed65da | 227 | length = LEN_KVAL_ACC; |
Brid | 2:97c203ed65da | 228 | address = ADD_KVAL_ACC; |
Brid | 2:97c203ed65da | 229 | break; |
Brid | 2:97c203ed65da | 230 | case KVAL_DEC: |
Brid | 2:97c203ed65da | 231 | length = LEN_KVAL_DEC; |
Brid | 2:97c203ed65da | 232 | address = ADD_KVAL_DEC; |
Brid | 2:97c203ed65da | 233 | break; |
Brid | 2:97c203ed65da | 234 | case INT_SPD: |
Brid | 2:97c203ed65da | 235 | length = LEN_INT_SPD; |
Brid | 2:97c203ed65da | 236 | address = ADD_INT_SPD; |
Brid | 2:97c203ed65da | 237 | break; |
Brid | 2:97c203ed65da | 238 | case ST_SLP: |
Brid | 2:97c203ed65da | 239 | length = LEN_ST_SLP; |
Brid | 2:97c203ed65da | 240 | address = ADD_ST_SLP; |
Brid | 2:97c203ed65da | 241 | break; |
Brid | 2:97c203ed65da | 242 | case FN_SLP_ACC: |
Brid | 2:97c203ed65da | 243 | length = LEN_FN_SLP_ACC; |
Brid | 2:97c203ed65da | 244 | address = ADD_FN_SLP_ACC; |
Brid | 2:97c203ed65da | 245 | break; |
Brid | 2:97c203ed65da | 246 | case FN_SLP_DEC: |
Brid | 2:97c203ed65da | 247 | length = LEN_FN_SLP_DEC; |
Brid | 2:97c203ed65da | 248 | address = ADD_FN_SLP_DEC; |
Brid | 2:97c203ed65da | 249 | break; |
Brid | 2:97c203ed65da | 250 | case K_THERA: |
Brid | 2:97c203ed65da | 251 | length = LEN_K_THERA; |
Brid | 2:97c203ed65da | 252 | address = ADD_K_THERA; |
Brid | 2:97c203ed65da | 253 | break; |
Brid | 2:97c203ed65da | 254 | case ADC_OUT: |
Brid | 2:97c203ed65da | 255 | length = LEN_ADC_OUT; |
Brid | 2:97c203ed65da | 256 | address = ADD_ADC_OUT; |
Brid | 2:97c203ed65da | 257 | break; |
Brid | 2:97c203ed65da | 258 | case OCR_TH: |
Brid | 2:97c203ed65da | 259 | length = LEN_OCR_TH; |
Brid | 2:97c203ed65da | 260 | address = ADD_OCR_TH; |
Brid | 2:97c203ed65da | 261 | break; |
Brid | 2:97c203ed65da | 262 | case STALL_TH: |
Brid | 2:97c203ed65da | 263 | length = LEN_STALL_TH; |
Brid | 2:97c203ed65da | 264 | address = ADD_STALL_TH; |
Brid | 2:97c203ed65da | 265 | break; |
Brid | 2:97c203ed65da | 266 | case FS_SPD: |
Brid | 2:97c203ed65da | 267 | length = LEN_FS_SPD; |
Brid | 2:97c203ed65da | 268 | address = ADD_FS_SPD; |
Brid | 2:97c203ed65da | 269 | break; |
Brid | 2:97c203ed65da | 270 | case STEP_MODE: |
Brid | 2:97c203ed65da | 271 | length = LEN_STEP_MODE; |
Brid | 2:97c203ed65da | 272 | address = ADD_STEP_MODE; |
Brid | 2:97c203ed65da | 273 | break; |
Brid | 2:97c203ed65da | 274 | case ARARM_FN: |
Brid | 2:97c203ed65da | 275 | length = LEN_ARARM_FN; |
Brid | 2:97c203ed65da | 276 | address = ADD_ARARM_FN; |
Brid | 2:97c203ed65da | 277 | break; |
Brid | 2:97c203ed65da | 278 | case CONFIG: |
Brid | 2:97c203ed65da | 279 | length = LEN_CONFIG; |
Brid | 2:97c203ed65da | 280 | address = ADD_CONFIG; |
Brid | 2:97c203ed65da | 281 | break; |
Brid | 2:97c203ed65da | 282 | case STATUS: |
Brid | 2:97c203ed65da | 283 | length = LEN_STATUS; |
Brid | 2:97c203ed65da | 284 | address = ADD_STATUS; |
Brid | 2:97c203ed65da | 285 | break; |
Brid | 2:97c203ed65da | 286 | default: |
Brid | 2:97c203ed65da | 287 | length = 0; |
Brid | 2:97c203ed65da | 288 | address = 0; |
Brid | 2:97c203ed65da | 289 | break; |
Brid | 2:97c203ed65da | 290 | |
Brid | 2:97c203ed65da | 291 | } |
Brid | 2:97c203ed65da | 292 | |
Brid | 2:97c203ed65da | 293 | int n = length/8; |
Brid | 2:97c203ed65da | 294 | int m = length%8; |
Brid | 0:26699d53cd2a | 295 | if(m==0){ |
Brid | 0:26699d53cd2a | 296 | unsigned char temp[n+1]; |
Brid | 0:26699d53cd2a | 297 | for(int i = 0; i < n+1; i++){ |
Brid | 0:26699d53cd2a | 298 | temp[i]=0; |
Brid | 0:26699d53cd2a | 299 | } |
Brid | 2:97c203ed65da | 300 | temp[n] = 0x20|(unsigned char)(address); |
Brid | 0:26699d53cd2a | 301 | send_bytes(temp,sizeof temp/sizeof temp[0]); |
Brid | 0:26699d53cd2a | 302 | while(0 < n--){ |
Brid | 0:26699d53cd2a | 303 | value |= (int)temp[n] << 8*n; |
Brid | 0:26699d53cd2a | 304 | } |
Brid | 0:26699d53cd2a | 305 | }else{ |
Brid | 0:26699d53cd2a | 306 | n++; |
Brid | 0:26699d53cd2a | 307 | unsigned char temp[n+1]; |
Brid | 0:26699d53cd2a | 308 | for(int i = 0; i < n+2; i++){ |
Brid | 0:26699d53cd2a | 309 | temp[i]=0; |
Brid | 0:26699d53cd2a | 310 | } |
Brid | 2:97c203ed65da | 311 | temp[n] = 0x20|(unsigned char)(address); |
Brid | 0:26699d53cd2a | 312 | send_bytes(temp,sizeof temp/sizeof temp[0]); |
Brid | 0:26699d53cd2a | 313 | while(0 < n--){ |
Brid | 0:26699d53cd2a | 314 | value |= (int)temp[n] << 8*n; |
Brid | 0:26699d53cd2a | 315 | } |
Brid | 0:26699d53cd2a | 316 | } |
Brid | 2:97c203ed65da | 317 | |
Brid | 0:26699d53cd2a | 318 | return(value); |
Brid | 0:26699d53cd2a | 319 | } |
Brid | 0:26699d53cd2a | 320 | |
Brid | 0:26699d53cd2a | 321 | |
Brid | 0:26699d53cd2a | 322 | |
Brid | 0:26699d53cd2a | 323 | void L6470::Run(unsigned char dir,int spd){ |
Brid | 0:26699d53cd2a | 324 | unsigned char temp[4]; |
Brid | 0:26699d53cd2a | 325 | temp[3] = 0x50|dir; |
Brid | 0:26699d53cd2a | 326 | temp[2] = (unsigned char) (spd >> 16)&0x0F; |
Brid | 0:26699d53cd2a | 327 | temp[1] = (unsigned char) (spd >> 8)&0xFF; |
Brid | 0:26699d53cd2a | 328 | temp[0] = (unsigned char) (spd >> 0)&0xFF; |
Brid | 0:26699d53cd2a | 329 | send_bytes(temp,sizeof temp/sizeof temp[0]); |
Brid | 0:26699d53cd2a | 330 | } |
Brid | 0:26699d53cd2a | 331 | |
Brid | 0:26699d53cd2a | 332 | void L6470::StepClock(unsigned char dir){ |
Brid | 0:26699d53cd2a | 333 | send(0x58|dir); |
Brid | 0:26699d53cd2a | 334 | } |
Brid | 0:26699d53cd2a | 335 | |
Brid | 0:26699d53cd2a | 336 | |
Brid | 0:26699d53cd2a | 337 | void L6470::Move(unsigned char dir,int n_step){ |
Brid | 0:26699d53cd2a | 338 | unsigned char temp[4]; |
Brid | 0:26699d53cd2a | 339 | temp[3] = 0x40|dir; |
Brid | 0:26699d53cd2a | 340 | temp[2] = (unsigned char) (n_step >> 16)&0x3F; |
Brid | 0:26699d53cd2a | 341 | temp[1] = (unsigned char) (n_step >> 8)&0xFF; |
Brid | 0:26699d53cd2a | 342 | temp[0] = (unsigned char) (n_step >> 0)&0xFF; |
Brid | 0:26699d53cd2a | 343 | send_bytes(temp,sizeof temp/sizeof temp[0]); |
Brid | 0:26699d53cd2a | 344 | } |
Brid | 0:26699d53cd2a | 345 | |
Brid | 0:26699d53cd2a | 346 | void L6470::GoTo(int abs_pos){ |
Brid | 0:26699d53cd2a | 347 | unsigned char temp[4]; |
Brid | 0:26699d53cd2a | 348 | temp[3] = 0x60; |
Brid | 0:26699d53cd2a | 349 | temp[2] = (unsigned char) (abs_pos >> 16)&0x3F; |
Brid | 0:26699d53cd2a | 350 | temp[1] = (unsigned char) (abs_pos >> 8)&0xFF; |
Brid | 0:26699d53cd2a | 351 | temp[0] = (unsigned char) (abs_pos >> 0)&0xFF; |
Brid | 0:26699d53cd2a | 352 | send_bytes(temp,sizeof temp/sizeof temp[0]); |
Brid | 0:26699d53cd2a | 353 | } |
Brid | 0:26699d53cd2a | 354 | |
Brid | 0:26699d53cd2a | 355 | |
Brid | 0:26699d53cd2a | 356 | void L6470::GoTo_DIR(unsigned char dir,int abs_pos){ |
Brid | 0:26699d53cd2a | 357 | unsigned char temp[4]; |
Brid | 0:26699d53cd2a | 358 | temp[3] = 0x68|dir; |
Brid | 0:26699d53cd2a | 359 | temp[2] = (unsigned char) (abs_pos >> 16)&0x3F; |
Brid | 0:26699d53cd2a | 360 | temp[1] = (unsigned char) (abs_pos >> 8)&0xFF; |
Brid | 0:26699d53cd2a | 361 | temp[0] = (unsigned char) (abs_pos >> 0)&0xFF; |
Brid | 0:26699d53cd2a | 362 | send_bytes(temp,sizeof temp/sizeof temp[0]); |
Brid | 0:26699d53cd2a | 363 | } |
Brid | 0:26699d53cd2a | 364 | |
Brid | 0:26699d53cd2a | 365 | |
Brid | 0:26699d53cd2a | 366 | void L6470::GoUntil(unsigned char act,unsigned char dir,int spd){ |
Brid | 0:26699d53cd2a | 367 | unsigned char temp[4]; |
Brid | 0:26699d53cd2a | 368 | temp[3] = 0x82|(act << 3)|dir; |
Brid | 0:26699d53cd2a | 369 | temp[2] = (unsigned char) (spd >> 16)&0x0F; |
Brid | 0:26699d53cd2a | 370 | temp[1] = (unsigned char) (spd >> 8)&0xFF; |
Brid | 0:26699d53cd2a | 371 | temp[0] = (unsigned char) (spd >> 0)&0xFF; |
Brid | 0:26699d53cd2a | 372 | send_bytes(temp,sizeof temp/sizeof temp[0]); |
Brid | 0:26699d53cd2a | 373 | } |
Brid | 0:26699d53cd2a | 374 | |
Brid | 0:26699d53cd2a | 375 | void L6470::ReleaseSW(unsigned char act,unsigned char dir){ |
Brid | 0:26699d53cd2a | 376 | send(0x92|(act << 3)|dir); |
Brid | 0:26699d53cd2a | 377 | } |
Brid | 0:26699d53cd2a | 378 | |
Brid | 0:26699d53cd2a | 379 | void L6470::GoHome(){ |
Brid | 0:26699d53cd2a | 380 | send(0x70); |
Brid | 0:26699d53cd2a | 381 | } |
Brid | 0:26699d53cd2a | 382 | |
Brid | 0:26699d53cd2a | 383 | void L6470::GoMark(){ |
Brid | 0:26699d53cd2a | 384 | send(0x78); |
Brid | 0:26699d53cd2a | 385 | } |
Brid | 0:26699d53cd2a | 386 | |
Brid | 0:26699d53cd2a | 387 | void L6470::ResetPos(){ |
Brid | 0:26699d53cd2a | 388 | send(0xD8); |
Brid | 0:26699d53cd2a | 389 | } |
Brid | 0:26699d53cd2a | 390 | |
Brid | 0:26699d53cd2a | 391 | void L6470::ResetDevice(){ |
Brid | 0:26699d53cd2a | 392 | send(0xC0); |
Brid | 0:26699d53cd2a | 393 | } |
Brid | 0:26699d53cd2a | 394 | |
Brid | 0:26699d53cd2a | 395 | void L6470::SoftStop(){ |
Brid | 0:26699d53cd2a | 396 | send(0xB0); |
Brid | 0:26699d53cd2a | 397 | } |
Brid | 0:26699d53cd2a | 398 | |
Brid | 0:26699d53cd2a | 399 | void L6470::HardStop(){ |
Brid | 0:26699d53cd2a | 400 | send(0xB8); |
Brid | 0:26699d53cd2a | 401 | } |
Brid | 0:26699d53cd2a | 402 | |
Brid | 0:26699d53cd2a | 403 | |
Brid | 0:26699d53cd2a | 404 | void L6470::SoftHiZ(){ |
Brid | 0:26699d53cd2a | 405 | send(0xA0); |
Brid | 0:26699d53cd2a | 406 | } |
Brid | 0:26699d53cd2a | 407 | |
Brid | 0:26699d53cd2a | 408 | void L6470::HardHiZ(){ |
Brid | 0:26699d53cd2a | 409 | send(0xA8); |
Brid | 0:26699d53cd2a | 410 | } |
Brid | 0:26699d53cd2a | 411 | |
Brid | 0:26699d53cd2a | 412 | void L6470::Resets(){ |
Brid | 0:26699d53cd2a | 413 | SoftStop(); |
Brid | 2:97c203ed65da | 414 | ResetDevice(); |
Brid | 2:97c203ed65da | 415 | SetParam(ABS_POS,INI_ABS_POS); |
Brid | 2:97c203ed65da | 416 | SetParam(EL_POS,INI_EL_POS); |
Brid | 2:97c203ed65da | 417 | SetParam(MARK,INI_MARK); |
Brid | 2:97c203ed65da | 418 | SetParam(SPEED,INI_SPEED); |
Brid | 2:97c203ed65da | 419 | SetParam(ACC,INI_ACC); |
Brid | 2:97c203ed65da | 420 | SetParam(DEC,INI_DEC); |
Brid | 2:97c203ed65da | 421 | SetParam(MAX_SPEED,INI_MAX_SPEED); |
Brid | 2:97c203ed65da | 422 | SetParam(MIN_SPEED,INI_MIN_SPEED); |
Brid | 2:97c203ed65da | 423 | SetParam(KVAL_HOLD,INI_KVAL_HOLD); |
Brid | 2:97c203ed65da | 424 | SetParam(KVAL_RUN,INI_KVAL_RUN); |
Brid | 2:97c203ed65da | 425 | SetParam(KVAL_ACC,INI_KVAL_ACC); |
Brid | 2:97c203ed65da | 426 | SetParam(KVAL_DEC,INI_KVAL_DEC); |
Brid | 2:97c203ed65da | 427 | SetParam(INT_SPD,INI_INT_SPD); |
Brid | 2:97c203ed65da | 428 | SetParam(ST_SLP,INI_ST_SLP); |
Brid | 2:97c203ed65da | 429 | SetParam(FN_SLP_ACC,INI_FN_SLP_ACC); |
Brid | 2:97c203ed65da | 430 | SetParam(FN_SLP_DEC,INI_FN_SLP_DEC); |
Brid | 2:97c203ed65da | 431 | SetParam(K_THERA,INI_K_THERA); |
Brid | 2:97c203ed65da | 432 | SetParam(OCR_TH,INI_OCR_TH); |
Brid | 2:97c203ed65da | 433 | SetParam(STALL_TH,INI_STALL_TH); |
Brid | 2:97c203ed65da | 434 | SetParam(FS_SPD,INI_FS_SPD); |
Brid | 2:97c203ed65da | 435 | SetParam(STEP_MODE,INI_STEP_MODE); |
Brid | 2:97c203ed65da | 436 | SetParam(ARARM_FN,INI_ARARM_FN); |
Brid | 2:97c203ed65da | 437 | SetParam(CONFIG,INI_CONFIG); |
Brid | 0:26699d53cd2a | 438 | } |
Brid | 0:26699d53cd2a | 439 |