First Last / Mbed 2 deprecated EtherCAT

EtherCAT slave based on SOES

Dependencies:   mbed

soes.cpp@10:4e9069e5d698, 2014-12-12 (annotated)

Committer:
vsluiter
Date:
Fri Dec 12 12:41:38 2014 +0000
Revision:
10:4e9069e5d698
Parent:
9:33673e05639f
Child:
11:166353137b95
Corrected CoP measurement and corrected grf value;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 1:5e22bf1a3817 1 /*
vsluiter 1:5e22bf1a3817 2 * SOES Simple Open EtherCAT Slave
vsluiter 1:5e22bf1a3817 3 *
vsluiter 1:5e22bf1a3817 4 * File : soes.c
vsluiter 1:5e22bf1a3817 5 * Version : 0.9.2
vsluiter 1:5e22bf1a3817 6 * Date : 22-02-2010
vsluiter 1:5e22bf1a3817 7 * Copyright (C) 2007-2010 Arthur Ketels
vsluiter 1:5e22bf1a3817 8 *
vsluiter 1:5e22bf1a3817 9 * SOES is free software; you can redistribute it and/or modify it under
vsluiter 1:5e22bf1a3817 10 * the terms of the GNU General Public License version 2 as published by the Free
vsluiter 1:5e22bf1a3817 11 * Software Foundation.
vsluiter 1:5e22bf1a3817 12 *
vsluiter 1:5e22bf1a3817 13 * SOES is distributed in the hope that it will be useful, but WITHOUT ANY
vsluiter 1:5e22bf1a3817 14 * WARRANTY; without even the implied warranty of MERCHANTABILITY or
vsluiter 1:5e22bf1a3817 15 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
vsluiter 1:5e22bf1a3817 16 * for more details.
vsluiter 1:5e22bf1a3817 17 *
vsluiter 1:5e22bf1a3817 18 * As a special exception, if other files instantiate templates or use macros
vsluiter 1:5e22bf1a3817 19 * or inline functions from this file, or you compile this file and link it
vsluiter 1:5e22bf1a3817 20 * with other works to produce a work based on this file, this file does not
vsluiter 1:5e22bf1a3817 21 * by itself cause the resulting work to be covered by the GNU General Public
vsluiter 1:5e22bf1a3817 22 * License. However the source code for this file must still be made available
vsluiter 1:5e22bf1a3817 23 * in accordance with section (3) of the GNU General Public License.
vsluiter 1:5e22bf1a3817 24 *
vsluiter 1:5e22bf1a3817 25 * This exception does not invalidate any other reasons why a work based on
vsluiter 1:5e22bf1a3817 26 * this file might be covered by the GNU General Public License.
vsluiter 1:5e22bf1a3817 27 *
vsluiter 1:5e22bf1a3817 28 * The EtherCAT Technology, the trade name and logo "EtherCAT" are the intellectual
vsluiter 1:5e22bf1a3817 29 * property of, and protected by Beckhoff Automation GmbH.
vsluiter 1:5e22bf1a3817 30 */
vsluiter 1:5e22bf1a3817 31
vsluiter 1:5e22bf1a3817 32 /****************************************************
vsluiter 1:5e22bf1a3817 33 Chip type : STM32F051R8
vsluiter 1:5e22bf1a3817 34 Clock frequency : 48 MHz
vsluiter 1:5e22bf1a3817 35 *****************************************************/
vsluiter 1:5e22bf1a3817 36
vsluiter 1:5e22bf1a3817 37 /* Includes ------------------------------------------------------------------*/
vsluiter 1:5e22bf1a3817 38 #include "mbed.h"
vsluiter 1:5e22bf1a3817 39 #include "cpuinit.h"
vsluiter 1:5e22bf1a3817 40 #include "utypes.h"
vsluiter 1:5e22bf1a3817 41 #include "esc.h"
vsluiter 1:5e22bf1a3817 42
vsluiter 1:5e22bf1a3817 43 /* Private typedef -----------------------------------------------------------*/
vsluiter 1:5e22bf1a3817 44 /* Private define ------------------------------------------------------------*/
vsluiter 1:5e22bf1a3817 45 #define wd_reset 1000
vsluiter 1:5e22bf1a3817 46
vsluiter 5:6d75f432a41f 47 #define BALANCE_WIDTH 0.435
vsluiter 5:6d75f432a41f 48 #define BALANCE_HEIGHT 0.24
vsluiter 1:5e22bf1a3817 49 /* Private macro -------------------------------------------------------------*/
vsluiter 1:5e22bf1a3817 50 /* Private variables ---------------------------------------------------------*/
vsluiter 1:5e22bf1a3817 51 _ESCvar ESCvar;
vsluiter 1:5e22bf1a3817 52 uint8 APPstate;
vsluiter 1:5e22bf1a3817 53 _MBX MBX[MBXBUFFERS];
vsluiter 1:5e22bf1a3817 54 _MBXcontrol MBXcontrol[MBXBUFFERS];
vsluiter 1:5e22bf1a3817 55 uint8 MBXrun=0;
vsluiter 1:5e22bf1a3817 56 uint16 SM2_sml,SM3_sml;
vsluiter 1:5e22bf1a3817 57 _Rbuffer Rb;
vsluiter 1:5e22bf1a3817 58 _Wbuffer Wb;
vsluiter 5:6d75f432a41f 59 //_Ebuffer Eb; //EEprom
vsluiter 1:5e22bf1a3817 60 uint8 TXPDOsize,RXPDOsize;
vsluiter 1:5e22bf1a3817 61 uint16 wd_ok = 1, wd_cnt = wd_reset;
vsluiter 5:6d75f432a41f 62 volatile uint8 correct_offset;
vsluiter 5:6d75f432a41f 63 float FrontRight_offset;
vsluiter 5:6d75f432a41f 64 float FrontLeft_offset;
vsluiter 5:6d75f432a41f 65 float BackLeft_offset;
vsluiter 5:6d75f432a41f 66 float BackRight_offset;
vsluiter 5:6d75f432a41f 67 float rawFrontLeft;
vsluiter 5:6d75f432a41f 68 float rawFrontRight;
vsluiter 5:6d75f432a41f 69 float rawBackLeft;
vsluiter 5:6d75f432a41f 70 float rawBackRight;
vsluiter 9:33673e05639f 71 float grf;
vsluiter 5:6d75f432a41f 72 //volatile uint8 diginput;
vsluiter 1:5e22bf1a3817 73
vsluiter 2:3dd1240eb938 74 //Serial shoe_serial(SHOE_SERIAL_TX,SHOE_SERIAL_RX);
vsluiter 1:5e22bf1a3817 75 DigitalOut led(LED_PIN);
vsluiter 1:5e22bf1a3817 76 DigitalOut et1100_ss(ET1100_SS);
vsluiter 1:5e22bf1a3817 77 DigitalIn et1100_miso(ET1100_MISO);
vsluiter 1:5e22bf1a3817 78 SPI et1100_spi(ET1100_MOSI,ET1100_MISO,ET1100_SCK);
vsluiter 5:6d75f432a41f 79 AnalogIn adcFrontLeft(ADC_FL);
vsluiter 5:6d75f432a41f 80 AnalogIn adcFrontRight(ADC_FR);
vsluiter 5:6d75f432a41f 81 AnalogIn adcBackLeft(ADC_BL);
vsluiter 5:6d75f432a41f 82 AnalogIn adcBackRight(ADC_BR);
vsluiter 1:5e22bf1a3817 83
vsluiter 1:5e22bf1a3817 84 /* Private function prototypes -----------------------------------------------*/
vsluiter 1:5e22bf1a3817 85 /* Private functions ---------------------------------------------------------*/
vsluiter 1:5e22bf1a3817 86
vsluiter 1:5e22bf1a3817 87 /** void ESC_objecthandler(uint16 index, uint8 subindex)
vsluiter 1:5e22bf1a3817 88 \brief Object handler, declared from esc.h, as extern function
vsluiter 1:5e22bf1a3817 89 \param index
vsluiter 1:5e22bf1a3817 90 \param subindex
vsluiter 1:5e22bf1a3817 91 */
vsluiter 1:5e22bf1a3817 92 void ESC_objecthandler(uint16 index, uint8 subindex);
vsluiter 1:5e22bf1a3817 93 void TXPDO_update(void);
vsluiter 1:5e22bf1a3817 94 void RXPDO_update(void);
vsluiter 1:5e22bf1a3817 95 void DIG_process(void);
vsluiter 5:6d75f432a41f 96 void sample(void);
vsluiter 1:5e22bf1a3817 97
vsluiter 1:5e22bf1a3817 98 void ESC_objecthandler(uint16 index, uint8 subindex)
vsluiter 1:5e22bf1a3817 99 {
vsluiter 1:5e22bf1a3817 100 uint8 dummy8;
vsluiter 1:5e22bf1a3817 101 uint16 dummy16;
vsluiter 1:5e22bf1a3817 102 switch (index)
vsluiter 1:5e22bf1a3817 103 {
vsluiter 1:5e22bf1a3817 104 case 0x8000:
vsluiter 1:5e22bf1a3817 105 switch (subindex)
vsluiter 1:5e22bf1a3817 106 {
vsluiter 1:5e22bf1a3817 107 case 0x01:
vsluiter 5:6d75f432a41f 108 dummy8 = 0;//Eb.setting8;//Write value to EEPROM; eeprom_write_byte(&eedat.setting8, Wb.setting8);
vsluiter 1:5e22bf1a3817 109 break;
vsluiter 1:5e22bf1a3817 110 case 0x02:
vsluiter 5:6d75f432a41f 111 dummy16 = 0;//Eb.setting16;//Write value to EEPROM; eeprom_write_word(&eedat.setting16, Wb.setting16);
vsluiter 1:5e22bf1a3817 112 break;
vsluiter 1:5e22bf1a3817 113 }
vsluiter 1:5e22bf1a3817 114 break;
vsluiter 1:5e22bf1a3817 115 }
vsluiter 1:5e22bf1a3817 116 }
vsluiter 1:5e22bf1a3817 117
vsluiter 1:5e22bf1a3817 118 void TXPDO_update(void)
vsluiter 1:5e22bf1a3817 119 {
vsluiter 1:5e22bf1a3817 120 ESC_write(SM3_sma, &Rb, TXPDOsize, &ESCvar.ALevent);
vsluiter 1:5e22bf1a3817 121 }
vsluiter 1:5e22bf1a3817 122
vsluiter 1:5e22bf1a3817 123 void RXPDO_update(void)
vsluiter 1:5e22bf1a3817 124 {
vsluiter 1:5e22bf1a3817 125 ESC_read(SM2_sma, &Wb, RXPDOsize, &ESCvar.ALevent);
vsluiter 1:5e22bf1a3817 126 }
vsluiter 1:5e22bf1a3817 127
vsluiter 1:5e22bf1a3817 128 void APP_safeoutput(void)
vsluiter 1:5e22bf1a3817 129 {
vsluiter 1:5e22bf1a3817 130 asm("nop");
vsluiter 1:5e22bf1a3817 131 //Wb.dout = 0;
vsluiter 1:5e22bf1a3817 132 //DOUTPORT = (Wb.dout >> 4) & 0xf0;
vsluiter 1:5e22bf1a3817 133 }
vsluiter 1:5e22bf1a3817 134
vsluiter 1:5e22bf1a3817 135 void DIG_process(void)
vsluiter 1:5e22bf1a3817 136 {
vsluiter 1:5e22bf1a3817 137 if (APPstate & APPSTATE_OUTPUT) //output enabled
vsluiter 1:5e22bf1a3817 138 {
vsluiter 1:5e22bf1a3817 139 if (ESCvar.ALevent & ESCREG_ALEVENT_SM2) // SM2 trigger ?
vsluiter 1:5e22bf1a3817 140 {
vsluiter 1:5e22bf1a3817 141 ESCvar.ALevent &= ~ESCREG_ALEVENT_SM2;
vsluiter 8:09dcef3ed467 142 RXPDO_update();
vsluiter 1:5e22bf1a3817 143 // dummy output point
vsluiter 5:6d75f432a41f 144 correct_offset = Wb.correct_offset;
vsluiter 5:6d75f432a41f 145 if(correct_offset & 0x01) {
vsluiter 8:09dcef3ed467 146 // led.write(1);
vsluiter 8:09dcef3ed467 147 FrontLeft_offset = rawFrontLeft;
vsluiter 8:09dcef3ed467 148 FrontRight_offset = rawFrontRight;
vsluiter 8:09dcef3ed467 149 BackLeft_offset = rawBackLeft;
vsluiter 8:09dcef3ed467 150 BackRight_offset = rawBackRight;
vsluiter 8:09dcef3ed467 151 }
vsluiter 8:09dcef3ed467 152 // else
vsluiter 8:09dcef3ed467 153 // led.write(0);
vsluiter 1:5e22bf1a3817 154 wd_cnt = wd_reset;
vsluiter 1:5e22bf1a3817 155 }
vsluiter 1:5e22bf1a3817 156
vsluiter 5:6d75f432a41f 157 if (!wd_cnt) {
vsluiter 1:5e22bf1a3817 158 ESC_stopoutput();
vsluiter 1:5e22bf1a3817 159 // watchdog, invalid outputs
vsluiter 1:5e22bf1a3817 160 ESC_ALerror(ALERR_WATCHDOG);
vsluiter 1:5e22bf1a3817 161 // goto safe-op with error bit set
vsluiter 1:5e22bf1a3817 162 ESC_ALstatus(ESCsafeop | ESCerror);
vsluiter 1:5e22bf1a3817 163 }
vsluiter 1:5e22bf1a3817 164 }
vsluiter 1:5e22bf1a3817 165 else
vsluiter 1:5e22bf1a3817 166 {
vsluiter 1:5e22bf1a3817 167 //wd_ok = 1;
vsluiter 1:5e22bf1a3817 168 wd_cnt = wd_reset;
vsluiter 1:5e22bf1a3817 169 }
vsluiter 1:5e22bf1a3817 170 if (APPstate) //input or output enabled
vsluiter 5:6d75f432a41f 171 {
vsluiter 5:6d75f432a41f 172 float fl,fr,br,bl,copx,copy;
vsluiter 10:4e9069e5d698 173 float total_f;
vsluiter 1:5e22bf1a3817 174 Rb.timestamp = ESCvar.Time;
vsluiter 1:5e22bf1a3817 175 //just some dummy data to test
vsluiter 4:bb72df6dce33 176 //Rb.counter++;
vsluiter 4:bb72df6dce33 177 //Rb.diginput = diginput;
vsluiter 4:bb72df6dce33 178 //Rb.analog[0] = 1;
vsluiter 4:bb72df6dce33 179 //Rb.analog[1] = 2;
vsluiter 5:6d75f432a41f 180 fr = rawFrontRight - FrontRight_offset;
vsluiter 5:6d75f432a41f 181 fl = rawFrontLeft - FrontLeft_offset;
vsluiter 5:6d75f432a41f 182 br = rawBackRight - BackRight_offset;
vsluiter 5:6d75f432a41f 183 bl = rawBackLeft - BackLeft_offset;
vsluiter 10:4e9069e5d698 184 total_f = fr+fl+br+bl;
vsluiter 10:4e9069e5d698 185 copx = (((fl+bl)-(fr+br))/total_f)*BALANCE_WIDTH;
vsluiter 10:4e9069e5d698 186 copy = (((fr+fl)-(br+bl))/total_f)*BALANCE_HEIGHT;
vsluiter 10:4e9069e5d698 187 grf = (total_f)*(4096*(1<<4)*(1./1100.0));
vsluiter 5:6d75f432a41f 188 Rb.CoPx = copx;
vsluiter 5:6d75f432a41f 189 Rb.CoPy = copy;
vsluiter 9:33673e05639f 190 Rb.grf = grf;
vsluiter 5:6d75f432a41f 191 Rb.FrontRight = rawFrontRight;
vsluiter 5:6d75f432a41f 192 Rb.FrontLeft = rawFrontLeft;
vsluiter 5:6d75f432a41f 193 Rb.BackRight = rawBackRight;
vsluiter 5:6d75f432a41f 194 Rb.BackLeft = rawBackLeft;
vsluiter 1:5e22bf1a3817 195
vsluiter 1:5e22bf1a3817 196 TXPDO_update();
vsluiter 5:6d75f432a41f 197 }
vsluiter 1:5e22bf1a3817 198 }
vsluiter 1:5e22bf1a3817 199
vsluiter 5:6d75f432a41f 200 void sample(void)
vsluiter 5:6d75f432a41f 201 {
vsluiter 5:6d75f432a41f 202 rawFrontLeft = adcFrontLeft;
vsluiter 5:6d75f432a41f 203 rawFrontRight = adcFrontRight;
vsluiter 5:6d75f432a41f 204 rawBackLeft = adcBackLeft;
vsluiter 5:6d75f432a41f 205 rawBackRight = adcBackRight;
vsluiter 5:6d75f432a41f 206
vsluiter 5:6d75f432a41f 207 }
vsluiter 1:5e22bf1a3817 208 int main(void)
vsluiter 1:5e22bf1a3817 209 {
vsluiter 1:5e22bf1a3817 210 /*!< At this stage the microcontroller clock setting is already configured,
vsluiter 1:5e22bf1a3817 211 this is done through SystemInit() function which is called from startup
vsluiter 1:5e22bf1a3817 212 file (startup_stm32f0xx.s) before to branch to application main.
vsluiter 1:5e22bf1a3817 213 To reconfigure the default setting of SystemInit() function, refer to
vsluiter 1:5e22bf1a3817 214 system_stm32f0xx.c file
vsluiter 1:5e22bf1a3817 215 */
vsluiter 5:6d75f432a41f 216 Ticker adc_sampler;
vsluiter 1:5e22bf1a3817 217 cpuinit();
vsluiter 5:6d75f432a41f 218 adc_sampler.attach(sample,0.001);
vsluiter 1:5e22bf1a3817 219 TXPDOsize = sizeTXPDO();
vsluiter 1:5e22bf1a3817 220 RXPDOsize = sizeRXPDO();
vsluiter 1:5e22bf1a3817 221 wait_ms(200);
vsluiter 1:5e22bf1a3817 222 /*initialize configuration*/
vsluiter 5:6d75f432a41f 223 //Eb.setting16 = 0xABCD;
vsluiter 5:6d75f432a41f 224 //Eb.setting8 = 111;
vsluiter 1:5e22bf1a3817 225 // wait until ESC is started up
vsluiter 1:5e22bf1a3817 226 while ((ESCvar.DLstatus & 0x0001) == 0)
vsluiter 1:5e22bf1a3817 227 ESC_read(ESCREG_DLSTATUS, &ESCvar.DLstatus, sizeof(ESCvar.DLstatus), &ESCvar.ALevent);
vsluiter 1:5e22bf1a3817 228
vsluiter 1:5e22bf1a3817 229 // reset ESC to init state
vsluiter 1:5e22bf1a3817 230 ESC_ALstatus(ESCinit);
vsluiter 1:5e22bf1a3817 231 ESC_ALerror(ALERR_NONE);
vsluiter 1:5e22bf1a3817 232 ESC_stopmbx();
vsluiter 1:5e22bf1a3817 233 ESC_stopinput();
vsluiter 1:5e22bf1a3817 234 ESC_stopoutput();
vsluiter 1:5e22bf1a3817 235
vsluiter 1:5e22bf1a3817 236 // application run loop
vsluiter 1:5e22bf1a3817 237 while (1)
vsluiter 1:5e22bf1a3817 238 {
vsluiter 1:5e22bf1a3817 239 ESC_read(ESCREG_LOCALTIME, &ESCvar.Time, sizeof(ESCvar.Time), &ESCvar.ALevent);
vsluiter 1:5e22bf1a3817 240 ESC_ALevent();
vsluiter 1:5e22bf1a3817 241 ESC_state();
vsluiter 1:5e22bf1a3817 242 if (ESC_mbxprocess())
vsluiter 1:5e22bf1a3817 243 {
vsluiter 1:5e22bf1a3817 244 ESC_coeprocess();
vsluiter 1:5e22bf1a3817 245 ESC_xoeprocess();
vsluiter 1:5e22bf1a3817 246 }
vsluiter 1:5e22bf1a3817 247 DIG_process();
vsluiter 1:5e22bf1a3817 248 }
vsluiter 1:5e22bf1a3817 249 }
vsluiter 1:5e22bf1a3817 250
vsluiter 1:5e22bf1a3817 251 /** void ESC_objecthandler(uint16 index, uint8 subindex)
vsluiter 1:5e22bf1a3817 252 \brief Object handler, declared from esc.h, as extern function
vsluiter 1:5e22bf1a3817 253 \param index
vsluiter 1:5e22bf1a3817 254 \param subindex
vsluiter 1:5e22bf1a3817 255 */
vsluiter 1:5e22bf1a3817 256
vsluiter 1:5e22bf1a3817 257
vsluiter 1:5e22bf1a3817 258 #ifdef USE_FULL_ASSERT
vsluiter 1:5e22bf1a3817 259
vsluiter 1:5e22bf1a3817 260 /**
vsluiter 1:5e22bf1a3817 261 * @brief Reports the name of the source file and the source line number
vsluiter 1:5e22bf1a3817 262 * where the assert_param error has occurred.
vsluiter 1:5e22bf1a3817 263 * @param file: pointer to the source file name
vsluiter 1:5e22bf1a3817 264 * @param line: assert_param error line source number
vsluiter 1:5e22bf1a3817 265 * @retval None
vsluiter 1:5e22bf1a3817 266 */
vsluiter 1:5e22bf1a3817 267 void assert_failed(uint8_t* file, uint32_t line)
vsluiter 1:5e22bf1a3817 268 {
vsluiter 1:5e22bf1a3817 269 /* User can add his own implementation to report the file name and line number,
vsluiter 1:5e22bf1a3817 270 ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
vsluiter 1:5e22bf1a3817 271
vsluiter 1:5e22bf1a3817 272 /* Infinite loop */
vsluiter 1:5e22bf1a3817 273 while (1)
vsluiter 1:5e22bf1a3817 274 {
vsluiter 1:5e22bf1a3817 275 }
vsluiter 1:5e22bf1a3817 276 }
vsluiter 1:5e22bf1a3817 277 #endif
vsluiter 1:5e22bf1a3817 278
vsluiter 1:5e22bf1a3817 279 /**
vsluiter 1:5e22bf1a3817 280 * @}
vsluiter 1:5e22bf1a3817 281 */
vsluiter 1:5e22bf1a3817 282
vsluiter 1:5e22bf1a3817 283 /**
vsluiter 1:5e22bf1a3817 284 * @}
vsluiter 1:5e22bf1a3817 285 */
vsluiter 1:5e22bf1a3817 286
vsluiter 1:5e22bf1a3817 287 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
vsluiter 1:5e22bf1a3817 288