로봇의 Joint & Link의 움직임을 절대 엔코더를 통해 측정하여 Master server에 전송. -RSL(Renishaw) 사의 linear & rotary absolute encoder -BiSS 통신을 SPI통신으로 변환해주는 iC-MB4 사용 -EtherCAT 통신으로 최종 데이터 전송
Dependencies: EsmacatShield
main.cpp
00001 00002 /** 00003 ****************************************************************************** 00004 * @file main.cpp 00005 * @date February 06, 2020 00006 * @brief mbed test application - Esmacat Shield(EASE) working together with 00007 * Base Board with Arduino UNO form factor as EtherCAT slave. 00008 * With the successful execution of this code the LED on EASE should 00009 * blink. It should also estabilish data transfer between the EtherCAT 00010 * Master of Esmacat on the PC. 00011 * For further information please refer to the tutorials at 00012 * https://www.esmacat.com/tutorials 00013 ****************************************************************************** 00014 00015 Copyright (c) 2020 https://www.esmacat.com/ 00016 00017 Licensed under the Apache License, Version 2.0 (the "License"); 00018 you may not use this file except in compliance with the License. 00019 You may obtain a copy of the License at 00020 00021 http://www.apache.org/licenses/LICENSE-2.0 00022 00023 Unless required by applicable law or agreed to in writing, software 00024 distributed under the License is distributed on an "AS IS" BASIS, 00025 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00026 See the License for the specific language governing permissions and 00027 limitations under the License. 00028 00029 EsmacatShield.h - Library for using EtherCAT Arduino Shield by Esmacat(EASE). 00030 Created by Esmacat, 01/22/2020 00031 00032 ******************************************************************************* 00033 * @file EsmacatShield.h 00034 ******************************************************************************* 00035 */ 00036 #include "mbed.h" 00037 //#include "BufferedSerial.h" 00038 #include <EsmacatShield.h> //Include EsmacatShield Library 00039 #include "keypad.h" 00040 00041 00042 int counter; 00043 int16_t v[8]; //an array of integer is declared for reading the 00044 //data packet of EtherCAT from EASE 8 registers 00045 00046 //BufferedSerial pc(PA_2, PA_3, 9600); // Configuring the serial port to host PC 00047 Ticker tick1; 00048 00049 SPI spi(D11, D12, D13); // Infineon <->ARM mosi, miso, sclk 00050 DigitalOut selectPin(D10); // D10 is used to drive chip enable low 00051 00052 00053 SPI spi2(PB_15, PB_14, PB_13); // iC-MB4 Encoder1 & Encoder2 <->ARM mosi, miso, sclk 00054 SPI spi3(PC_12, PC_11, PC_10); // iC-MB4 Encoder1 & Encoder2 <->ARM mosi, miso, sclk 00055 DigitalOut CS1(PA_8); //iC-MB4 Encoder1 CS1; 00056 DigitalOut CS2(PB_1); //iC-MB4 Encoder2 CS2; 00057 DigitalOut CS3(PA_13); //iC-MB4 Encoder2 CS3; 00058 DigitalOut CS4(PA_14); //iC-MB4 Encoder2 CS4; 00059 00060 DigitalOut CS_ENC(PC_0); 00061 00062 DigitalOut NRES1(PH_1); 00063 DigitalOut NRES2(PH_0); 00064 DigitalOut NRES3(PC_15); 00065 DigitalOut NRES4(PC_14); 00066 00067 uint32_t Encoder2_Raw; 00068 uint32_t Encoder2_Filter; 00069 uint32_t Encoder2_Data; 00070 00071 uint32_t Encoder3_Raw; 00072 uint32_t Encoder3_Filter; 00073 uint32_t Encoder3_Data; 00074 00075 uint32_t Encoder1_Raw; 00076 uint32_t Encoder1_Filter; 00077 uint32_t Encoder1_Data; 00078 00079 uint32_t Encoder4_Raw; 00080 uint32_t Encoder4_Filter; 00081 uint32_t Encoder4_Data; 00082 00083 00084 uint16_t E1_HIGH; 00085 uint16_t E1_LOW; 00086 uint16_t E2_HIGH; 00087 uint16_t E2_LOW; 00088 uint16_t E3_HIGH; 00089 uint16_t E3_LOW; 00090 uint16_t E4_HIGH; 00091 uint16_t E4_LOW; 00092 00093 uint8_t Hbyte_enc = 0; 00094 uint8_t Lbyte_enc = 0; 00095 uint16_t Angle_encoder = 0; 00096 00097 #define WRITE_DATA 0x02 00098 #define READ_DATA 0x03 00099 #define READ_STATUS 0x05 00100 #define WRITE_INSTRUCTION 0x07 00101 00102 #define CHSEL 0xE4 00103 #define REGVERS 0xE5 00104 #define FREQ 0xE6 00105 #define CFGCH1 0xED 00106 #define FREQAGS 0xE8 00107 #define BREAK 0x80 00108 #define AGSFREQ 0x81 00109 #define CFGIF 0xF5 00110 #define SCDLEN1 0xC0 00111 #define SELCRCS1 0xC1 00112 #define SCRCSTART1_L 0xC2 00113 #define SCRCSTART1_H 0xC3 00114 #define ACTnSENS 0xEF 00115 #define INSTR 0xF4 00116 00117 uint8_t Cmd; 00118 uint8_t CFG_Data[16]; 00119 uint8_t Read_Data2[32]; 00120 uint8_t Read_Data3[32]; 00121 uint8_t Read_Data1[32]; 00122 uint8_t Read_Data4[32]; 00123 00124 uint8_t RegisterData_RDATA = 0x80; 00125 00126 00127 00128 //////////////////////////////////////////////////////////////////////////////// 00129 //////////////////////////////////////////////////////////////////////////////// 00130 void SetSPI(SPI&spi) 00131 { 00132 spi.format(8, 0); 00133 spi.frequency(1000000); 00134 } 00135 00136 00137 void iC_MB4_WriteInstruction_BREAK(SPI&spi, DigitalOut &CS) 00138 { 00139 /* 00140 Tell Master to stop any previous and start fresh 00141 */ 00142 CS = 0; 00143 wait_us(1); 00144 00145 spi.write(WRITE_INSTRUCTION); 00146 spi.write(BREAK); 00147 00148 CS = 1; 00149 } 00150 00151 00152 void iC_MB4_WriteRegister_CHSEL(SPI&spi, DigitalOut &CS) 00153 { 00154 /* 00155 Set the Channel 1 as the only active channel 00156 */ 00157 CS = 0; 00158 wait_us(10); 00159 00160 spi.write(WRITE_DATA); 00161 spi.write(CHSEL); //0xE4 00162 spi.write(0x01); 00163 00164 CS = 1; 00165 } 00166 00167 00168 void iC_MB4_WriteRegister_REGVERS(SPI&spi, DigitalOut &CS) 00169 { 00170 /* 00171 Set up the channel as BiSS register configuration 00172 The iC-MB4 does support autonomous register communication with BiSS B and BiSS C. 00173 0 - BiSS A/B 00174 1 - BiSS C 00175 */ 00176 uint8_t temp; 00177 temp = 0x01 << 6; //Command/instruction communication 00178 //temp = 0xC0; //Register access 00179 00180 CS = 0; 00181 wait_us(10); 00182 00183 spi.write(WRITE_DATA); 00184 spi.write(REGVERS); //0xE5 00185 spi.write(temp); //h0x40 b0100 0000 00186 00187 CS = 1; 00188 } 00189 00190 00191 void iC_MB4_WriteRegister_FREQ(SPI&spi, DigitalOut &CS) 00192 { 00193 /* 00194 Set the FREQ register bit 4:0 to communicate with encoder 00195 Set 0.5 MHz 00196 RLS - BiSS Mode Clock frequency range: 150kHz to 5 MHz 00197 */ 00198 CS = 0; 00199 wait_us(10); 00200 00201 spi.write(WRITE_DATA); 00202 spi.write(FREQ); //0xE6 00203 spi.write(0x11); // 00204 00205 CS = 1; 00206 } 00207 00208 00209 void iC_MB4_WriteRegister_BiSS_C(SPI&spi, DigitalOut &CS) 00210 { 00211 /* 00212 Set up the communicaton for BiSS_C protocol 00213 0x00 - BiSS B 00214 0x01 - BiSS C 00215 0x02 - SSI 00216 0x03 - Channel is not used 00217 */ 00218 CS = 0; 00219 wait_us(10); 00220 00221 spi.write(WRITE_DATA); 00222 spi.write(CFGCH1); 00223 spi.write(0x01); 00224 00225 CS = 1; 00226 } 00227 00228 00229 void iC_MB4_WriteRegister_BiSS_frame_rate(SPI&spi, DigitalOut &CS) 00230 { 00231 /* 00232 Set up for automatically starting read cycles 00233 The FREQAGS controls the automatic data transmission (AutoGetSens) enabled by the instruction bit AGS 00234 Set sampling rate 16kHz 00235 */ 00236 CS = 0; 00237 wait_us(10); 00238 00239 spi.write(WRITE_DATA); 00240 spi.write(FREQAGS); //0xE8 00241 spi.write(0x7C); //AGSMIN 00242 //spi.write(AGSFREQ); // 00243 00244 CS = 1; 00245 } 00246 00247 00248 void iC_MB4_WriteRegister_RS422Mode(SPI&spi, DigitalOut &CS) 00249 { 00250 /* 00251 Set up for RS422 line level 00252 */ 00253 uint8_t temp; 00254 temp = 0x09; 00255 00256 CS = 0; 00257 wait_us(10); 00258 00259 spi.write(WRITE_DATA); 00260 spi.write(CFGIF); 00261 spi.write(temp); 00262 00263 CS = 1; 00264 } 00265 00266 00267 void iC_MB4_WriteRegister_SetSCD(SPI&spi, DigitalOut &CS) 00268 { 00269 /* 00270 Configure the data length of the SCD. 00271 Resolution: MB029 17B 00272 -> 17bits of position + 2 status bits(Error+Warning) + 6 bits CRC = 25 bits long data packet. 00273 Resolution: MB039 19B 00274 -> 19bits of position + 2 status bits(Error+Warning) + 6 bits CRC = 27 bits long data packet. 00275 */ 00276 uint8_t temp; 00277 uint8_t temp1; 00278 uint8_t temp2; 00279 temp = 0x22 |0x40; //24b 00280 temp1 = 0x19 |0x40; //25bits 00281 temp2 = 0x1B |0x40; //27bits 00282 00283 00284 CS = 0; 00285 wait_us(10); 00286 00287 spi.write(WRITE_DATA); 00288 spi.write(SCDLEN1); //0xC0 00289 spi.write(temp1); 00290 00291 CS = 1; 00292 } 00293 00294 00295 void iC_MB4_WriteRegister_SetSCD2(SPI&spi, DigitalOut &CS) 00296 { 00297 /* 00298 Configure the data length of the SCD. 00299 Resolution: MB029 17B 00300 -> 17bits of position + 2 status bits(Error+Warning) + 6 bits CRC = 25 bits long data packet. 00301 Resolution: MB039 19B 00302 -> 19bits of position + 2 status bits(Error+Warning) + 6 bits CRC = 27 bits long data packet. 00303 */ 00304 uint8_t temp; 00305 uint8_t temp1; 00306 uint8_t temp2; 00307 temp = 0x22 |0x40; //24b 00308 temp1 = 0x19 |0x40; //25bits 00309 temp2 = 0x1B |0x40; //27bits 00310 00311 00312 CS = 0; 00313 wait_us(10); 00314 00315 spi.write(WRITE_DATA); 00316 spi.write(SCDLEN1); //0xC0 00317 spi.write(temp); 00318 00319 CS = 1; 00320 } 00321 00322 00323 void iC_MB4_WriteRegister_SELCRCS(SPI&spi, DigitalOut &CS) 00324 { 00325 /* 00326 Configure the CRC info 00327 */ 00328 uint8_t temp; 00329 temp = (0x00 << 7) |0x06; 00330 //temp = (0x01 << 7) |0x21; 00331 00332 CS = 0; 00333 wait_us(10); 00334 00335 spi.write(WRITE_DATA); 00336 spi.write(SELCRCS1); //0xC1 00337 spi.write(temp); //Code: 6 00338 00339 CS = 1; 00340 } 00341 00342 00343 void iC_MB4_WriteRegister_SCRCSTART1_L(SPI&spi, DigitalOut &CS) 00344 { 00345 /* 00346 Configure the CRC start 00347 */ 00348 CS = 0; 00349 wait_us(10); 00350 00351 spi.write(WRITE_DATA); 00352 spi.write(SCRCSTART1_L); 00353 spi.write(0x00); 00354 00355 CS = 1; 00356 } 00357 00358 00359 void iC_MB4_WriteRegister_SCRCSTART1_H(SPI&spi, DigitalOut &CS) 00360 { 00361 /* 00362 Configure the CRC start 00363 */ 00364 CS = 0; 00365 wait_us(10); 00366 00367 spi.write(WRITE_DATA); 00368 spi.write(SCRCSTART1_H); 00369 spi.write(0x00); 00370 00371 CS = 1; 00372 } 00373 00374 00375 void iC_MB4_WriteRegister_ACTnSENS(SPI&spi, DigitalOut &CS) 00376 { 00377 /* 00378 Configure all slaves to be sensors 00379 */ 00380 CS = 0; 00381 wait_us(10); 00382 00383 spi.write(WRITE_DATA); 00384 spi.write(ACTnSENS); 00385 spi.write(0x00); 00386 00387 CS = 1; 00388 } 00389 00390 00391 void iC_MB4_WriteInstruction_INSTR(SPI&spi, DigitalOut &CS) 00392 { 00393 /* 00394 Enable the AGS (Automatic Get Sensor) bit so that the iC-MB4 now polls 00395 00396 */ 00397 CS = 0; 00398 wait_us(10); 00399 uint8_t temp; 00400 00401 // spi.write(READ_DATA); 00402 // spi.write(0xF4); 00403 // temp = spi.write(0x00); 00404 00405 // wait_us(10); 00406 temp = temp | 0x01; 00407 00408 spi.write(0x07); 00409 //spi.write(0xF4); //Instruction Register 0xF4 00410 spi.write(0x01); 00411 00412 CS = 1; 00413 } 00414 00415 00416 void ReadiCMB4(SPI&spi,uint8_t cmd, uint8_t &data, DigitalOut& CS) 00417 { 00418 CS = 0; 00419 wait_us(10); 00420 00421 spi.write(READ_DATA); 00422 spi.write(cmd); 00423 data = spi.write(0x00); 00424 00425 CS = 1; 00426 } 00427 00428 00429 void iCMB4_Read_SensorData(SPI&spi, uint8_t Read_Data[], DigitalOut& CS) 00430 { 00431 CS = 0; 00432 wait_us(10); 00433 00434 spi.write(READ_DATA); 00435 spi.write(0x00); //SCDATA Addr 0x00 전송 00436 Read_Data[0] = spi.write(0x01); //SCDATA Addr 0x01 전송 & 0x00번지의 값 읽음 00437 Read_Data[1] = spi.write(0x02); //SCDATA Addr 0x02 전송 & 0x01번지의 값 읽음 00438 Read_Data[2] = spi.write(0x03); //0x02번지의 값 읽음 00439 Read_Data[3] = spi.write(0x04); 00440 Read_Data[4] = spi.write(0x05); 00441 Read_Data[5] = spi.write(0x06); 00442 Read_Data[6] = spi.write(0x07); 00443 Read_Data[7] = spi.write(0x00); 00444 00445 CS = 1; 00446 CS = 1; 00447 } 00448 00449 //only camera robot 00450 void ReadEncoder(SPI&spi, uint8_t &low, uint8_t &high, DigitalOut& CS, uint16_t &angle) 00451 { 00452 CS = 0; 00453 high = spi.write(0x00); 00454 low = spi.write(0x00); 00455 CS = 1; 00456 00457 uint16_t Data = ((uint16_t)high << 2) + ((uint16_t)low >>6); 00458 high = Data >> 8; 00459 low = Data & 0xff; 00460 //angle = (float)Data * (360.0 / 1024.0); 00461 angle = (uint16_t) angle * 1; 00462 } 00463 00464 //////////////////////////////////////////////////////////////////////////////// 00465 //////////////////////////////////////////////////////////////////////////////// 00466 int main() 00467 { 00468 CS1 = 1; 00469 CS2 = 1; 00470 CS3 = 1; 00471 CS4 = 1; //SPI CS HIGH 00472 //CS_ENC = 1; 00473 00474 EsmacatShield slave(spi, selectPin); 00475 slave.setup_spi(); 00476 00477 SetSPI(spi2); //Setup SPI for iC-MB4 00478 SetSPI(spi3); 00479 00480 00481 00482 wait_us(100000); 00483 00484 00485 //iC-MB4 Configuration 00486 iC_MB4_WriteInstruction_BREAK(spi3, CS3); 00487 Cmd = 0xF4; 00488 ReadiCMB4(spi3, Cmd, CFG_Data[0], CS3); 00489 00490 00491 iC_MB4_WriteRegister_CHSEL(spi3, CS3); 00492 Cmd = CHSEL; 00493 ReadiCMB4(spi3, Cmd, CFG_Data[1], CS3); 00494 00495 00496 iC_MB4_WriteRegister_REGVERS(spi3, CS3); 00497 Cmd = REGVERS; 00498 ReadiCMB4(spi3, Cmd, CFG_Data[2], CS3); 00499 00500 00501 iC_MB4_WriteRegister_FREQ(spi3, CS3); 00502 Cmd = FREQ; 00503 ReadiCMB4(spi3, Cmd, CFG_Data[3], CS3); 00504 00505 00506 iC_MB4_WriteRegister_BiSS_C(spi3, CS3); 00507 Cmd = CFGCH1; 00508 ReadiCMB4(spi3, Cmd, CFG_Data[4], CS3); 00509 00510 00511 iC_MB4_WriteRegister_BiSS_frame_rate(spi3, CS3); 00512 Cmd = FREQAGS; 00513 ReadiCMB4(spi3, Cmd, CFG_Data[5], CS3); 00514 00515 00516 iC_MB4_WriteRegister_RS422Mode(spi3, CS3); 00517 Cmd = CFGIF; 00518 ReadiCMB4(spi3, Cmd, CFG_Data[6], CS3); 00519 00520 00521 iC_MB4_WriteRegister_SetSCD2(spi3, CS3); 00522 Cmd = SCDLEN1; 00523 ReadiCMB4(spi3, Cmd, CFG_Data[7], CS3); 00524 00525 00526 iC_MB4_WriteRegister_SELCRCS(spi3, CS3); 00527 Cmd = SELCRCS1; 00528 ReadiCMB4(spi3, Cmd, CFG_Data[8], CS3); 00529 00530 00531 iC_MB4_WriteRegister_SCRCSTART1_L(spi3, CS3); 00532 Cmd = SCRCSTART1_L; 00533 ReadiCMB4(spi3, Cmd, CFG_Data[9], CS3); 00534 00535 00536 iC_MB4_WriteRegister_SCRCSTART1_H(spi3, CS3); 00537 Cmd = SCRCSTART1_H; 00538 ReadiCMB4(spi3, Cmd, CFG_Data[10], CS3); 00539 00540 00541 iC_MB4_WriteRegister_ACTnSENS(spi3, CS3); 00542 Cmd = ACTnSENS; 00543 ReadiCMB4(spi3, Cmd, CFG_Data[11], CS3); 00544 00545 00546 iC_MB4_WriteInstruction_INSTR(spi3, CS3); 00547 00548 Cmd = INSTR; 00549 ReadiCMB4(spi3, Cmd, CFG_Data[12], CS3); 00550 00551 //pc.write(CFG_Data, 13); 00552 //wait_us(10000); 00553 00554 00555 //iC-MB4 Configuration 00556 iC_MB4_WriteInstruction_BREAK(spi2, CS2); 00557 Cmd = 0xF4; 00558 ReadiCMB4(spi2, Cmd, CFG_Data[0], CS2); 00559 00560 00561 iC_MB4_WriteRegister_CHSEL(spi2, CS2); 00562 Cmd = CHSEL; 00563 ReadiCMB4(spi2, Cmd, CFG_Data[1], CS2); 00564 00565 00566 iC_MB4_WriteRegister_REGVERS(spi2, CS2); 00567 Cmd = REGVERS; 00568 ReadiCMB4(spi2, Cmd, CFG_Data[2], CS2); 00569 00570 00571 iC_MB4_WriteRegister_FREQ(spi2, CS2); 00572 Cmd = FREQ; 00573 ReadiCMB4(spi2, Cmd, CFG_Data[3], CS2); 00574 00575 00576 iC_MB4_WriteRegister_BiSS_C(spi2, CS2); 00577 Cmd = CFGCH1; 00578 ReadiCMB4(spi2, Cmd, CFG_Data[4], CS2); 00579 00580 00581 iC_MB4_WriteRegister_BiSS_frame_rate(spi2, CS2); 00582 Cmd = FREQAGS; 00583 ReadiCMB4(spi2, Cmd, CFG_Data[5], CS2); 00584 00585 00586 iC_MB4_WriteRegister_RS422Mode(spi2, CS2); 00587 Cmd = CFGIF; 00588 ReadiCMB4(spi2, Cmd, CFG_Data[6], CS2); 00589 00590 00591 iC_MB4_WriteRegister_SetSCD(spi2, CS2); 00592 Cmd = SCDLEN1; 00593 ReadiCMB4(spi2, Cmd, CFG_Data[7], CS2); 00594 00595 00596 iC_MB4_WriteRegister_SELCRCS(spi2, CS2); 00597 Cmd = SELCRCS1; 00598 ReadiCMB4(spi2, Cmd, CFG_Data[8], CS2); 00599 00600 00601 iC_MB4_WriteRegister_SCRCSTART1_L(spi2, CS2); 00602 Cmd = SCRCSTART1_L; 00603 ReadiCMB4(spi2, Cmd, CFG_Data[9], CS2); 00604 00605 00606 iC_MB4_WriteRegister_SCRCSTART1_H(spi2, CS2); 00607 Cmd = SCRCSTART1_H; 00608 ReadiCMB4(spi2, Cmd, CFG_Data[10], CS2); 00609 00610 00611 iC_MB4_WriteRegister_ACTnSENS(spi2, CS2); 00612 Cmd = ACTnSENS; 00613 ReadiCMB4(spi2, Cmd, CFG_Data[11], CS2); 00614 00615 00616 iC_MB4_WriteInstruction_INSTR(spi2, CS2); 00617 00618 Cmd = INSTR; 00619 ReadiCMB4(spi2, Cmd, CFG_Data[12], CS2); 00620 00621 wait_us(1000); 00622 00623 //iC-MB4 Configuration 00624 iC_MB4_WriteInstruction_BREAK(spi2, CS1); 00625 Cmd = 0xF4; 00626 ReadiCMB4(spi2, Cmd, CFG_Data[0], CS1); 00627 00628 00629 iC_MB4_WriteRegister_CHSEL(spi2, CS1); 00630 Cmd = CHSEL; 00631 ReadiCMB4(spi2, Cmd, CFG_Data[1], CS1); 00632 00633 00634 iC_MB4_WriteRegister_REGVERS(spi2, CS1); 00635 Cmd = REGVERS; 00636 ReadiCMB4(spi2, Cmd, CFG_Data[2], CS1); 00637 00638 00639 iC_MB4_WriteRegister_FREQ(spi2, CS1); 00640 Cmd = FREQ; 00641 ReadiCMB4(spi2, Cmd, CFG_Data[3], CS1); 00642 00643 00644 iC_MB4_WriteRegister_BiSS_C(spi2, CS1); 00645 Cmd = CFGCH1; 00646 ReadiCMB4(spi2, Cmd, CFG_Data[4], CS1); 00647 00648 00649 iC_MB4_WriteRegister_BiSS_frame_rate(spi2, CS1); 00650 Cmd = FREQAGS; 00651 ReadiCMB4(spi2, Cmd, CFG_Data[5], CS1); 00652 00653 00654 iC_MB4_WriteRegister_RS422Mode(spi2, CS1); 00655 Cmd = CFGIF; 00656 ReadiCMB4(spi2, Cmd, CFG_Data[6], CS1); 00657 00658 00659 iC_MB4_WriteRegister_SetSCD(spi2, CS1); 00660 Cmd = SCDLEN1; 00661 ReadiCMB4(spi2, Cmd, CFG_Data[7], CS1); 00662 00663 00664 iC_MB4_WriteRegister_SELCRCS(spi2, CS1); 00665 Cmd = SELCRCS1; 00666 ReadiCMB4(spi2, Cmd, CFG_Data[8], CS1); 00667 00668 00669 iC_MB4_WriteRegister_SCRCSTART1_L(spi2, CS1); 00670 Cmd = SCRCSTART1_L; 00671 ReadiCMB4(spi2, Cmd, CFG_Data[9], CS1); 00672 00673 00674 iC_MB4_WriteRegister_SCRCSTART1_H(spi2, CS1); 00675 Cmd = SCRCSTART1_H; 00676 ReadiCMB4(spi2, Cmd, CFG_Data[10], CS1); 00677 00678 00679 iC_MB4_WriteRegister_ACTnSENS(spi2, CS1); 00680 Cmd = ACTnSENS; 00681 ReadiCMB4(spi2, Cmd, CFG_Data[11], CS1); 00682 00683 00684 iC_MB4_WriteInstruction_INSTR(spi2, CS1); 00685 00686 Cmd = INSTR; 00687 ReadiCMB4(spi2, Cmd, CFG_Data[12], CS1); 00688 00689 //pc.write(CFG_Data, 13); 00690 //wait_us(10000); 00691 00692 00693 //iC-MB4 Configuration 00694 iC_MB4_WriteInstruction_BREAK(spi3, CS4); 00695 Cmd = 0xF4; 00696 ReadiCMB4(spi3, Cmd, CFG_Data[0], CS4); 00697 00698 00699 iC_MB4_WriteRegister_CHSEL(spi3, CS4); 00700 Cmd = CHSEL; 00701 ReadiCMB4(spi3, Cmd, CFG_Data[1], CS4); 00702 00703 00704 iC_MB4_WriteRegister_REGVERS(spi3, CS4); 00705 Cmd = REGVERS; 00706 ReadiCMB4(spi3, Cmd, CFG_Data[2], CS4); 00707 00708 00709 iC_MB4_WriteRegister_FREQ(spi3, CS4); 00710 Cmd = FREQ; 00711 ReadiCMB4(spi3, Cmd, CFG_Data[3], CS4); 00712 00713 00714 iC_MB4_WriteRegister_BiSS_C(spi3, CS4); 00715 Cmd = CFGCH1; 00716 ReadiCMB4(spi3, Cmd, CFG_Data[4], CS4); 00717 00718 00719 iC_MB4_WriteRegister_BiSS_frame_rate(spi3, CS4); 00720 Cmd = FREQAGS; 00721 ReadiCMB4(spi3, Cmd, CFG_Data[5], CS4); 00722 00723 00724 iC_MB4_WriteRegister_RS422Mode(spi3, CS4); 00725 Cmd = CFGIF; 00726 ReadiCMB4(spi3, Cmd, CFG_Data[6], CS4); 00727 00728 00729 iC_MB4_WriteRegister_SetSCD2(spi3, CS4); 00730 Cmd = SCDLEN1; 00731 ReadiCMB4(spi3, Cmd, CFG_Data[7], CS4); 00732 00733 00734 iC_MB4_WriteRegister_SELCRCS(spi3, CS4); 00735 Cmd = SELCRCS1; 00736 ReadiCMB4(spi3, Cmd, CFG_Data[8], CS4); 00737 00738 00739 iC_MB4_WriteRegister_SCRCSTART1_L(spi3, CS4); 00740 Cmd = SCRCSTART1_L; 00741 ReadiCMB4(spi3, Cmd, CFG_Data[9], CS4); 00742 00743 00744 iC_MB4_WriteRegister_SCRCSTART1_H(spi3, CS4); 00745 Cmd = SCRCSTART1_H; 00746 ReadiCMB4(spi3, Cmd, CFG_Data[10], CS4); 00747 00748 00749 iC_MB4_WriteRegister_ACTnSENS(spi3, CS4); 00750 Cmd = ACTnSENS; 00751 ReadiCMB4(spi3, Cmd, CFG_Data[11], CS4); 00752 00753 00754 iC_MB4_WriteInstruction_INSTR(spi3, CS4); 00755 00756 Cmd = INSTR; 00757 ReadiCMB4(spi3, Cmd, CFG_Data[12], CS4); 00758 00759 00760 //pc.write(CFG_Data, 13); 00761 wait_us(10000); 00762 00763 00764 00765 00766 while(1) 00767 { 00768 iCMB4_Read_SensorData(spi2,Read_Data1, CS1); 00769 iCMB4_Read_SensorData(spi2,Read_Data2, CS2); 00770 iCMB4_Read_SensorData(spi3,Read_Data3, CS3); //camera robot -> absolute encoder 00771 iCMB4_Read_SensorData(spi3,Read_Data4, CS4); 00772 00773 //only camera robot 00774 //ReadEncoder(spi3, Lbyte_enc, Hbyte_enc, CS_ENC, Angle_encoder); 00775 00776 //Encoder circle1 00777 Encoder2_Raw = ((Read_Data2[3] << 24)| (Read_Data2[2] << 16) | (Read_Data2[1] << 8) | (Read_Data2[0])); 00778 Encoder2_Filter = Encoder2_Raw >> 9; 00779 //Encoder2_Data = (((unsigned long)Encoder2_Filter * 360 * 10000) / 0x1FFFF); // range: 0~360 degree , 0.001 degree 00780 float a = (((float)Encoder2_Filter * 360 * 10000) / 0x1FFFF); // range: 0~360 degree , 0.001 degree 00781 Encoder2_Data=a; 00782 E2_HIGH = Encoder2_Data >> 16; 00783 E2_LOW = Encoder2_Data & 0xffff; 00784 if(Read_Data2[4]==0){ 00785 //정상 상태 00786 } 00787 else { 00788 //엔코더 비정상 상태이므로, 최상위 비트를 1로 바꿔서 전달한다. 00789 E2_HIGH = 0x8000; 00790 } 00791 00792 00793 //Encoder linear2 00794 Encoder3_Raw = ((Read_Data3[3] << 24)| (Read_Data3[2] << 16) | (Read_Data3[1] << 8) | (Read_Data3[0])); 00795 Encoder3_Filter = Encoder3_Raw >> 1; 00796 Encoder3_Data = Encoder3_Filter / 1000; // unit: 0.001 mm 00797 E3_HIGH = Encoder3_Data >> 16; 00798 E3_LOW = Encoder3_Data & 0xffff; 00799 if(Read_Data3[4]==0){ 00800 //정상 상태 00801 } 00802 else { 00803 //엔코더 비정상 상태이므로, 최상위 비트를 1로 바꿔서 전달한다. 00804 E3_HIGH = 0x8000; 00805 } 00806 00807 00808 //Encoder circle2 00809 Encoder1_Raw = ((Read_Data1[3] << 24)| (Read_Data1[2] << 16) | (Read_Data1[1] << 8) | (Read_Data1[0])); 00810 Encoder1_Filter = Encoder1_Raw >> 9; 00811 //Encoder1_Data = ((Encoder1_Filter * 360) / 0x1FFFF)*10000; // range: 0~360 degree, 0.001 resolution 00812 float b = (((float)Encoder1_Filter * 360 * 10000) / 0x1FFFF); // range: 0~360 degree , 0.001 degree 00813 Encoder1_Data = b; 00814 E1_HIGH = Encoder1_Data >> 16; 00815 E1_LOW = Encoder1_Data & 0xffff; 00816 if(Read_Data1[4]==0){ 00817 //정상 상태 00818 } 00819 else { 00820 //엔코더 비정상 상태이므로, 최상위 비트를 1로 바꿔서 전달한다. 00821 E1_HIGH = 0x8000; 00822 } 00823 00824 //Encoder linear1 00825 Encoder4_Raw = ((Read_Data4[3] << 24)| (Read_Data4[2] << 16) | (Read_Data4[1] << 8) | (Read_Data4[0])); 00826 Encoder4_Filter = Encoder4_Raw >> 1; 00827 Encoder4_Data = Encoder4_Filter / 1000; // unit: 0.001 mm 00828 E4_HIGH = Encoder4_Data >> 16; 00829 E4_LOW = Encoder4_Data & 0x0ffff; 00830 if(Read_Data4[4]==0){ 00831 //정상 상태 00832 } 00833 else { 00834 //엔코더 비정상 상태이므로, 최상위 비트를 1로 바꿔서 전달한다. 00835 E4_HIGH = 0x8000; 00836 } 00837 00838 00839 printf("Encoder2: %d, Encoder3: %d\n", Encoder2_Data, Encoder3_Data); 00840 printf("Encoder1: %d, Encoder4: %d\n", Encoder1_Data, Encoder4_Data); 00841 printf("%02X, %02X\n", E1_HIGH, E1_LOW); 00842 //printf("Encoder_States: %d, Encoder4: %d \n", Read_Data4[3], Encoder4_Data); 00843 /* 00844 printf("Encoder3: %d, [4]:%02X", Encoder3_Data, E3_HIGH); 00845 if(Read_Data3[4]==0){ 00846 printf("Good Data: %d\n", Encoder3_Data); 00847 } 00848 else{ 00849 printf("Error/n"); 00850 } 00851 */ 00852 slave.write_reg_value(0,E2_HIGH, true); // 완전 서클 00853 slave.write_reg_value(1,E2_LOW, true); 00854 slave.write_reg_value(2,E4_HIGH, true); // 리니어1 00855 slave.write_reg_value(3,E4_LOW, true); 00856 slave.write_reg_value(4,E1_HIGH, true); // 반원 00857 slave.write_reg_value(5,E1_LOW, true); 00858 slave.write_reg_value(6,E3_HIGH, true); // 리니어2 00859 slave.write_reg_value(7,E3_LOW, true); 00860 00861 //slave.write_reg_value(6,Angle_encoder, true); 00862 //slave.write_reg_value(7,0x00, true); 00863 00864 wait_us(10000); 00865 00866 //printf("%d\n", Angle_encoder); 00867 00868 } 00869 00870 }
Generated on Fri Aug 19 2022 14:31:23 by
1.7.2