로봇의 Joint & Link의 움직임을 절대 엔코더를 통해 측정하여 Master server에 전송. -RSL(Renishaw) 사의 linear & rotary absolute encoder -BiSS 통신을 SPI통신으로 변환해주는 iC-MB4 사용 -EtherCAT 통신으로 최종 데이터 전송

Dependencies:   EsmacatShield

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

Go to the documentation of this file.
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 }