HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

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@5:9cad4ce807b9, 2021-01-21 (annotated)

Committer:
NaotoMorita
Date:
Thu Jan 21 09:09:57 2021 +0000
Revision:
5:9cad4ce807b9
Parent:
4:7d2eae0115a2
Child:
6:2cba569272fe
chanto naoshita

Who changed what in which revision?

UserRevisionLine numberNew 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
NaotoMorita 5:9cad4ce807b9 16 int ch1, ch2;
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 {
NaotoMorita 5:9cad4ce807b9 25
cocorlow 3:79e62f9b13c8 26 ch1 = int(sbus.getData(1));
cocorlow 3:79e62f9b13c8 27 ch2 = int(sbus.getData(2));
cocorlow 3:79e62f9b13c8 28 pc.printf("ch1 :%d\r\n", ch1);
cocorlow 3:79e62f9b13c8 29 pc.printf("ch2 :%d\r\n", ch1);
NaotoMorita 5:9cad4ce807b9 30 float LP_rc = 0.65;
NaotoMorita 5:9cad4ce807b9 31 float LP_rc3 = 0.15;
NaotoMorita 5:9cad4ce807b9 32 rc1 = LP_rc*float(map(ch1,368,1680,-1000,1000))/1000.0+(1.0-LP_rc)*rc1;
NaotoMorita 5:9cad4ce807b9 33 rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0+(1.0-LP_rc)*rc2;
cocorlow 3:79e62f9b13c8 34 pc.printf("rc1 :%f\r\n", rc1);
cocorlow 3:79e62f9b13c8 35 pc.printf("rc2 :%f\r\n", rc1);
cocorlow 3:79e62f9b13c8 36 int pwmMax = 1800;
cocorlow 3:79e62f9b13c8 37 int pwmMin = 1200;
cocorlow 3:79e62f9b13c8 38 out1 = map((int)(rc1*1000.0),-1000,1000,pwmMin,pwmMax);
cocorlow 3:79e62f9b13c8 39 if(out1<pwmMin){out1 = pwmMin;};
cocorlow 3:79e62f9b13c8 40 if(out1>pwmMax){out1 = pwmMax;};
cocorlow 3:79e62f9b13c8 41 out2 = map((int)(rc2*1000.0),-1000,1000,pwmMin,pwmMax);
cocorlow 3:79e62f9b13c8 42 if(out2<pwmMin){out2 = pwmMin;};
cocorlow 3:79e62f9b13c8 43 if(out2>pwmMax){out2 = pwmMax;};
cocorlow 3:79e62f9b13c8 44
cocorlow 3:79e62f9b13c8 45 pc.printf("out1:%d\r\n", out1);
cocorlow 3:79e62f9b13c8 46 pc.printf("out2:%d\r\n", out2);
cocorlow 3:79e62f9b13c8 47 servo1.pulsewidth_us(out1);
cocorlow 3:79e62f9b13c8 48 servo2.pulsewidth_us(out2);
cocorlow 3:79e62f9b13c8 49 }
cocorlow 3:79e62f9b13c8 50
cocorlow 3:79e62f9b13c8 51
cocorlow 3:79e62f9b13c8 52 }
cocorlow 3:79e62f9b13c8 53
cocorlow 3:79e62f9b13c8 54 void interrupt_Gyro()
cocorlow 3:79e62f9b13c8 55 {
cocorlow 3:79e62f9b13c8 56 led3 = !led3;
cocorlow 3:79e62f9b13c8 57 }
cocorlow 3:79e62f9b13c8 58
NaotoMorita 0:6b18a09a6628 59 int main()
NaotoMorita 0:6b18a09a6628 60 {
NaotoMorita 0:6b18a09a6628 61 pc.baud(9600);
cocorlow 3:79e62f9b13c8 62 servo1.period(0.020);
cocorlow 3:79e62f9b13c8 63 servo2.period(0.020);
cocorlow 3:79e62f9b13c8 64 Ticker timer_SBUS;
cocorlow 3:79e62f9b13c8 65 Ticker timer_Gyro;
NaotoMorita 4:7d2eae0115a2 66 timer_SBUS.attach(interrupt_SBUS, 0.020);
cocorlow 3:79e62f9b13c8 67 timer_Gyro.attach(interrupt_Gyro, 0.1);
NaotoMorita 0:6b18a09a6628 68 while(1) {
cocorlow 3:79e62f9b13c8 69
NaotoMorita 0:6b18a09a6628 70 }
NaotoMorita 0:6b18a09a6628 71 }