SOES implementation i.c.w. ET1100 (Beckhoff ASIC)
Dependencies: KL25Z_ClockControl MODSERIAL mbed
Fork of EtherCAT-XbusMaster by
Diff: soes.cpp
- Revision:
- 5:6d75f432a41f
- Parent:
- 4:bb72df6dce33
- Child:
- 6:3ccb89a58ff8
- Child:
- 8:09dcef3ed467
--- a/soes.cpp Thu Dec 11 22:31:10 2014 +0000 +++ b/soes.cpp Thu Dec 11 23:12:11 2014 +0000 @@ -44,6 +44,8 @@ /* Private define ------------------------------------------------------------*/ #define wd_reset 1000 +#define BALANCE_WIDTH 0.435 +#define BALANCE_HEIGHT 0.24 /* Private macro -------------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ _ESCvar ESCvar; @@ -54,17 +56,29 @@ uint16 SM2_sml,SM3_sml; _Rbuffer Rb; _Wbuffer Wb; -_Ebuffer Eb; //EEprom +//_Ebuffer Eb; //EEprom uint8 TXPDOsize,RXPDOsize; uint16 wd_ok = 1, wd_cnt = wd_reset; -volatile uint8 digoutput; -volatile uint8 diginput; +volatile uint8 correct_offset; +float FrontRight_offset; +float FrontLeft_offset; +float BackLeft_offset; +float BackRight_offset; +float rawFrontLeft; +float rawFrontRight; +float rawBackLeft; +float rawBackRight; +//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 ---------------------------------------------------------*/ @@ -78,18 +92,7 @@ void TXPDO_update(void); void RXPDO_update(void); void DIG_process(void); -/* -void _delay_ms(uint16 number_of_ms) -{ - volatile uint16_t cpu_counter = 0; - uint16_t ms_counter; - for(ms_counter = 0 ; ms_counter < number_of_ms ; ms_counter++) - { - cpu_counter = 48000; //48MHz processor - while(cpu_counter >0) - cpu_counter--; - } -}*/ +void sample(void); void ESC_objecthandler(uint16 index, uint8 subindex) { @@ -101,10 +104,10 @@ switch (subindex) { case 0x01: - dummy8 = Eb.setting8;//Write value to EEPROM; eeprom_write_byte(&eedat.setting8, Wb.setting8); + dummy8 = 0;//Eb.setting8;//Write value to EEPROM; eeprom_write_byte(&eedat.setting8, Wb.setting8); break; case 0x02: - dummy16 = Eb.setting16;//Write value to EEPROM; eeprom_write_word(&eedat.setting16, Wb.setting16); + dummy16 = 0;//Eb.setting16;//Write value to EEPROM; eeprom_write_word(&eedat.setting16, Wb.setting16); break; } break; @@ -137,16 +140,18 @@ ESCvar.ALevent &= ~ESCREG_ALEVENT_SM2; RXPDO_update(); // dummy output point - digoutput = Wb.digoutput; - if(digoutput & 0x01) + correct_offset = Wb.correct_offset; + if(correct_offset & 0x01) { led.write(1); - else - led.write(0); + FrontLeft_offset = adcFrontLeft; + FrontRight_offset = adcFrontRight; + BackLeft_offset = adcBackLeft; + BackRight_offset = adcBackRight; + } wd_cnt = wd_reset; } - if (!wd_cnt) - { + if (!wd_cnt) { ESC_stopoutput(); // watchdog, invalid outputs ESC_ALerror(ALERR_WATCHDOG); @@ -160,25 +165,39 @@ wd_cnt = wd_reset; } if (APPstate) //input or output enabled - { + { + float fl,fr,br,bl,copx,copy; Rb.timestamp = ESCvar.Time; //just some dummy data to test //Rb.counter++; //Rb.diginput = diginput; //Rb.analog[0] = 1; //Rb.analog[1] = 2; - Rb.CoPx = 3; - Rb.CoPy = 4; - Rb.FrontRight = 5; - Rb.FrontLeft = 6; - Rb.BackRight = 7; - Rb.BackLeft = 8; + fr = rawFrontRight - FrontRight_offset; + fl = rawFrontLeft - FrontLeft_offset; + br = rawBackRight - BackRight_offset; + bl = rawBackLeft - BackLeft_offset; + copx = ((fl+bl)-(fr+br))*BALANCE_WIDTH; + copy = ((fr+fl)-(br+bl))*BALANCE_HEIGHT; + Rb.CoPx = copx; + Rb.CoPy = copy; + 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, @@ -187,13 +206,15 @@ 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; + //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);