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-21
- Revision:
- 3:79e62f9b13c8
- Parent:
- 0:6b18a09a6628
- Child:
- 4:7d2eae0115a2
File content as of revision 3:79e62f9b13c8:
#include "mbed.h" #include "SBUS.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); 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, ch1_b, ch2_b; float rc1, rc2; int out1, out2; void interrupt_SBUS() { led1 = !led1; if(sbus.failSafe == false) { ch1_b = ch1; ch2_b = ch2; ch1 = int(sbus.getData(1)); ch2 = int(sbus.getData(2)); pc.printf("ch1 :%d\r\n", ch1); pc.printf("ch2 :%d\r\n", ch1); if(abs(ch1-ch1_b) < 100){ rc1 = (float)(map(ch1,368,1680,-1000,1000))/1000.0; } if(abs(ch2-ch2_b) < 100){ rc2 = (float)(map(ch2,368,1680,-1000,1000))/1000.0; } pc.printf("rc1 :%f\r\n", rc1); pc.printf("rc2 :%f\r\n", rc1); int pwmMax = 1800; int pwmMin = 1200; out1 = map((int)(rc1*1000.0),-1000,1000,pwmMin,pwmMax); if(out1<pwmMin){out1 = pwmMin;}; if(out1>pwmMax){out1 = pwmMax;}; out2 = map((int)(rc2*1000.0),-1000,1000,pwmMin,pwmMax); if(out2<pwmMin){out2 = pwmMin;}; if(out2>pwmMax){out2 = pwmMax;}; pc.printf("out1:%d\r\n", 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); Ticker timer_SBUS; Ticker timer_Gyro; timer_SBUS.attach(interrupt_SBUS, 1.0); timer_Gyro.attach(interrupt_Gyro, 0.1); while(1) { } }