마스터 로봇 조종기(EsmaCAT EtherCAT Shield 이용)

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  * @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 }