Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: main.cpp
- Revision:
- 6:2cba569272fe
- Parent:
- 5:9cad4ce807b9
- Child:
- 7:70161eb0f854
diff -r 9cad4ce807b9 -r 2cba569272fe main.cpp --- a/main.cpp Thu Jan 21 09:09:57 2021 +0000 +++ b/main.cpp Thu Jan 21 15:25:51 2021 +0000 @@ -1,5 +1,8 @@ #include "mbed.h" #include "SBUS.hpp" +#include "LoopTicker.hpp" + + SBUS sbus(PD_5, PD_6); Serial pc(USBTX, USBRX); @@ -8,11 +11,6 @@ PwmOut servo1(PB_4); PwmOut servo2(PB_5); -long map(long x, long in_min, long in_max, long out_min, long out_max) -{ - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; -} - int ch1, ch2; float rc1, rc2; int out1, out2; @@ -22,33 +20,30 @@ led1 = !led1; if(sbus.failSafe == false) { - ch1 = int(sbus.getData(1)); ch2 = int(sbus.getData(2)); - pc.printf("ch1 :%d\r\n", ch1); - pc.printf("ch2 :%d\r\n", ch1); - float LP_rc = 0.65; - float LP_rc3 = 0.15; - rc1 = LP_rc*float(map(ch1,368,1680,-1000,1000))/1000.0+(1.0-LP_rc)*rc1; - rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0+(1.0-LP_rc)*rc2; - pc.printf("rc1 :%f\r\n", rc1); - pc.printf("rc2 :%f\r\n", rc1); + pc.printf("ch1 :%d ", ch1); + pc.printf("ch2 :%d ", ch2); + float LP_rc = 0.65f; + float LP_rc3 = 0.15f; + rc1 = LP_rc*float(SBUS::map(ch1,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1; + rc2 = LP_rc*float(SBUS::map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2; + pc.printf("rc1 :%f ", rc1); + pc.printf("rc2 :%f ", rc1); int pwmMax = 1800; int pwmMin = 1200; - out1 = map((int)(rc1*1000.0),-1000,1000,pwmMin,pwmMax); + out1 = SBUS::map((int)(rc1*1000.0f),-1000,1000,pwmMin,pwmMax); if(out1<pwmMin){out1 = pwmMin;}; if(out1>pwmMax){out1 = pwmMax;}; - out2 = map((int)(rc2*1000.0),-1000,1000,pwmMin,pwmMax); + out2 = SBUS::map((int)(rc2*1000.0f),-1000,1000,pwmMin,pwmMax); if(out2<pwmMin){out2 = pwmMin;}; if(out2>pwmMax){out2 = pwmMax;}; - pc.printf("out1:%d\r\n", out1); + pc.printf("out1:%d ", out1); pc.printf("out2:%d\r\n", out2); servo1.pulsewidth_us(out1); servo2.pulsewidth_us(out2); } - - } void interrupt_Gyro() @@ -61,11 +56,12 @@ pc.baud(9600); servo1.period(0.020); servo2.period(0.020); - Ticker timer_SBUS; - Ticker timer_Gyro; + LoopTicker timer_SBUS; + LoopTicker timer_Gyro; timer_SBUS.attach(interrupt_SBUS, 0.020); - timer_Gyro.attach(interrupt_Gyro, 0.1); + timer_Gyro.attach(interrupt_Gyro, 0.5); while(1) { - + timer_SBUS.loop(); + timer_SBUS.loop(); } } \ No newline at end of file