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

LIVS Studio » Code » f401_EASE_7axis_controller

LIVS Studio / Mbed OS f401_EASE_7axis_controller

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

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	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed OS
Created:	03 Nov 2020
Imports:	5
Forks:	0
Commits:	2
Dependents:	0
Dependencies:	1
Followers:	3

main.cpp@1:9a05b0a731f3, 2020-12-01 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning