Brid . / L6470

test upload STMicroelectronics L6470

Dependents:   test L6470exaple

L6470.cpp@0:26699d53cd2a, 2012-12-25 (annotated)

Committer:
Brid
Date:
Tue Dec 25 02:39:30 2012 +0000
Revision:
0:26699d53cd2a
Child:
1:bb4bb05904cb
test upload

Who changed what in which revision?

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