CatPot 2015-2016 / Mbed 2 deprecated Stepper_L6470

L6470を使用した秋月のステッピングモータードライバをうごかすライブラリとプログラム，なおもともとあるやつをぱくった上で機能を追加している

Dependencies:   mbed

L6470_lib/L6470.cpp@0:298e718ad4cb, 2014-11-11 (annotated)

Committer:
ryuna
Date:
Tue Nov 11 09:11:47 2014 +0000
Revision:
0:298e718ad4cb
??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 0:298e718ad4cb 1 /*
ryuna 0:298e718ad4cb 2 *mbed L6470 SPI Library
ryuna 0:298e718ad4cb 3 */
ryuna 0:298e718ad4cb 4
ryuna 0:298e718ad4cb 5 #include "mbed.h"
ryuna 0:298e718ad4cb 6 #include "L6470.h"
ryuna 0:298e718ad4cb 7
ryuna 0:298e718ad4cb 8
ryuna 0:298e718ad4cb 9 L6470::L6470(PinName mosi, PinName miso, PinName sclk, PinName cs,PinName busy)
ryuna 0:298e718ad4cb 10 : m_spi(mosi, miso, sclk), m_cs(cs),m_busy(busy,PullUp)
ryuna 0:298e718ad4cb 11 {
ryuna 0:298e718ad4cb 12 //定義時に行う
ryuna 0:298e718ad4cb 13 m_cs = 1;
ryuna 0:298e718ad4cb 14 m_spi.format(8,3);
ryuna 0:298e718ad4cb 15 m_spi.frequency(1000000);
ryuna 0:298e718ad4cb 16
ryuna 0:298e718ad4cb 17 /*///前回の送りデータを流す
ryuna 0:298e718ad4cb 18 send(0x00);
ryuna 0:298e718ad4cb 19 send(0x00);
ryuna 0:298e718ad4cb 20 send(0x00);
ryuna 0:298e718ad4cb 21 send(0x00);
ryuna 0:298e718ad4cb 22 /**/
ryuna 0:298e718ad4cb 23
ryuna 0:298e718ad4cb 24 }
ryuna 0:298e718ad4cb 25 void L6470::send(uint8_t temp)
ryuna 0:298e718ad4cb 26 {
ryuna 0:298e718ad4cb 27 //while(!m_busy){
ryuna 0:298e718ad4cb 28 //}
ryuna 0:298e718ad4cb 29 m_cs = 0;//#cs = 1;
ryuna 0:298e718ad4cb 30 temp = m_spi.write((uint8_t)temp);
ryuna 0:298e718ad4cb 31 m_cs = 1;//#cs = 0;
ryuna 0:298e718ad4cb 32
ryuna 0:298e718ad4cb 33 wait_us(0.5);
ryuna 0:298e718ad4cb 34
ryuna 0:298e718ad4cb 35 }
ryuna 0:298e718ad4cb 36
ryuna 0:298e718ad4cb 37
ryuna 0:298e718ad4cb 38 uint8_t L6470::send_r(uint8_t temp)
ryuna 0:298e718ad4cb 39 {
ryuna 0:298e718ad4cb 40 //while(!m_busy){
ryuna 0:298e718ad4cb 41 //}
ryuna 0:298e718ad4cb 42 m_cs = 0;//#cs = 1;
ryuna 0:298e718ad4cb 43 temp = m_spi.write((uint8_t)temp);
ryuna 0:298e718ad4cb 44 m_cs = 1;//#cs = 0;
ryuna 0:298e718ad4cb 45
ryuna 0:298e718ad4cb 46 wait_us(1);
ryuna 0:298e718ad4cb 47 return temp;
ryuna 0:298e718ad4cb 48 }
ryuna 0:298e718ad4cb 49
ryuna 0:298e718ad4cb 50 void L6470::send_bytes(uint8_t temp[], int i)
ryuna 0:298e718ad4cb 51 {
ryuna 0:298e718ad4cb 52 while(0< i--)
ryuna 0:298e718ad4cb 53 {
ryuna 0:298e718ad4cb 54 send(temp[i]);
ryuna 0:298e718ad4cb 55 }
ryuna 0:298e718ad4cb 56 }
ryuna 0:298e718ad4cb 57
ryuna 0:298e718ad4cb 58 void L6470::send_bytes_r(uint8_t temp[], int i)
ryuna 0:298e718ad4cb 59 {
ryuna 0:298e718ad4cb 60 while(0< i--)
ryuna 0:298e718ad4cb 61 {
ryuna 0:298e718ad4cb 62 temp[i] = send_r(temp[i]);
ryuna 0:298e718ad4cb 63 }
ryuna 0:298e718ad4cb 64 }
ryuna 0:298e718ad4cb 65
ryuna 0:298e718ad4cb 66
ryuna 0:298e718ad4cb 67
ryuna 0:298e718ad4cb 68
ryuna 0:298e718ad4cb 69 void L6470::NOP()
ryuna 0:298e718ad4cb 70 {
ryuna 0:298e718ad4cb 71 send(0x00);
ryuna 0:298e718ad4cb 72
ryuna 0:298e718ad4cb 73 }
ryuna 0:298e718ad4cb 74
ryuna 0:298e718ad4cb 75 void L6470::SetParam(int param, int value)
ryuna 0:298e718ad4cb 76 {
ryuna 0:298e718ad4cb 77 int n = (param>>8)/8;//配列の数
ryuna 0:298e718ad4cb 78 int m = (param>>8)%8;//nは割り算だからあまりの分の値が取れないためこちらをif文に活用
ryuna 0:298e718ad4cb 79 if(!m){
ryuna 0:298e718ad4cb 80 uint8_t temp[n+1];
ryuna 0:298e718ad4cb 81 temp[n] = 0x00 | (uint8_t)(param & 0xFF);
ryuna 0:298e718ad4cb 82 while(0 < n--){//比較後マイナス
ryuna 0:298e718ad4cb 83 temp[n] = (uint8_t)(value >> 8*n )&0xFF;
ryuna 0:298e718ad4cb 84 }
ryuna 0:298e718ad4cb 85 send_bytes(temp,sizeof temp/sizeof temp[0]);
ryuna 0:298e718ad4cb 86
ryuna 0:298e718ad4cb 87 }else{//余りが出た場合.　例 22とか7
ryuna 0:298e718ad4cb 88 uint8_t temp[n+2];
ryuna 0:298e718ad4cb 89 temp[n+1] = 0x00 | (uint8_t)(param & 0xFF);
ryuna 0:298e718ad4cb 90 temp[n] = (uint8_t)(value >> 8*n)&~(0xFF<<m);//FFをずらしたあとで1の補数変換で反転.
ryuna 0:298e718ad4cb 91 while(0 < n--){
ryuna 0:298e718ad4cb 92 temp[n]= (uint8_t)(value >> 8*n)&0xFF;
ryuna 0:298e718ad4cb 93 }
ryuna 0:298e718ad4cb 94 send_bytes(temp,sizeof temp/sizeof temp[0]);
ryuna 0:298e718ad4cb 95 }
ryuna 0:298e718ad4cb 96
ryuna 0:298e718ad4cb 97 }
ryuna 0:298e718ad4cb 98 int L6470::GetParam(int param)
ryuna 0:298e718ad4cb 99 {
ryuna 0:298e718ad4cb 100 int value = 0;
ryuna 0:298e718ad4cb 101 int n = (param>>8)/8;//配列の数
ryuna 0:298e718ad4cb 102 int m = (param>>8)%8;//nは割り算だからあまりの分の値が取れないためこちらをif文に活用
ryuna 0:298e718ad4cb 103
ryuna 0:298e718ad4cb 104 if(!m){
ryuna 0:298e718ad4cb 105 uint8_t temp[n+1];
ryuna 0:298e718ad4cb 106 for(int i = 0; i < n+1; i++){
ryuna 0:298e718ad4cb 107 temp[i] = 0;
ryuna 0:298e718ad4cb 108 }
ryuna 0:298e718ad4cb 109 temp[n] = 0x20|(uint8_t)(param&0xFF);
ryuna 0:298e718ad4cb 110 send_bytes_r(temp,sizeof temp/sizeof temp[0]);
ryuna 0:298e718ad4cb 111 while(0 < n--){
ryuna 0:298e718ad4cb 112 value |= (int)temp[n] << 8*n;
ryuna 0:298e718ad4cb 113 }
ryuna 0:298e718ad4cb 114
ryuna 0:298e718ad4cb 115 }else{
ryuna 0:298e718ad4cb 116 n++;
ryuna 0:298e718ad4cb 117 uint8_t temp[n+1];
ryuna 0:298e718ad4cb 118 for(int i = 0; i < n+2; i++){
ryuna 0:298e718ad4cb 119 temp[i] = 0;
ryuna 0:298e718ad4cb 120 }
ryuna 0:298e718ad4cb 121 temp[n] = 0x20|(uint8_t)(param&0xFF);
ryuna 0:298e718ad4cb 122 send_bytes_r(temp,sizeof temp/sizeof temp[0]);
ryuna 0:298e718ad4cb 123 while(0 < n--){
ryuna 0:298e718ad4cb 124 value |= (int)temp[n]<< 8*n;
ryuna 0:298e718ad4cb 125 }
ryuna 0:298e718ad4cb 126
ryuna 0:298e718ad4cb 127 }
ryuna 0:298e718ad4cb 128
ryuna 0:298e718ad4cb 129 return value;
ryuna 0:298e718ad4cb 130 }
ryuna 0:298e718ad4cb 131
ryuna 0:298e718ad4cb 132
ryuna 0:298e718ad4cb 133
ryuna 0:298e718ad4cb 134 void L6470::Run(bool dir, int speed)
ryuna 0:298e718ad4cb 135 {
ryuna 0:298e718ad4cb 136 uint8_t temp[4];
ryuna 0:298e718ad4cb 137 temp[3] = 0x50|dir;
ryuna 0:298e718ad4cb 138 temp[2] = (uint8_t)(speed >> 16)&0x0F;
ryuna 0:298e718ad4cb 139 temp[1] = (uint8_t)(speed >> 8)&0xFF;
ryuna 0:298e718ad4cb 140 temp[0] = (uint8_t)(speed >> 0)&0xFF;
ryuna 0:298e718ad4cb 141 send_bytes(temp,sizeof temp/sizeof temp[0]);
ryuna 0:298e718ad4cb 142
ryuna 0:298e718ad4cb 143 }
ryuna 0:298e718ad4cb 144
ryuna 0:298e718ad4cb 145 void L6470::StepClock(bool dir)
ryuna 0:298e718ad4cb 146 {
ryuna 0:298e718ad4cb 147 send(0x58|dir);
ryuna 0:298e718ad4cb 148 }
ryuna 0:298e718ad4cb 149
ryuna 0:298e718ad4cb 150 void L6470::Move(bool dir,int n_step )
ryuna 0:298e718ad4cb 151 {
ryuna 0:298e718ad4cb 152 uint8_t temp[4];
ryuna 0:298e718ad4cb 153 temp[3] = 0x40|dir;
ryuna 0:298e718ad4cb 154 temp[2] = (uint8_t)(n_step >> 16)&0x3F;
ryuna 0:298e718ad4cb 155 temp[1] = (uint8_t)(n_step >> 8)&0xFF;
ryuna 0:298e718ad4cb 156 temp[0] = (uint8_t)(n_step >> 0)&0xFF;
ryuna 0:298e718ad4cb 157 send_bytes(temp,sizeof temp/sizeof temp[0]);
ryuna 0:298e718ad4cb 158
ryuna 0:298e718ad4cb 159 }
ryuna 0:298e718ad4cb 160 void L6470::GoTo(int abs_pos)
ryuna 0:298e718ad4cb 161 {
ryuna 0:298e718ad4cb 162 uint8_t temp[4];
ryuna 0:298e718ad4cb 163 temp[3] = 0x60;
ryuna 0:298e718ad4cb 164 temp[2] = (uint8_t)(abs_pos >> 16)&0x3F;
ryuna 0:298e718ad4cb 165 temp[1] = (uint8_t)(abs_pos >> 8)&0xFF;
ryuna 0:298e718ad4cb 166 temp[0] = (uint8_t)(abs_pos >> 0)&0xFF;
ryuna 0:298e718ad4cb 167 send_bytes(temp,sizeof temp/sizeof temp[0]);
ryuna 0:298e718ad4cb 168
ryuna 0:298e718ad4cb 169 }
ryuna 0:298e718ad4cb 170
ryuna 0:298e718ad4cb 171 void L6470::GoTo_dir(bool dir, int abs_pos)
ryuna 0:298e718ad4cb 172 {
ryuna 0:298e718ad4cb 173 uint8_t temp[4];
ryuna 0:298e718ad4cb 174 temp[3] = 0x68|dir;
ryuna 0:298e718ad4cb 175 temp[2] = (uint8_t)(abs_pos >> 16)&0x3F;
ryuna 0:298e718ad4cb 176 temp[1] = (uint8_t)(abs_pos >> 8)&0xFF;
ryuna 0:298e718ad4cb 177 temp[0] = (uint8_t)(abs_pos >> 0)&0xFF;
ryuna 0:298e718ad4cb 178 send_bytes(temp,sizeof temp/sizeof temp[0]);
ryuna 0:298e718ad4cb 179
ryuna 0:298e718ad4cb 180 }
ryuna 0:298e718ad4cb 181
ryuna 0:298e718ad4cb 182 void L6470::GoUntill(bool act, bool dir, int speed)
ryuna 0:298e718ad4cb 183 {
ryuna 0:298e718ad4cb 184 uint8_t temp[4];
ryuna 0:298e718ad4cb 185 temp[3] = 0x82|(act<<8)|dir;
ryuna 0:298e718ad4cb 186 temp[2] = (uint8_t)(speed >> 16)&0x3F;
ryuna 0:298e718ad4cb 187 temp[1] = (uint8_t)(speed >> 8)&0xFF;
ryuna 0:298e718ad4cb 188 temp[0] = (uint8_t)(speed >> 0)&0xFF;
ryuna 0:298e718ad4cb 189 send_bytes(temp,sizeof temp/sizeof temp[0]);
ryuna 0:298e718ad4cb 190 }
ryuna 0:298e718ad4cb 191
ryuna 0:298e718ad4cb 192 void L6470::ReleseSW(bool act,bool dir)
ryuna 0:298e718ad4cb 193 {
ryuna 0:298e718ad4cb 194 send(0x92|(act <<3)|dir);
ryuna 0:298e718ad4cb 195 }
ryuna 0:298e718ad4cb 196 void L6470::GoHome()
ryuna 0:298e718ad4cb 197 {
ryuna 0:298e718ad4cb 198 send(0x70);
ryuna 0:298e718ad4cb 199 }
ryuna 0:298e718ad4cb 200
ryuna 0:298e718ad4cb 201 void L6470::GoMark()
ryuna 0:298e718ad4cb 202 {
ryuna 0:298e718ad4cb 203 send(0x78);
ryuna 0:298e718ad4cb 204 }
ryuna 0:298e718ad4cb 205
ryuna 0:298e718ad4cb 206 void L6470::ResetPos()
ryuna 0:298e718ad4cb 207 {
ryuna 0:298e718ad4cb 208 send(0xD8);
ryuna 0:298e718ad4cb 209 }
ryuna 0:298e718ad4cb 210
ryuna 0:298e718ad4cb 211 void L6470::ResetDevice()
ryuna 0:298e718ad4cb 212 {
ryuna 0:298e718ad4cb 213 send(0xC0);
ryuna 0:298e718ad4cb 214 }
ryuna 0:298e718ad4cb 215
ryuna 0:298e718ad4cb 216 void L6470::SoftStop()
ryuna 0:298e718ad4cb 217 {
ryuna 0:298e718ad4cb 218 send(0xB0);
ryuna 0:298e718ad4cb 219 }
ryuna 0:298e718ad4cb 220
ryuna 0:298e718ad4cb 221 void L6470::HardStop()
ryuna 0:298e718ad4cb 222 {
ryuna 0:298e718ad4cb 223 send(0xB8);
ryuna 0:298e718ad4cb 224 }
ryuna 0:298e718ad4cb 225
ryuna 0:298e718ad4cb 226 void L6470::SoftHiz()
ryuna 0:298e718ad4cb 227 {
ryuna 0:298e718ad4cb 228 send(0xA0);
ryuna 0:298e718ad4cb 229 }
ryuna 0:298e718ad4cb 230
ryuna 0:298e718ad4cb 231 void L6470::HardHiz()
ryuna 0:298e718ad4cb 232 {
ryuna 0:298e718ad4cb 233 send(0xA8);
ryuna 0:298e718ad4cb 234 }
ryuna 0:298e718ad4cb 235
ryuna 0:298e718ad4cb 236 void L6470::Resets()
ryuna 0:298e718ad4cb 237 {
ryuna 0:298e718ad4cb 238 SoftStop();
ryuna 0:298e718ad4cb 239 wait(0.5);
ryuna 0:298e718ad4cb 240 ResetDevice();
ryuna 0:298e718ad4cb 241 SetParam(ABS_POS,INI_ABS_POS);
ryuna 0:298e718ad4cb 242 SetParam(EL_POS,INI_EL_POS);
ryuna 0:298e718ad4cb 243 SetParam(MARK,INI_MARK);
ryuna 0:298e718ad4cb 244 SetParam(SPEED,INI_SPEED);
ryuna 0:298e718ad4cb 245 SetParam(ACC,INI_ACC);
ryuna 0:298e718ad4cb 246 SetParam(DEC,INI_DEC);
ryuna 0:298e718ad4cb 247 SetParam(MAX_SPEED,INI_MAX_SPEED);
ryuna 0:298e718ad4cb 248 SetParam(MIN_SPEED,INI_MIN_SPEED);
ryuna 0:298e718ad4cb 249 SetParam(KVAL_HOLD,INI_KVAL_HOLD);
ryuna 0:298e718ad4cb 250 SetParam(KVAL_RUN,INI_KVAL_RUN);
ryuna 0:298e718ad4cb 251 SetParam(KVAL_ACC,INI_KVAL_ACC);
ryuna 0:298e718ad4cb 252 SetParam(KVAL_DEC,INI_KVAL_DEC);
ryuna 0:298e718ad4cb 253 SetParam(INT_SPD,INI_INT_SPD);
ryuna 0:298e718ad4cb 254 SetParam(ST_SLP,INI_ST_SLP);
ryuna 0:298e718ad4cb 255 SetParam(FN_SLP_ACC,INI_FN_SLP_ACC);
ryuna 0:298e718ad4cb 256 SetParam(FN_SLP_DEC,INI_FN_SLP_DEC);
ryuna 0:298e718ad4cb 257 SetParam(K_THERM,INI_K_THERM);
ryuna 0:298e718ad4cb 258 SetParam(OCD_TH,INI_OCD_TH);
ryuna 0:298e718ad4cb 259 SetParam(STALL_TH,INI_STALL_TH);
ryuna 0:298e718ad4cb 260 SetParam(FS_SPD,INI_FS_SPD);
ryuna 0:298e718ad4cb 261 SetParam(STEP_MODE,INI_STEP_MODE);
ryuna 0:298e718ad4cb 262 SetParam(ALARM_EN,INI_ALARM_EN);
ryuna 0:298e718ad4cb 263 SetParam(CONFIG,INI_CONFIG);
ryuna 0:298e718ad4cb 264
ryuna 0:298e718ad4cb 265 }
ryuna 0:298e718ad4cb 266
ryuna 0:298e718ad4cb 267 void L6470::BusyWait(unsigned int time)
ryuna 0:298e718ad4cb 268 {//BESYが解除されるまで待機
ryuna 0:298e718ad4cb 269 while(!m_busy){
ryuna 0:298e718ad4cb 270 }
ryuna 0:298e718ad4cb 271
ryuna 0:298e718ad4cb 272 wait_ms(time);
ryuna 0:298e718ad4cb 273 }
ryuna 0:298e718ad4cb 274
ryuna 0:298e718ad4cb 275 void L6470::Step(int dosu)
ryuna 0:298e718ad4cb 276 {
ryuna 0:298e718ad4cb 277 //初期値180度とする
ryuna 0:298e718ad4cb 278 int temp;
ryuna 0:298e718ad4cb 279
ryuna 0:298e718ad4cb 280 temp = dosu*200/360;
ryuna 0:298e718ad4cb 281
ryuna 0:298e718ad4cb 282 GoTo(temp);
ryuna 0:298e718ad4cb 283
ryuna 0:298e718ad4cb 284
ryuna 0:298e718ad4cb 285
ryuna 0:298e718ad4cb 286 }
ryuna 0:298e718ad4cb 287
ryuna 0:298e718ad4cb 288
ryuna 0:298e718ad4cb 289
ryuna 0:298e718ad4cb 290
ryuna 0:298e718ad4cb 291
ryuna 0:298e718ad4cb 292
ryuna 0:298e718ad4cb 293
ryuna 0:298e718ad4cb 294
ryuna 0:298e718ad4cb 295
ryuna 0:298e718ad4cb 296