마스터 로봇 조종기(EsmaCAT EtherCAT Shield 이용)
Dependencies: EsmacatShield
main.cpp
00001 /** 00002 ****************************************************************************** 00003 * @file main.cpp 00004 * @date February 06, 2020 00005 * @brief mbed test application - Esmacat Shield(EASE) working together with 00006 * Base Board with Arduino UNO form factor as EtherCAT slave. 00007 * With the successful execution of this code the LED on EASE should 00008 * blink. It should also estabilish data transfer between the EtherCAT 00009 * Master of Esmacat on the PC. 00010 * For further information please refer to the tutorials at 00011 * https://www.esmacat.com/tutorials 00012 ****************************************************************************** 00013 00014 Copyright (c) 2020 https://www.esmacat.com/ 00015 00016 Licensed under the Apache License, Version 2.0 (the "License"); 00017 you may not use this file except in compliance with the License. 00018 You may obtain a copy of the License at 00019 00020 http://www.apache.org/licenses/LICENSE-2.0 00021 00022 Unless required by applicable law or agreed to in writing, software 00023 distributed under the License is distributed on an "AS IS" BASIS, 00024 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00025 See the License for the specific language governing permissions and 00026 limitations under the License. 00027 00028 EsmacatShield.h - Library for using EtherCAT Arduino Shield by Esmacat(EASE). 00029 Created by Esmacat, 01/22/2020 00030 00031 ******************************************************************************* 00032 * @file EsmacatShield.h 00033 ******************************************************************************* 00034 LIVSMED 00035 @brief 마스터 로봇 조종기로부터 7개의 절대 엔코더의 값을 SSI통신으로 읽는다. 00036 4개의 입력이 있는 풋 페달장치로부터 입력 값을 받는다. 00037 총 8가지의 정보(7개의 엔코더 값 + 선택된 1개의 풋 페달값)를 EtherCAT IC 00038 로 전달한다. 00039 00040 ******************************************************************************* 00041 00042 */ 00043 #include "mbed.h" 00044 #include <EsmacatShield.h> //Include EsmacatShield Library 00045 #include "keypad.h" 00046 00047 int counter; 00048 int16_t v[8]; //an array of integer is declared for reading the 00049 //data packet of EtherCAT from EASE 8 registers 00050 BufferedSerial pc(USBTX, USBRX); // Configuring the serial port to host PC 00051 Ticker tick1; 00052 00053 00054 00055 SPI spi(D11, D12, D13); // Infineon <->ARM mosi, miso, sclk 00056 SPI spi2(PB_15, PB_14, PB_13); // Encoder <->ARM mosi, miso, sclk 00057 SPI spi3(PC_12, PC_11, PC_10); // Encoder <->ARM mosi, miso, sclk 00058 00059 DigitalOut selectPin(D10); // D10 is used to drive chip enable low 00060 00061 DigitalOut CS1(PA_10); // Encoder CS 00062 DigitalOut CS2(PA_8); 00063 DigitalOut CS3(PA_9); 00064 DigitalOut CS4(PC_7); 00065 DigitalOut CS5(PB_8); 00066 DigitalOut CS6(PC_1); 00067 DigitalOut CS7(PC_0); 00068 00069 DigitalOut greenLED(PB_0); 00070 ////////////////////////// 00071 00072 00073 00074 ////////////////////////// 00075 uint8_t Hbyte_enc1 = 0; 00076 uint8_t Lbyte_enc1 = 0; 00077 uint8_t Hbyte_enc2 = 0; 00078 uint8_t Lbyte_enc2 = 0; 00079 uint8_t Hbyte_enc3 = 0; 00080 uint8_t Lbyte_enc3 = 0; 00081 uint8_t Hbyte_enc4 = 0; 00082 uint8_t Lbyte_enc4 = 0; 00083 uint8_t Hbyte_enc5 = 0; 00084 uint8_t Lbyte_enc5 = 0; 00085 uint8_t Hbyte_enc6 = 0; 00086 uint8_t Lbyte_enc6 = 0; 00087 uint8_t Hbyte_enc7 = 0; 00088 uint8_t Lbyte_enc7 = 0; 00089 00090 uint16_t Angle_encoder1 = 0; 00091 uint16_t Angle_encoder2 = 0; 00092 uint16_t Angle_encoder3 = 0; 00093 uint16_t Angle_encoder4 = 0; 00094 uint16_t Angle_encoder5 = 0; 00095 uint16_t Angle_encoder6 = 0; 00096 uint16_t Angle_encoder7 = 0; 00097 00098 uint32_t presentT = 0; 00099 uint32_t previousT = 0; 00100 uint32_t DeltaT = 0; 00101 00102 uint8_t TxdataBuffer[20]; 00103 uint8_t KeypadBuffer[4]; 00104 uint8_t keydata = 0; 00105 uint32_t keypush = 0; 00106 uint32_t pre_keypush = 0; 00107 uint8_t keystate = 0; 00108 uint8_t pushIndex = 0; 00109 uint16_t Keypad_data = 0; 00110 00111 uint16_t TestData = 0; 00112 //////////////////////////////////////////////////////////////////////////////// 00113 //////////////////////////////////////////////////////////////////////////////// 00114 void SetSSI(SPI&spi) 00115 { 00116 spi.format(8, 1); 00117 spi.frequency(500000); //500 kHz 00118 } 00119 00120 00121 void ReadEncoder(SPI&spi, uint8_t &low, uint8_t &high, DigitalOut& CS, uint16_t &angle) 00122 { 00123 CS = 0; 00124 high = spi.write(0x00); 00125 low = spi.write(0x00); 00126 CS = 1; 00127 00128 uint16_t Data = ((uint16_t)high << 2) + ((uint16_t)low >>6); 00129 high = Data >> 8; 00130 low = Data & 0xff; 00131 00132 angle = Data; 00133 //angle = (float)Data * (360.0 / 1024.0); 00134 //angle = (uint16_t)(angle * 1000); 00135 } 00136 00137 00138 00139 //////////////////////////////////////////////////////////////////////////////// 00140 //////////////////////////////////////////////////////////////////////////////// 00141 int main() 00142 { 00143 00144 EsmacatShield slave(spi, selectPin); //Create an Esmacat slave object with defined 00145 // spi and chip SelectPin 00146 slave.setup_spi(); //Setup SPI for EASE 00147 00148 SetSSI(spi2); 00149 SetSSI(spi3); 00150 00151 greenLED = 0; 00152 //tick1.attach(MeasureEncoder, 0.01); 00153 00154 Keypad keypad(PC_3, PC_2, NC, NC, PA_14, PA_13, NC, NC); 00155 keypad.enablePullUp(); 00156 char key; 00157 00158 00159 while(1) 00160 { 00161 TestData++; 00162 if(TestData == 5) { 00163 TestData = 0; 00164 greenLED = !greenLED; 00165 } 00166 00167 key = keypad.getKey(); 00168 if(key != KEY_RELEASED) 00169 { 00170 keydata = key; 00171 00172 if(keydata == '1'){KeypadBuffer[1]= 0xC1;} 00173 else if(keydata == '4'){KeypadBuffer[1]=0xC2;} 00174 else if(keydata == '2'){KeypadBuffer[1]=0xC3;} 00175 else if(keydata == '5'){keypush++;} 00176 00177 } 00178 00179 if (keypush == pre_keypush){ 00180 keypush = 0; 00181 } 00182 00183 if((keypush > 2)&&(keystate == 0x00)){ 00184 KeypadBuffer[2] = 0x00; 00185 keystate = 0x00; 00186 pushIndex = 1; 00187 } 00188 else if((keypush > 2)&&(keystate == 0x01)){ 00189 KeypadBuffer[2] = 0x01; 00190 keystate = 0x01; 00191 pushIndex = 1; 00192 } 00193 else if ((keypush == 0)&&(keystate == 0x00)){ 00194 if(pushIndex == 0){ 00195 KeypadBuffer[2] = 0x00; 00196 keystate = 0x00; 00197 } 00198 if(pushIndex == 1){ 00199 KeypadBuffer[2] = 0x01; 00200 keystate = 0x01; 00201 pushIndex = 0; 00202 } 00203 } 00204 else if ((keypush == 0)&&(keystate == 0x01)){ 00205 if(pushIndex == 0){ 00206 KeypadBuffer[2] = 0x01; 00207 keystate = 0x01; 00208 } 00209 if(pushIndex == 1){ 00210 KeypadBuffer[2] = 0x00; 00211 keystate = 0x00; 00212 pushIndex = 0; 00213 } 00214 } 00215 00216 pre_keypush = keypush; 00217 00218 Keypad_data = (uint16_t)(KeypadBuffer[1] << 8) | KeypadBuffer[2]; 00219 00220 ReadEncoder(spi3, Lbyte_enc4, Hbyte_enc4, CS4, Angle_encoder4); 00221 ReadEncoder(spi3, Lbyte_enc5, Hbyte_enc5, CS5, Angle_encoder5); 00222 ReadEncoder(spi3, Lbyte_enc6, Hbyte_enc6, CS6, Angle_encoder6); 00223 ReadEncoder(spi3, Lbyte_enc7, Hbyte_enc7, CS7, Angle_encoder7); 00224 ReadEncoder(spi2, Lbyte_enc1, Hbyte_enc1, CS1, Angle_encoder1); 00225 ReadEncoder(spi2, Lbyte_enc2, Hbyte_enc2, CS2, Angle_encoder2); 00226 ReadEncoder(spi2, Lbyte_enc3, Hbyte_enc3, CS3, Angle_encoder3); 00227 00228 00229 slave.write_reg_value(0,Angle_encoder1, true); 00230 slave.write_reg_value(1,Angle_encoder2, true); 00231 slave.write_reg_value(2,Angle_encoder3, true); 00232 slave.write_reg_value(3,Angle_encoder4, true); 00233 slave.write_reg_value(4,Angle_encoder5, true); 00234 slave.write_reg_value(5,Angle_encoder6, true); 00235 slave.write_reg_value(6,Angle_encoder7, true); 00236 slave.write_reg_value(7,Keypad_data, true); 00237 00238 00239 printf("ENC1: %d, ENC2: %d, ENC3: %d, ENC4: %d,ENC5: %d, ENC6: %d, ENC7: %d\n", Angle_encoder1, Angle_encoder2, Angle_encoder3, Angle_encoder4,Angle_encoder5, Angle_encoder6, Angle_encoder7); 00240 00241 wait_us(100000); 00242 00243 } 00244 00245 }
Generated on Wed Jul 27 2022 05:58:53 by
1.7.2