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@4:7d2eae0115a2, 2021-01-21 (annotated)
- Committer:
- NaotoMorita
- Date:
- Thu Jan 21 09:04:24 2021 +0000
- Revision:
- 4:7d2eae0115a2
- Parent:
- 3:79e62f9b13c8
- Child:
- 5:9cad4ce807b9
morita mod
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
NaotoMorita | 0:6b18a09a6628 | 1 | #include "mbed.h" |
NaotoMorita | 0:6b18a09a6628 | 2 | #include "SBUS.hpp" |
NaotoMorita | 0:6b18a09a6628 | 3 | SBUS sbus(PD_5, PD_6); |
NaotoMorita | 0:6b18a09a6628 | 4 | Serial pc(USBTX, USBRX); |
NaotoMorita | 0:6b18a09a6628 | 5 | |
cocorlow | 3:79e62f9b13c8 | 6 | DigitalOut led1(LED1); |
cocorlow | 3:79e62f9b13c8 | 7 | DigitalOut led3(LED3); |
cocorlow | 3:79e62f9b13c8 | 8 | PwmOut servo1(PB_4); |
cocorlow | 3:79e62f9b13c8 | 9 | PwmOut servo2(PB_5); |
cocorlow | 3:79e62f9b13c8 | 10 | |
cocorlow | 3:79e62f9b13c8 | 11 | long map(long x, long in_min, long in_max, long out_min, long out_max) |
cocorlow | 3:79e62f9b13c8 | 12 | { |
cocorlow | 3:79e62f9b13c8 | 13 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
cocorlow | 3:79e62f9b13c8 | 14 | } |
cocorlow | 3:79e62f9b13c8 | 15 | |
cocorlow | 3:79e62f9b13c8 | 16 | int ch1, ch2, ch1_b, ch2_b; |
cocorlow | 3:79e62f9b13c8 | 17 | float rc1, rc2; |
cocorlow | 3:79e62f9b13c8 | 18 | int out1, out2; |
cocorlow | 3:79e62f9b13c8 | 19 | |
cocorlow | 3:79e62f9b13c8 | 20 | void interrupt_SBUS() |
cocorlow | 3:79e62f9b13c8 | 21 | { |
cocorlow | 3:79e62f9b13c8 | 22 | led1 = !led1; |
cocorlow | 3:79e62f9b13c8 | 23 | if(sbus.failSafe == false) |
cocorlow | 3:79e62f9b13c8 | 24 | { |
cocorlow | 3:79e62f9b13c8 | 25 | ch1_b = ch1; |
cocorlow | 3:79e62f9b13c8 | 26 | ch2_b = ch2; |
cocorlow | 3:79e62f9b13c8 | 27 | ch1 = int(sbus.getData(1)); |
cocorlow | 3:79e62f9b13c8 | 28 | ch2 = int(sbus.getData(2)); |
cocorlow | 3:79e62f9b13c8 | 29 | pc.printf("ch1 :%d\r\n", ch1); |
cocorlow | 3:79e62f9b13c8 | 30 | pc.printf("ch2 :%d\r\n", ch1); |
cocorlow | 3:79e62f9b13c8 | 31 | pc.printf("rc1 :%f\r\n", rc1); |
cocorlow | 3:79e62f9b13c8 | 32 | pc.printf("rc2 :%f\r\n", rc1); |
cocorlow | 3:79e62f9b13c8 | 33 | int pwmMax = 1800; |
cocorlow | 3:79e62f9b13c8 | 34 | int pwmMin = 1200; |
cocorlow | 3:79e62f9b13c8 | 35 | out1 = map((int)(rc1*1000.0),-1000,1000,pwmMin,pwmMax); |
cocorlow | 3:79e62f9b13c8 | 36 | if(out1<pwmMin){out1 = pwmMin;}; |
cocorlow | 3:79e62f9b13c8 | 37 | if(out1>pwmMax){out1 = pwmMax;}; |
cocorlow | 3:79e62f9b13c8 | 38 | out2 = map((int)(rc2*1000.0),-1000,1000,pwmMin,pwmMax); |
cocorlow | 3:79e62f9b13c8 | 39 | if(out2<pwmMin){out2 = pwmMin;}; |
cocorlow | 3:79e62f9b13c8 | 40 | if(out2>pwmMax){out2 = pwmMax;}; |
cocorlow | 3:79e62f9b13c8 | 41 | |
cocorlow | 3:79e62f9b13c8 | 42 | pc.printf("out1:%d\r\n", out1); |
cocorlow | 3:79e62f9b13c8 | 43 | pc.printf("out2:%d\r\n", out2); |
cocorlow | 3:79e62f9b13c8 | 44 | servo1.pulsewidth_us(out1); |
cocorlow | 3:79e62f9b13c8 | 45 | servo2.pulsewidth_us(out2); |
cocorlow | 3:79e62f9b13c8 | 46 | } |
cocorlow | 3:79e62f9b13c8 | 47 | |
cocorlow | 3:79e62f9b13c8 | 48 | |
cocorlow | 3:79e62f9b13c8 | 49 | } |
cocorlow | 3:79e62f9b13c8 | 50 | |
cocorlow | 3:79e62f9b13c8 | 51 | void interrupt_Gyro() |
cocorlow | 3:79e62f9b13c8 | 52 | { |
cocorlow | 3:79e62f9b13c8 | 53 | led3 = !led3; |
cocorlow | 3:79e62f9b13c8 | 54 | } |
cocorlow | 3:79e62f9b13c8 | 55 | |
NaotoMorita | 0:6b18a09a6628 | 56 | int main() |
NaotoMorita | 0:6b18a09a6628 | 57 | { |
NaotoMorita | 0:6b18a09a6628 | 58 | pc.baud(9600); |
cocorlow | 3:79e62f9b13c8 | 59 | servo1.period(0.020); |
cocorlow | 3:79e62f9b13c8 | 60 | servo2.period(0.020); |
cocorlow | 3:79e62f9b13c8 | 61 | Ticker timer_SBUS; |
cocorlow | 3:79e62f9b13c8 | 62 | Ticker timer_Gyro; |
NaotoMorita | 4:7d2eae0115a2 | 63 | timer_SBUS.attach(interrupt_SBUS, 0.020); |
cocorlow | 3:79e62f9b13c8 | 64 | timer_Gyro.attach(interrupt_Gyro, 0.1); |
NaotoMorita | 0:6b18a09a6628 | 65 | while(1) { |
cocorlow | 3:79e62f9b13c8 | 66 | |
NaotoMorita | 0:6b18a09a6628 | 67 | } |
NaotoMorita | 0:6b18a09a6628 | 68 | } |