EtherCAT slave that reads 3 Xsens IMU's connected to a Xbus Master
Dependencies: MODSERIAL mbed KL25Z_ClockControl
Fork of EtherCAT by
Diff: soes.cpp
- Revision:
- 14:e3c49b75fee9
- Parent:
- 12:cac4e7c2bd0f
- Child:
- 16:bfc7ea6bd1af
--- a/soes.cpp Tue Mar 03 12:14:22 2015 +0000 +++ b/soes.cpp Tue Mar 03 12:17:15 2015 +0000 @@ -127,10 +127,7 @@ correct_offset = Wb.correct_offset; if(correct_offset & 0x01) { // led.write(1); - FrontLeft_offset = rawFrontLeft; - FrontRight_offset = rawFrontRight; - BackLeft_offset = rawBackLeft; - BackRight_offset = rawBackRight; + asm("nop"); } // else // led.write(0); @@ -152,42 +149,18 @@ } 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, @@ -196,9 +169,7 @@ 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);