test upload STMicroelectronics L6470
L6470.cpp@1:bb4bb05904cb, 2012-12-25 (annotated)
- Committer:
- Brid
- Date:
- Tue Dec 25 02:46:32 2012 +0000
- Revision:
- 1:bb4bb05904cb
- Parent:
- 0:26699d53cd2a
- Child:
- 2:97c203ed65da
- Child:
- 3:f0e29d94b863
test upload
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 | 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 | 0:26699d53cd2a | 66 | int L6470::GetParam(int param){ |
Brid | 0:26699d53cd2a | 67 | int value = 0; |
Brid | 0:26699d53cd2a | 68 | int n = (param>>8)/8; |
Brid | 0:26699d53cd2a | 69 | int m = (param>>8)%8; |
Brid | 0:26699d53cd2a | 70 | // pc.printf("palam = %x\n",param); |
Brid | 0:26699d53cd2a | 71 | // pc.printf("n = %x\n",n); |
Brid | 0:26699d53cd2a | 72 | // pc.printf("m = %x\n",m); |
Brid | 0:26699d53cd2a | 73 | if(m==0){ |
Brid | 0:26699d53cd2a | 74 | unsigned char temp[n+1]; |
Brid | 0:26699d53cd2a | 75 | for(int i = 0; i < n+1; i++){ |
Brid | 0:26699d53cd2a | 76 | temp[i]=0; |
Brid | 0:26699d53cd2a | 77 | } |
Brid | 0:26699d53cd2a | 78 | temp[n] = 0x20|(unsigned char)(param&0xFF); |
Brid | 0:26699d53cd2a | 79 | send_bytes(temp,sizeof temp/sizeof temp[0]); |
Brid | 0:26699d53cd2a | 80 | while(0 < n--){ |
Brid | 0:26699d53cd2a | 81 | value |= (int)temp[n] << 8*n; |
Brid | 0:26699d53cd2a | 82 | } |
Brid | 0:26699d53cd2a | 83 | }else{ |
Brid | 0:26699d53cd2a | 84 | n++; |
Brid | 0:26699d53cd2a | 85 | unsigned char temp[n+1]; |
Brid | 0:26699d53cd2a | 86 | for(int i = 0; i < n+2; i++){ |
Brid | 0:26699d53cd2a | 87 | temp[i]=0; |
Brid | 0:26699d53cd2a | 88 | } |
Brid | 0:26699d53cd2a | 89 | temp[n] = 0x20|(unsigned char)(param&0xFF); |
Brid | 0:26699d53cd2a | 90 | send_bytes(temp,sizeof temp/sizeof temp[0]); |
Brid | 0:26699d53cd2a | 91 | while(0 < n--){ |
Brid | 0:26699d53cd2a | 92 | value |= (int)temp[n] << 8*n; |
Brid | 0:26699d53cd2a | 93 | } |
Brid | 0:26699d53cd2a | 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 | 0:26699d53cd2a | 191 | SetParam(ABS_POS,RH_ABS_POS); |
Brid | 0:26699d53cd2a | 192 | SetParam(EL_POS,RH_EL_POS); |
Brid | 0:26699d53cd2a | 193 | SetParam(MARK,RH_MARK); |
Brid | 0:26699d53cd2a | 194 | SetParam(SPEED,RH_SPEED); |
Brid | 0:26699d53cd2a | 195 | SetParam(ACC,RH_ACC); |
Brid | 0:26699d53cd2a | 196 | SetParam(DEC,RH_DEC); |
Brid | 0:26699d53cd2a | 197 | SetParam(MAX_SPEED,RH_MAX_SPEED); |
Brid | 0:26699d53cd2a | 198 | SetParam(MIN_SPEED,RH_MIN_SPEED); |
Brid | 0:26699d53cd2a | 199 | SetParam(KVAL_HOLD,RH_KVAL_HOLD); |
Brid | 0:26699d53cd2a | 200 | SetParam(KVAL_RUN,RH_KVAL_RUN); |
Brid | 0:26699d53cd2a | 201 | SetParam(KVAL_ACC,RH_KVAL_ACC); |
Brid | 0:26699d53cd2a | 202 | SetParam(KVAL_DEC,RH_KVAL_DEC); |
Brid | 0:26699d53cd2a | 203 | SetParam(INT_SPD,RH_INT_SPD); |
Brid | 0:26699d53cd2a | 204 | SetParam(ST_SLP,RH_ST_SLP); |
Brid | 0:26699d53cd2a | 205 | SetParam(FN_SLP_ACC,RH_FN_SLP_ACC); |
Brid | 0:26699d53cd2a | 206 | SetParam(FN_SLP_DEC,RH_FN_SLP_DEC); |
Brid | 0:26699d53cd2a | 207 | SetParam(K_THERA,RH_K_THERA); |
Brid | 0:26699d53cd2a | 208 | SetParam(OCR_TH,RH_OCR_TH); |
Brid | 0:26699d53cd2a | 209 | SetParam(STALL_TH,RH_STALL_TH); |
Brid | 0:26699d53cd2a | 210 | SetParam(FS_SPD,RH_FS_SPD); |
Brid | 0:26699d53cd2a | 211 | SetParam(STEP_MODE,RH_STEP_MODE); |
Brid | 0:26699d53cd2a | 212 | SetParam(ARARM_FN,RH_ARARM_FN); |
Brid | 0:26699d53cd2a | 213 | SetParam(CONFIG,RH_CONFIG); |
Brid | 0:26699d53cd2a | 214 | } |
Brid | 0:26699d53cd2a | 215 |