![](/media/cache/group/BRNLIVSMED_logo-01.png.50x50_q85.png)
마스터 로봇 조종기(EsmaCAT EtherCAT Shield 이용)
Dependencies: EsmacatShield
main.cpp@1:9a05b0a731f3, 2020-12-01 (annotated)
- Committer:
- Jangikjae
- Date:
- Tue Dec 01 01:42:01 2020 +0000
- Revision:
- 1:9a05b0a731f3
- Parent:
- 0:76a0d29416ee
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Jangikjae | 0:76a0d29416ee | 1 | /** |
Jangikjae | 0:76a0d29416ee | 2 | ****************************************************************************** |
Jangikjae | 0:76a0d29416ee | 3 | * @file main.cpp |
Jangikjae | 0:76a0d29416ee | 4 | * @date February 06, 2020 |
Jangikjae | 0:76a0d29416ee | 5 | * @brief mbed test application - Esmacat Shield(EASE) working together with |
Jangikjae | 0:76a0d29416ee | 6 | * Base Board with Arduino UNO form factor as EtherCAT slave. |
Jangikjae | 0:76a0d29416ee | 7 | * With the successful execution of this code the LED on EASE should |
Jangikjae | 0:76a0d29416ee | 8 | * blink. It should also estabilish data transfer between the EtherCAT |
Jangikjae | 0:76a0d29416ee | 9 | * Master of Esmacat on the PC. |
Jangikjae | 0:76a0d29416ee | 10 | * For further information please refer to the tutorials at |
Jangikjae | 0:76a0d29416ee | 11 | * https://www.esmacat.com/tutorials |
Jangikjae | 0:76a0d29416ee | 12 | ****************************************************************************** |
Jangikjae | 0:76a0d29416ee | 13 | |
Jangikjae | 0:76a0d29416ee | 14 | Copyright (c) 2020 https://www.esmacat.com/ |
Jangikjae | 0:76a0d29416ee | 15 | |
Jangikjae | 0:76a0d29416ee | 16 | Licensed under the Apache License, Version 2.0 (the "License"); |
Jangikjae | 0:76a0d29416ee | 17 | you may not use this file except in compliance with the License. |
Jangikjae | 0:76a0d29416ee | 18 | You may obtain a copy of the License at |
Jangikjae | 0:76a0d29416ee | 19 | |
Jangikjae | 0:76a0d29416ee | 20 | http://www.apache.org/licenses/LICENSE-2.0 |
Jangikjae | 0:76a0d29416ee | 21 | |
Jangikjae | 0:76a0d29416ee | 22 | Unless required by applicable law or agreed to in writing, software |
Jangikjae | 0:76a0d29416ee | 23 | distributed under the License is distributed on an "AS IS" BASIS, |
Jangikjae | 0:76a0d29416ee | 24 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
Jangikjae | 0:76a0d29416ee | 25 | See the License for the specific language governing permissions and |
Jangikjae | 0:76a0d29416ee | 26 | limitations under the License. |
Jangikjae | 0:76a0d29416ee | 27 | |
Jangikjae | 0:76a0d29416ee | 28 | EsmacatShield.h - Library for using EtherCAT Arduino Shield by Esmacat(EASE). |
Jangikjae | 0:76a0d29416ee | 29 | Created by Esmacat, 01/22/2020 |
Jangikjae | 0:76a0d29416ee | 30 | |
Jangikjae | 0:76a0d29416ee | 31 | ******************************************************************************* |
Jangikjae | 0:76a0d29416ee | 32 | * @file EsmacatShield.h |
Jangikjae | 0:76a0d29416ee | 33 | ******************************************************************************* |
Jangikjae | 1:9a05b0a731f3 | 34 | LIVSMED |
Jangikjae | 1:9a05b0a731f3 | 35 | @brief 마스터 로봇 조종기로부터 7개의 절대 엔코더의 값을 SSI통신으로 읽는다. |
Jangikjae | 1:9a05b0a731f3 | 36 | 4개의 입력이 있는 풋 페달장치로부터 입력 값을 받는다. |
Jangikjae | 1:9a05b0a731f3 | 37 | 총 8가지의 정보(7개의 엔코더 값 + 선택된 1개의 풋 페달값)를 EtherCAT IC |
Jangikjae | 1:9a05b0a731f3 | 38 | 로 전달한다. |
Jangikjae | 1:9a05b0a731f3 | 39 | |
Jangikjae | 1:9a05b0a731f3 | 40 | ******************************************************************************* |
Jangikjae | 1:9a05b0a731f3 | 41 | |
Jangikjae | 0:76a0d29416ee | 42 | */ |
Jangikjae | 0:76a0d29416ee | 43 | #include "mbed.h" |
Jangikjae | 0:76a0d29416ee | 44 | #include <EsmacatShield.h> //Include EsmacatShield Library |
Jangikjae | 1:9a05b0a731f3 | 45 | #include "keypad.h" |
Jangikjae | 0:76a0d29416ee | 46 | |
Jangikjae | 0:76a0d29416ee | 47 | int counter; |
Jangikjae | 0:76a0d29416ee | 48 | int16_t v[8]; //an array of integer is declared for reading the |
Jangikjae | 0:76a0d29416ee | 49 | //data packet of EtherCAT from EASE 8 registers |
Jangikjae | 1:9a05b0a731f3 | 50 | BufferedSerial pc(USBTX, USBRX); // Configuring the serial port to host PC |
Jangikjae | 0:76a0d29416ee | 51 | Ticker tick1; |
Jangikjae | 0:76a0d29416ee | 52 | |
Jangikjae | 0:76a0d29416ee | 53 | |
Jangikjae | 0:76a0d29416ee | 54 | |
Jangikjae | 0:76a0d29416ee | 55 | SPI spi(D11, D12, D13); // Infineon <->ARM mosi, miso, sclk |
Jangikjae | 0:76a0d29416ee | 56 | SPI spi2(PB_15, PB_14, PB_13); // Encoder <->ARM mosi, miso, sclk |
Jangikjae | 0:76a0d29416ee | 57 | SPI spi3(PC_12, PC_11, PC_10); // Encoder <->ARM mosi, miso, sclk |
Jangikjae | 0:76a0d29416ee | 58 | |
Jangikjae | 0:76a0d29416ee | 59 | DigitalOut selectPin(D10); // D10 is used to drive chip enable low |
Jangikjae | 0:76a0d29416ee | 60 | |
Jangikjae | 0:76a0d29416ee | 61 | DigitalOut CS1(PA_10); // Encoder CS |
Jangikjae | 0:76a0d29416ee | 62 | DigitalOut CS2(PA_8); |
Jangikjae | 0:76a0d29416ee | 63 | DigitalOut CS3(PA_9); |
Jangikjae | 0:76a0d29416ee | 64 | DigitalOut CS4(PC_7); |
Jangikjae | 1:9a05b0a731f3 | 65 | DigitalOut CS5(PB_8); |
Jangikjae | 0:76a0d29416ee | 66 | DigitalOut CS6(PC_1); |
Jangikjae | 0:76a0d29416ee | 67 | DigitalOut CS7(PC_0); |
Jangikjae | 1:9a05b0a731f3 | 68 | |
Jangikjae | 0:76a0d29416ee | 69 | DigitalOut greenLED(PB_0); |
Jangikjae | 1:9a05b0a731f3 | 70 | ////////////////////////// |
Jangikjae | 0:76a0d29416ee | 71 | |
Jangikjae | 1:9a05b0a731f3 | 72 | |
Jangikjae | 1:9a05b0a731f3 | 73 | |
Jangikjae | 1:9a05b0a731f3 | 74 | ////////////////////////// |
Jangikjae | 0:76a0d29416ee | 75 | uint8_t Hbyte_enc1 = 0; |
Jangikjae | 0:76a0d29416ee | 76 | uint8_t Lbyte_enc1 = 0; |
Jangikjae | 0:76a0d29416ee | 77 | uint8_t Hbyte_enc2 = 0; |
Jangikjae | 0:76a0d29416ee | 78 | uint8_t Lbyte_enc2 = 0; |
Jangikjae | 0:76a0d29416ee | 79 | uint8_t Hbyte_enc3 = 0; |
Jangikjae | 0:76a0d29416ee | 80 | uint8_t Lbyte_enc3 = 0; |
Jangikjae | 0:76a0d29416ee | 81 | uint8_t Hbyte_enc4 = 0; |
Jangikjae | 0:76a0d29416ee | 82 | uint8_t Lbyte_enc4 = 0; |
Jangikjae | 0:76a0d29416ee | 83 | uint8_t Hbyte_enc5 = 0; |
Jangikjae | 0:76a0d29416ee | 84 | uint8_t Lbyte_enc5 = 0; |
Jangikjae | 0:76a0d29416ee | 85 | uint8_t Hbyte_enc6 = 0; |
Jangikjae | 0:76a0d29416ee | 86 | uint8_t Lbyte_enc6 = 0; |
Jangikjae | 0:76a0d29416ee | 87 | uint8_t Hbyte_enc7 = 0; |
Jangikjae | 0:76a0d29416ee | 88 | uint8_t Lbyte_enc7 = 0; |
Jangikjae | 0:76a0d29416ee | 89 | |
Jangikjae | 0:76a0d29416ee | 90 | uint16_t Angle_encoder1 = 0; |
Jangikjae | 0:76a0d29416ee | 91 | uint16_t Angle_encoder2 = 0; |
Jangikjae | 0:76a0d29416ee | 92 | uint16_t Angle_encoder3 = 0; |
Jangikjae | 0:76a0d29416ee | 93 | uint16_t Angle_encoder4 = 0; |
Jangikjae | 0:76a0d29416ee | 94 | uint16_t Angle_encoder5 = 0; |
Jangikjae | 0:76a0d29416ee | 95 | uint16_t Angle_encoder6 = 0; |
Jangikjae | 0:76a0d29416ee | 96 | uint16_t Angle_encoder7 = 0; |
Jangikjae | 0:76a0d29416ee | 97 | |
Jangikjae | 0:76a0d29416ee | 98 | uint32_t presentT = 0; |
Jangikjae | 0:76a0d29416ee | 99 | uint32_t previousT = 0; |
Jangikjae | 0:76a0d29416ee | 100 | uint32_t DeltaT = 0; |
Jangikjae | 0:76a0d29416ee | 101 | |
Jangikjae | 1:9a05b0a731f3 | 102 | uint8_t TxdataBuffer[20]; |
Jangikjae | 1:9a05b0a731f3 | 103 | uint8_t KeypadBuffer[4]; |
Jangikjae | 1:9a05b0a731f3 | 104 | uint8_t keydata = 0; |
Jangikjae | 1:9a05b0a731f3 | 105 | uint32_t keypush = 0; |
Jangikjae | 1:9a05b0a731f3 | 106 | uint32_t pre_keypush = 0; |
Jangikjae | 1:9a05b0a731f3 | 107 | uint8_t keystate = 0; |
Jangikjae | 1:9a05b0a731f3 | 108 | uint8_t pushIndex = 0; |
Jangikjae | 1:9a05b0a731f3 | 109 | uint16_t Keypad_data = 0; |
Jangikjae | 1:9a05b0a731f3 | 110 | |
Jangikjae | 0:76a0d29416ee | 111 | uint16_t TestData = 0; |
Jangikjae | 0:76a0d29416ee | 112 | //////////////////////////////////////////////////////////////////////////////// |
Jangikjae | 0:76a0d29416ee | 113 | //////////////////////////////////////////////////////////////////////////////// |
Jangikjae | 0:76a0d29416ee | 114 | void SetSSI(SPI&spi) |
Jangikjae | 0:76a0d29416ee | 115 | { |
Jangikjae | 0:76a0d29416ee | 116 | spi.format(8, 1); |
Jangikjae | 0:76a0d29416ee | 117 | spi.frequency(500000); //500 kHz |
Jangikjae | 0:76a0d29416ee | 118 | } |
Jangikjae | 0:76a0d29416ee | 119 | |
Jangikjae | 1:9a05b0a731f3 | 120 | |
Jangikjae | 0:76a0d29416ee | 121 | void ReadEncoder(SPI&spi, uint8_t &low, uint8_t &high, DigitalOut& CS, uint16_t &angle) |
Jangikjae | 0:76a0d29416ee | 122 | { |
Jangikjae | 0:76a0d29416ee | 123 | CS = 0; |
Jangikjae | 0:76a0d29416ee | 124 | high = spi.write(0x00); |
Jangikjae | 0:76a0d29416ee | 125 | low = spi.write(0x00); |
Jangikjae | 0:76a0d29416ee | 126 | CS = 1; |
Jangikjae | 0:76a0d29416ee | 127 | |
Jangikjae | 0:76a0d29416ee | 128 | uint16_t Data = ((uint16_t)high << 2) + ((uint16_t)low >>6); |
Jangikjae | 0:76a0d29416ee | 129 | high = Data >> 8; |
Jangikjae | 0:76a0d29416ee | 130 | low = Data & 0xff; |
Jangikjae | 1:9a05b0a731f3 | 131 | |
Jangikjae | 1:9a05b0a731f3 | 132 | angle = Data; |
Jangikjae | 1:9a05b0a731f3 | 133 | //angle = (float)Data * (360.0 / 1024.0); |
Jangikjae | 1:9a05b0a731f3 | 134 | //angle = (uint16_t)(angle * 1000); |
Jangikjae | 0:76a0d29416ee | 135 | } |
Jangikjae | 0:76a0d29416ee | 136 | |
Jangikjae | 0:76a0d29416ee | 137 | |
Jangikjae | 1:9a05b0a731f3 | 138 | |
Jangikjae | 0:76a0d29416ee | 139 | //////////////////////////////////////////////////////////////////////////////// |
Jangikjae | 0:76a0d29416ee | 140 | //////////////////////////////////////////////////////////////////////////////// |
Jangikjae | 0:76a0d29416ee | 141 | int main() |
Jangikjae | 0:76a0d29416ee | 142 | { |
Jangikjae | 0:76a0d29416ee | 143 | |
Jangikjae | 0:76a0d29416ee | 144 | EsmacatShield slave(spi, selectPin); //Create an Esmacat slave object with defined |
Jangikjae | 0:76a0d29416ee | 145 | // spi and chip SelectPin |
Jangikjae | 0:76a0d29416ee | 146 | slave.setup_spi(); //Setup SPI for EASE |
Jangikjae | 0:76a0d29416ee | 147 | |
Jangikjae | 0:76a0d29416ee | 148 | SetSSI(spi2); |
Jangikjae | 0:76a0d29416ee | 149 | SetSSI(spi3); |
Jangikjae | 0:76a0d29416ee | 150 | |
Jangikjae | 0:76a0d29416ee | 151 | greenLED = 0; |
Jangikjae | 0:76a0d29416ee | 152 | //tick1.attach(MeasureEncoder, 0.01); |
Jangikjae | 0:76a0d29416ee | 153 | |
Jangikjae | 1:9a05b0a731f3 | 154 | Keypad keypad(PC_3, PC_2, NC, NC, PA_14, PA_13, NC, NC); |
Jangikjae | 1:9a05b0a731f3 | 155 | keypad.enablePullUp(); |
Jangikjae | 1:9a05b0a731f3 | 156 | char key; |
Jangikjae | 1:9a05b0a731f3 | 157 | |
Jangikjae | 1:9a05b0a731f3 | 158 | |
Jangikjae | 0:76a0d29416ee | 159 | while(1) |
Jangikjae | 0:76a0d29416ee | 160 | { |
Jangikjae | 0:76a0d29416ee | 161 | TestData++; |
Jangikjae | 0:76a0d29416ee | 162 | if(TestData == 5) { |
Jangikjae | 0:76a0d29416ee | 163 | TestData = 0; |
Jangikjae | 1:9a05b0a731f3 | 164 | greenLED = !greenLED; |
Jangikjae | 1:9a05b0a731f3 | 165 | } |
Jangikjae | 1:9a05b0a731f3 | 166 | |
Jangikjae | 1:9a05b0a731f3 | 167 | key = keypad.getKey(); |
Jangikjae | 1:9a05b0a731f3 | 168 | if(key != KEY_RELEASED) |
Jangikjae | 1:9a05b0a731f3 | 169 | { |
Jangikjae | 1:9a05b0a731f3 | 170 | keydata = key; |
Jangikjae | 1:9a05b0a731f3 | 171 | |
Jangikjae | 1:9a05b0a731f3 | 172 | if(keydata == '1'){KeypadBuffer[1]= 0xC1;} |
Jangikjae | 1:9a05b0a731f3 | 173 | else if(keydata == '4'){KeypadBuffer[1]=0xC2;} |
Jangikjae | 1:9a05b0a731f3 | 174 | else if(keydata == '2'){KeypadBuffer[1]=0xC3;} |
Jangikjae | 1:9a05b0a731f3 | 175 | else if(keydata == '5'){keypush++;} |
Jangikjae | 1:9a05b0a731f3 | 176 | |
Jangikjae | 1:9a05b0a731f3 | 177 | } |
Jangikjae | 1:9a05b0a731f3 | 178 | |
Jangikjae | 1:9a05b0a731f3 | 179 | if (keypush == pre_keypush){ |
Jangikjae | 1:9a05b0a731f3 | 180 | keypush = 0; |
Jangikjae | 1:9a05b0a731f3 | 181 | } |
Jangikjae | 1:9a05b0a731f3 | 182 | |
Jangikjae | 1:9a05b0a731f3 | 183 | if((keypush > 2)&&(keystate == 0x00)){ |
Jangikjae | 1:9a05b0a731f3 | 184 | KeypadBuffer[2] = 0x00; |
Jangikjae | 1:9a05b0a731f3 | 185 | keystate = 0x00; |
Jangikjae | 1:9a05b0a731f3 | 186 | pushIndex = 1; |
Jangikjae | 0:76a0d29416ee | 187 | } |
Jangikjae | 1:9a05b0a731f3 | 188 | else if((keypush > 2)&&(keystate == 0x01)){ |
Jangikjae | 1:9a05b0a731f3 | 189 | KeypadBuffer[2] = 0x01; |
Jangikjae | 1:9a05b0a731f3 | 190 | keystate = 0x01; |
Jangikjae | 1:9a05b0a731f3 | 191 | pushIndex = 1; |
Jangikjae | 1:9a05b0a731f3 | 192 | } |
Jangikjae | 1:9a05b0a731f3 | 193 | else if ((keypush == 0)&&(keystate == 0x00)){ |
Jangikjae | 1:9a05b0a731f3 | 194 | if(pushIndex == 0){ |
Jangikjae | 1:9a05b0a731f3 | 195 | KeypadBuffer[2] = 0x00; |
Jangikjae | 1:9a05b0a731f3 | 196 | keystate = 0x00; |
Jangikjae | 1:9a05b0a731f3 | 197 | } |
Jangikjae | 1:9a05b0a731f3 | 198 | if(pushIndex == 1){ |
Jangikjae | 1:9a05b0a731f3 | 199 | KeypadBuffer[2] = 0x01; |
Jangikjae | 1:9a05b0a731f3 | 200 | keystate = 0x01; |
Jangikjae | 1:9a05b0a731f3 | 201 | pushIndex = 0; |
Jangikjae | 1:9a05b0a731f3 | 202 | } |
Jangikjae | 1:9a05b0a731f3 | 203 | } |
Jangikjae | 1:9a05b0a731f3 | 204 | else if ((keypush == 0)&&(keystate == 0x01)){ |
Jangikjae | 1:9a05b0a731f3 | 205 | if(pushIndex == 0){ |
Jangikjae | 1:9a05b0a731f3 | 206 | KeypadBuffer[2] = 0x01; |
Jangikjae | 1:9a05b0a731f3 | 207 | keystate = 0x01; |
Jangikjae | 1:9a05b0a731f3 | 208 | } |
Jangikjae | 1:9a05b0a731f3 | 209 | if(pushIndex == 1){ |
Jangikjae | 1:9a05b0a731f3 | 210 | KeypadBuffer[2] = 0x00; |
Jangikjae | 1:9a05b0a731f3 | 211 | keystate = 0x00; |
Jangikjae | 1:9a05b0a731f3 | 212 | pushIndex = 0; |
Jangikjae | 1:9a05b0a731f3 | 213 | } |
Jangikjae | 1:9a05b0a731f3 | 214 | } |
Jangikjae | 1:9a05b0a731f3 | 215 | |
Jangikjae | 1:9a05b0a731f3 | 216 | pre_keypush = keypush; |
Jangikjae | 1:9a05b0a731f3 | 217 | |
Jangikjae | 1:9a05b0a731f3 | 218 | Keypad_data = (uint16_t)(KeypadBuffer[1] << 8) | KeypadBuffer[2]; |
Jangikjae | 0:76a0d29416ee | 219 | |
Jangikjae | 0:76a0d29416ee | 220 | ReadEncoder(spi3, Lbyte_enc4, Hbyte_enc4, CS4, Angle_encoder4); |
Jangikjae | 1:9a05b0a731f3 | 221 | ReadEncoder(spi3, Lbyte_enc5, Hbyte_enc5, CS5, Angle_encoder5); |
Jangikjae | 1:9a05b0a731f3 | 222 | ReadEncoder(spi3, Lbyte_enc6, Hbyte_enc6, CS6, Angle_encoder6); |
Jangikjae | 1:9a05b0a731f3 | 223 | ReadEncoder(spi3, Lbyte_enc7, Hbyte_enc7, CS7, Angle_encoder7); |
Jangikjae | 0:76a0d29416ee | 224 | ReadEncoder(spi2, Lbyte_enc1, Hbyte_enc1, CS1, Angle_encoder1); |
Jangikjae | 0:76a0d29416ee | 225 | ReadEncoder(spi2, Lbyte_enc2, Hbyte_enc2, CS2, Angle_encoder2); |
Jangikjae | 0:76a0d29416ee | 226 | ReadEncoder(spi2, Lbyte_enc3, Hbyte_enc3, CS3, Angle_encoder3); |
Jangikjae | 1:9a05b0a731f3 | 227 | |
Jangikjae | 0:76a0d29416ee | 228 | |
Jangikjae | 0:76a0d29416ee | 229 | slave.write_reg_value(0,Angle_encoder1, true); |
Jangikjae | 0:76a0d29416ee | 230 | slave.write_reg_value(1,Angle_encoder2, true); |
Jangikjae | 0:76a0d29416ee | 231 | slave.write_reg_value(2,Angle_encoder3, true); |
Jangikjae | 0:76a0d29416ee | 232 | slave.write_reg_value(3,Angle_encoder4, true); |
Jangikjae | 0:76a0d29416ee | 233 | slave.write_reg_value(4,Angle_encoder5, true); |
Jangikjae | 0:76a0d29416ee | 234 | slave.write_reg_value(5,Angle_encoder6, true); |
Jangikjae | 0:76a0d29416ee | 235 | slave.write_reg_value(6,Angle_encoder7, true); |
Jangikjae | 1:9a05b0a731f3 | 236 | slave.write_reg_value(7,Keypad_data, true); |
Jangikjae | 1:9a05b0a731f3 | 237 | |
Jangikjae | 1:9a05b0a731f3 | 238 | |
Jangikjae | 1:9a05b0a731f3 | 239 | 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); |
Jangikjae | 1:9a05b0a731f3 | 240 | |
Jangikjae | 1:9a05b0a731f3 | 241 | wait_us(100000); |
Jangikjae | 0:76a0d29416ee | 242 | |
Jangikjae | 0:76a0d29416ee | 243 | } |
Jangikjae | 0:76a0d29416ee | 244 | |
Jangikjae | 0:76a0d29416ee | 245 | } |