Brid . / L6470

test upload STMicroelectronics L6470

Dependents:   test L6470exaple

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?

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 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