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@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?

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
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 }