HAPSRG / Mbed 2 deprecated hexaTest_Eigen

solaESKF_EIGEN

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

main.cpp@3:79e62f9b13c8, 2021-01-21 (annotated)

Committer:
cocorlow
Date:
Thu Jan 21 08:59:55 2021 +0000
Revision:
3:79e62f9b13c8
Parent:
0:6b18a09a6628
Child:
4:7d2eae0115a2
sbus

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 if(abs(ch1-ch1_b) < 100){
cocorlow 3:79e62f9b13c8 32 rc1 = (float)(map(ch1,368,1680,-1000,1000))/1000.0;
cocorlow 3:79e62f9b13c8 33 }
cocorlow 3:79e62f9b13c8 34 if(abs(ch2-ch2_b) < 100){
cocorlow 3:79e62f9b13c8 35 rc2 = (float)(map(ch2,368,1680,-1000,1000))/1000.0;
cocorlow 3:79e62f9b13c8 36 }
cocorlow 3:79e62f9b13c8 37 pc.printf("rc1 :%f\r\n", rc1);
cocorlow 3:79e62f9b13c8 38 pc.printf("rc2 :%f\r\n", rc1);
cocorlow 3:79e62f9b13c8 39 int pwmMax = 1800;
cocorlow 3:79e62f9b13c8 40 int pwmMin = 1200;
cocorlow 3:79e62f9b13c8 41 out1 = map((int)(rc1*1000.0),-1000,1000,pwmMin,pwmMax);
cocorlow 3:79e62f9b13c8 42 if(out1<pwmMin){out1 = pwmMin;};
cocorlow 3:79e62f9b13c8 43 if(out1>pwmMax){out1 = pwmMax;};
cocorlow 3:79e62f9b13c8 44 out2 = map((int)(rc2*1000.0),-1000,1000,pwmMin,pwmMax);
cocorlow 3:79e62f9b13c8 45 if(out2<pwmMin){out2 = pwmMin;};
cocorlow 3:79e62f9b13c8 46 if(out2>pwmMax){out2 = pwmMax;};
cocorlow 3:79e62f9b13c8 47
cocorlow 3:79e62f9b13c8 48 pc.printf("out1:%d\r\n", out1);
cocorlow 3:79e62f9b13c8 49 pc.printf("out2:%d\r\n", out2);
cocorlow 3:79e62f9b13c8 50 servo1.pulsewidth_us(out1);
cocorlow 3:79e62f9b13c8 51 servo2.pulsewidth_us(out2);
cocorlow 3:79e62f9b13c8 52 }
cocorlow 3:79e62f9b13c8 53
cocorlow 3:79e62f9b13c8 54
cocorlow 3:79e62f9b13c8 55 }
cocorlow 3:79e62f9b13c8 56
cocorlow 3:79e62f9b13c8 57 void interrupt_Gyro()
cocorlow 3:79e62f9b13c8 58 {
cocorlow 3:79e62f9b13c8 59 led3 = !led3;
cocorlow 3:79e62f9b13c8 60 }
cocorlow 3:79e62f9b13c8 61
NaotoMorita 0:6b18a09a6628 62 int main()
NaotoMorita 0:6b18a09a6628 63 {
NaotoMorita 0:6b18a09a6628 64 pc.baud(9600);
cocorlow 3:79e62f9b13c8 65 servo1.period(0.020);
cocorlow 3:79e62f9b13c8 66 servo2.period(0.020);
cocorlow 3:79e62f9b13c8 67 Ticker timer_SBUS;
cocorlow 3:79e62f9b13c8 68 Ticker timer_Gyro;
cocorlow 3:79e62f9b13c8 69 timer_SBUS.attach(interrupt_SBUS, 1.0);
cocorlow 3:79e62f9b13c8 70 timer_Gyro.attach(interrupt_Gyro, 0.1);
NaotoMorita 0:6b18a09a6628 71 while(1) {
cocorlow 3:79e62f9b13c8 72
NaotoMorita 0:6b18a09a6628 73 }
NaotoMorita 0:6b18a09a6628 74 }