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
main.cpp
- Committer:
- cocorlow
- Date:
- 2021-01-28
- Revision:
- 11:ec79564cf265
- Parent:
- 7:70161eb0f854
- Child:
- 12:06891de5c601
File content as of revision 11:ec79564cf265:
#include "mbed.h" #include "SBUS.hpp" #include "LoopTicker.hpp" SBUS sbus(PD_5, PD_6); Serial pc(USBTX, USBRX); DigitalOut led1(LED1); DigitalOut led3(LED3); PwmOut servo1(PB_4); PwmOut servo2(PB_5); int ch1, ch2; float rc1, rc2; int out1, out2; 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; } void interrupt_SBUS() { led1 = !led1; if(sbus.failSafe == false) { ch1 = int(sbus.getData(1)); ch2 = int(sbus.getData(2)); pc.printf("ch1 :%d ", ch1); pc.printf("ch2 :%d ", ch2); float LP_rc = 0.65f; float LP_rc3 = 0.15f; rc1 = LP_rc*float(map(ch1,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1; rc2 = LP_rc*float(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*2000.0f),-2000,2000,pwmMin,pwmMax); if(out1<pwmMin){out1 = pwmMin;}; if(out1>pwmMax){out1 = pwmMax;}; out2 = map((int)(rc2*2000.0f),-2000,2000,pwmMin,pwmMax); if(out2<pwmMin){out2 = pwmMin;}; if(out2>pwmMax){out2 = pwmMax;}; pc.printf("out1:%d ", out1); pc.printf("out2:%d\r\n", out2); servo1.pulsewidth_us(out1); servo2.pulsewidth_us(out2); } } void interrupt_Gyro() { led3 = !led3; } int main() { pc.baud(9600); servo1.period(0.020); servo2.period(0.020); LoopTicker timer_SBUS; LoopTicker timer_Gyro; timer_SBUS.attach(interrupt_SBUS, 0.015); timer_Gyro.attach(interrupt_Gyro, 0.5); while(1) { timer_SBUS.loop(); timer_Gyro.loop(); // interrupt_SBUS(); interrupt_Gyro(); } }