solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: main.cpp
- Revision:
- 3:79e62f9b13c8
- Parent:
- 0:6b18a09a6628
- Child:
- 4:7d2eae0115a2
diff -r 988872988a1e -r 79e62f9b13c8 main.cpp --- a/main.cpp Thu Jan 21 07:32:48 2021 +0000 +++ b/main.cpp Thu Jan 21 08:59:55 2021 +0000 @@ -3,15 +3,72 @@ 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) { - if(sbus.failSafe == false){ - for (int i = 0; i < 4; i++){ - pc.printf("%d\t", sbus.getData(i)); - } - pc.printf("\n"); - } + } } \ No newline at end of file