First Last
/
EtherCAT
EtherCAT slave based on SOES
soes.cpp
- Committer:
- vsluiter
- Date:
- 2014-12-12
- Revision:
- 10:4e9069e5d698
- Parent:
- 9:33673e05639f
- Child:
- 11:166353137b95
File content as of revision 10:4e9069e5d698:
/* * SOES Simple Open EtherCAT Slave * * File : soes.c * Version : 0.9.2 * Date : 22-02-2010 * Copyright (C) 2007-2010 Arthur Ketels * * SOES is free software; you can redistribute it and/or modify it under * the terms of the GNU General Public License version 2 as published by the Free * Software Foundation. * * SOES is distributed in the hope that it will be useful, but WITHOUT ANY * WARRANTY; without even the implied warranty of MERCHANTABILITY or * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. * * As a special exception, if other files instantiate templates or use macros * or inline functions from this file, or you compile this file and link it * with other works to produce a work based on this file, this file does not * by itself cause the resulting work to be covered by the GNU General Public * License. However the source code for this file must still be made available * in accordance with section (3) of the GNU General Public License. * * This exception does not invalidate any other reasons why a work based on * this file might be covered by the GNU General Public License. * * The EtherCAT Technology, the trade name and logo "EtherCAT" are the intellectual * property of, and protected by Beckhoff Automation GmbH. */ /**************************************************** Chip type : STM32F051R8 Clock frequency : 48 MHz *****************************************************/ /* Includes ------------------------------------------------------------------*/ #include "mbed.h" #include "cpuinit.h" #include "utypes.h" #include "esc.h" /* Private typedef -----------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/ #define wd_reset 1000 #define BALANCE_WIDTH 0.435 #define BALANCE_HEIGHT 0.24 /* Private macro -------------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ _ESCvar ESCvar; uint8 APPstate; _MBX MBX[MBXBUFFERS]; _MBXcontrol MBXcontrol[MBXBUFFERS]; uint8 MBXrun=0; uint16 SM2_sml,SM3_sml; _Rbuffer Rb; _Wbuffer Wb; //_Ebuffer Eb; //EEprom uint8 TXPDOsize,RXPDOsize; uint16 wd_ok = 1, wd_cnt = wd_reset; volatile uint8 correct_offset; float FrontRight_offset; float FrontLeft_offset; float BackLeft_offset; float BackRight_offset; float rawFrontLeft; float rawFrontRight; float rawBackLeft; float rawBackRight; float grf; //volatile uint8 diginput; //Serial shoe_serial(SHOE_SERIAL_TX,SHOE_SERIAL_RX); DigitalOut led(LED_PIN); DigitalOut et1100_ss(ET1100_SS); DigitalIn et1100_miso(ET1100_MISO); SPI et1100_spi(ET1100_MOSI,ET1100_MISO,ET1100_SCK); AnalogIn adcFrontLeft(ADC_FL); AnalogIn adcFrontRight(ADC_FR); AnalogIn adcBackLeft(ADC_BL); AnalogIn adcBackRight(ADC_BR); /* Private function prototypes -----------------------------------------------*/ /* Private functions ---------------------------------------------------------*/ /** void ESC_objecthandler(uint16 index, uint8 subindex) \brief Object handler, declared from esc.h, as extern function \param index \param subindex */ void ESC_objecthandler(uint16 index, uint8 subindex); void TXPDO_update(void); void RXPDO_update(void); void DIG_process(void); void sample(void); void ESC_objecthandler(uint16 index, uint8 subindex) { uint8 dummy8; uint16 dummy16; switch (index) { case 0x8000: switch (subindex) { case 0x01: dummy8 = 0;//Eb.setting8;//Write value to EEPROM; eeprom_write_byte(&eedat.setting8, Wb.setting8); break; case 0x02: dummy16 = 0;//Eb.setting16;//Write value to EEPROM; eeprom_write_word(&eedat.setting16, Wb.setting16); break; } break; } } void TXPDO_update(void) { ESC_write(SM3_sma, &Rb, TXPDOsize, &ESCvar.ALevent); } void RXPDO_update(void) { ESC_read(SM2_sma, &Wb, RXPDOsize, &ESCvar.ALevent); } void APP_safeoutput(void) { asm("nop"); //Wb.dout = 0; //DOUTPORT = (Wb.dout >> 4) & 0xf0; } void DIG_process(void) { if (APPstate & APPSTATE_OUTPUT) //output enabled { if (ESCvar.ALevent & ESCREG_ALEVENT_SM2) // SM2 trigger ? { ESCvar.ALevent &= ~ESCREG_ALEVENT_SM2; RXPDO_update(); // dummy output point correct_offset = Wb.correct_offset; if(correct_offset & 0x01) { // led.write(1); FrontLeft_offset = rawFrontLeft; FrontRight_offset = rawFrontRight; BackLeft_offset = rawBackLeft; BackRight_offset = rawBackRight; } // else // led.write(0); wd_cnt = wd_reset; } if (!wd_cnt) { ESC_stopoutput(); // watchdog, invalid outputs ESC_ALerror(ALERR_WATCHDOG); // goto safe-op with error bit set ESC_ALstatus(ESCsafeop | ESCerror); } } else { //wd_ok = 1; wd_cnt = wd_reset; } if (APPstate) //input or output enabled { float fl,fr,br,bl,copx,copy; float total_f; Rb.timestamp = ESCvar.Time; //just some dummy data to test //Rb.counter++; //Rb.diginput = diginput; //Rb.analog[0] = 1; //Rb.analog[1] = 2; fr = rawFrontRight - FrontRight_offset; fl = rawFrontLeft - FrontLeft_offset; br = rawBackRight - BackRight_offset; bl = rawBackLeft - BackLeft_offset; total_f = fr+fl+br+bl; copx = (((fl+bl)-(fr+br))/total_f)*BALANCE_WIDTH; copy = (((fr+fl)-(br+bl))/total_f)*BALANCE_HEIGHT; grf = (total_f)*(4096*(1<<4)*(1./1100.0)); Rb.CoPx = copx; Rb.CoPy = copy; Rb.grf = grf; Rb.FrontRight = rawFrontRight; Rb.FrontLeft = rawFrontLeft; Rb.BackRight = rawBackRight; Rb.BackLeft = rawBackLeft; TXPDO_update(); } } void sample(void) { rawFrontLeft = adcFrontLeft; rawFrontRight = adcFrontRight; rawBackLeft = adcBackLeft; rawBackRight = adcBackRight; } int main(void) { /*!< At this stage the microcontroller clock setting is already configured, this is done through SystemInit() function which is called from startup file (startup_stm32f0xx.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to system_stm32f0xx.c file */ Ticker adc_sampler; cpuinit(); adc_sampler.attach(sample,0.001); TXPDOsize = sizeTXPDO(); RXPDOsize = sizeRXPDO(); wait_ms(200); /*initialize configuration*/ //Eb.setting16 = 0xABCD; //Eb.setting8 = 111; // wait until ESC is started up while ((ESCvar.DLstatus & 0x0001) == 0) ESC_read(ESCREG_DLSTATUS, &ESCvar.DLstatus, sizeof(ESCvar.DLstatus), &ESCvar.ALevent); // reset ESC to init state ESC_ALstatus(ESCinit); ESC_ALerror(ALERR_NONE); ESC_stopmbx(); ESC_stopinput(); ESC_stopoutput(); // application run loop while (1) { ESC_read(ESCREG_LOCALTIME, &ESCvar.Time, sizeof(ESCvar.Time), &ESCvar.ALevent); ESC_ALevent(); ESC_state(); if (ESC_mbxprocess()) { ESC_coeprocess(); ESC_xoeprocess(); } DIG_process(); } } /** void ESC_objecthandler(uint16 index, uint8 subindex) \brief Object handler, declared from esc.h, as extern function \param index \param subindex */ #ifdef USE_FULL_ASSERT /** * @brief Reports the name of the source file and the source line number * where the assert_param error has occurred. * @param file: pointer to the source file name * @param line: assert_param error line source number * @retval None */ void assert_failed(uint8_t* file, uint32_t line) { /* User can add his own implementation to report the file name and line number, ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ /* Infinite loop */ while (1) { } } #endif /** * @} */ /** * @} */ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/