Minato Shoji / Mbed 2 deprecated mbed_2018

mbed_2018

Dependencies:   mbed JY901 TinyGPS AsyncSerial

main.cpp@0:e4bf955dac58, 2019-12-23 (annotated)

Committer:
Minato
Date:
Mon Dec 23 07:23:01 2019 +0000
Revision:
0:e4bf955dac58
mbed_2018

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Minato 0:e4bf955dac58 1 #include "mbed.h"
Minato 0:e4bf955dac58 2 #include "AsyncSerial.hpp"
Minato 0:e4bf955dac58 3 #include "TinyGPS.h"
Minato 0:e4bf955dac58 4 #include "JY901.h"
Minato 0:e4bf955dac58 5
Minato 0:e4bf955dac58 6 Serial pc(USBTX, USBRX);//CoolTermを用いてパソコン上に表示
Minato 0:e4bf955dac58 7 JY901 i2c(p28,p27);// sda, scl
Minato 0:e4bf955dac58 8 AsyncSerial Serial1(p9,p10,38400,128); // tx, rx
Minato 0:e4bf955dac58 9 AsyncSerial Serial2(p13,p14,9600,256); // tx, rx
Minato 0:e4bf955dac58 10 DigitalOut myled(LED1);
Minato 0:e4bf955dac58 11 InterruptIn alt(p21);
Minato 0:e4bf955dac58 12 InterruptIn rot(p22);
Minato 0:e4bf955dac58 13 DigitalIn mid(p23);
Minato 0:e4bf955dac58 14 AnalogIn an1(p19);
Minato 0:e4bf955dac58 15 AnalogIn an2(p20);
Minato 0:e4bf955dac58 16 Ticker flipper;
Minato 0:e4bf955dac58 17 Ticker flipper2;
Minato 0:e4bf955dac58 18 TinyGPS gps;
Minato 0:e4bf955dac58 19 Timer t;
Minato 0:e4bf955dac58 20 Timer t2;
Minato 0:e4bf955dac58 21 int rot_count_buf=0;
Minato 0:e4bf955dac58 22 int send_f1=0;
Minato 0:e4bf955dac58 23 int send_f2=0;
Minato 0:e4bf955dac58 24
Minato 0:e4bf955dac58 25 #define coef_a -21.05 //ELE
Minato 0:e4bf955dac58 26 #define coef_b 22.2 //RUD
Minato 0:e4bf955dac58 27 float mid_a=0.5,mid_b=0.5;
Minato 0:e4bf955dac58 28 int mid_t=0;
Minato 0:e4bf955dac58 29
Minato 0:e4bf955dac58 30 struct DATA{
Minato 0:e4bf955dac58 31 int height;
Minato 0:e4bf955dac58 32 int rot_count;
Minato 0:e4bf955dac58 33 float air_speed;
Minato 0:e4bf955dac58 34 float analog[2];//0=ELE 1=RUD
Minato 0:e4bf955dac58 35 float fmps,falt;
Minato 0:e4bf955dac58 36 unsigned long fix_age;
Minato 0:e4bf955dac58 37 long lat, lon;
Minato 0:e4bf955dac58 38 unsigned long time, date, speed, course;
Minato 0:e4bf955dac58 39 float angle[3];
Minato 0:e4bf955dac58 40 int a,b;
Minato 0:e4bf955dac58 41 };
Minato 0:e4bf955dac58 42 DATA data;
Minato 0:e4bf955dac58 43
Minato 0:e4bf955dac58 44 void ini(){
Minato 0:e4bf955dac58 45 data.height=0;data.rot_count=0;data.air_speed=0;data.analog[0]=0.0;data.analog[1]=0.0;
Minato 0:e4bf955dac58 46 data.time=0;data.date=0;
Minato 0:e4bf955dac58 47 data.lat=0;data.lon=0.0;data.fmps=0.0;data.falt=0.0;data.fix_age=0;
Minato 0:e4bf955dac58 48 data.course=0;
Minato 0:e4bf955dac58 49 data.angle[0]=0.0;data.angle[1]=0.0;data.angle[2]=0.0;
Minato 0:e4bf955dac58 50 send_f1=0;
Minato 0:e4bf955dac58 51 send_f2=0;
Minato 0:e4bf955dac58 52 mid.mode(PullUp);
Minato 0:e4bf955dac58 53 }
Minato 0:e4bf955dac58 54 float con(float a){
Minato 0:e4bf955dac58 55 if(a>180.0)return a-360.0;
Minato 0:e4bf955dac58 56 else return a;
Minato 0:e4bf955dac58 57 }
Minato 0:e4bf955dac58 58
Minato 0:e4bf955dac58 59 void read_gyro(){
Minato 0:e4bf955dac58 60 data.angle[0]=con(i2c.getXaxisAngle());
Minato 0:e4bf955dac58 61 data.angle[1]=con(i2c.getYaxisAngle());
Minato 0:e4bf955dac58 62 data.angle[2]=con(i2c.getZaxisAngle());
Minato 0:e4bf955dac58 63 }
Minato 0:e4bf955dac58 64 void read_analog(){
Minato 0:e4bf955dac58 65 data.analog[0]=an1.read();//ELE
Minato 0:e4bf955dac58 66 data.analog[1]=an2.read();//RUD
Minato 0:e4bf955dac58 67 data.a=(int)((data.analog[0]-mid_a)*coef_a+4.5);
Minato 0:e4bf955dac58 68 data.b=(int)((data.analog[1]-mid_b)*coef_b+4.5);
Minato 0:e4bf955dac58 69 if(data.a>8)data.a=8;
Minato 0:e4bf955dac58 70 if(data.a<0)data.a=0;
Minato 0:e4bf955dac58 71 if(data.b>8)data.b=8;
Minato 0:e4bf955dac58 72 if(data.b<0)data.b=0;
Minato 0:e4bf955dac58 73 }
Minato 0:e4bf955dac58 74
Minato 0:e4bf955dac58 75 void mid1(){
Minato 0:e4bf955dac58 76 mid_t=t2.read_ms();
Minato 0:e4bf955dac58 77 mid_a=an1.read();
Minato 0:e4bf955dac58 78 mid_b=an2.read();
Minato 0:e4bf955dac58 79 }
Minato 0:e4bf955dac58 80
Minato 0:e4bf955dac58 81 void alt1() {
Minato 0:e4bf955dac58 82 t.start();
Minato 0:e4bf955dac58 83 }
Minato 0:e4bf955dac58 84 void alt2() {
Minato 0:e4bf955dac58 85 t.stop();
Minato 0:e4bf955dac58 86 data.height=(int)(t.read_us()/58);
Minato 0:e4bf955dac58 87 t.reset();
Minato 0:e4bf955dac58 88 }
Minato 0:e4bf955dac58 89 void rot1(){
Minato 0:e4bf955dac58 90 rot_count_buf++;
Minato 0:e4bf955dac58 91 }
Minato 0:e4bf955dac58 92
Minato 0:e4bf955dac58 93 void rot2(){
Minato 0:e4bf955dac58 94 data.rot_count=rot_count_buf;
Minato 0:e4bf955dac58 95 rot_count_buf=0;
Minato 0:e4bf955dac58 96 send_f1=1;
Minato 0:e4bf955dac58 97 }
Minato 0:e4bf955dac58 98
Minato 0:e4bf955dac58 99
Minato 0:e4bf955dac58 100 void send(){//disp
Minato 0:e4bf955dac58 101 //def
Minato 0:e4bf955dac58 102 Serial2.printf("%d,%d,%.2f,%d,%d,%.2f,%.2f,%f,%f\r\n",data.height,data.rot_count,data.air_speed,data.a,data.b,data.fmps,data.angle[0],data.analog[0],data.analog[1]);
Minato 0:e4bf955dac58 103
Minato 0:e4bf955dac58 104 //pc.printf("%d,%d,%.2f,%d,%d,%.2f,%.2f,%f,%f\r\n",data.height,data.rot_count,data.air_speed,data.a,data.b,data.fmps,data.angle[0],data.analog[0],data.analog[1]);
Minato 0:e4bf955dac58 105 pc.printf("time=%5ds, height=%4.2fm, rot=%3drpm, speed=%4.2fm/s, ELE=%f, RUD=%f, %d, %d, %d, %d, %d, %.2f, %.2f, %.2f, %.2f, %.2f\r\n"
Minato 0:e4bf955dac58 106 ,t2.read_ms()/1000,data.height/100,data.rot_count,data.air_speed
Minato 0:e4bf955dac58 107 ,data.analog[0],data.analog[1],data.date,data.time
Minato 0:e4bf955dac58 108 ,data.lat,data.lon,data.course,data.fmps,data.falt,data.angle[0],data.angle[1],data.angle[2]);
Minato 0:e4bf955dac58 109 //rudele
Minato 0:e4bf955dac58 110 //Serial2.printf("ELE,RUD,%d,%d,%f,%f\r\n",data.a,data.b,data.analog[0],data.analog[1]);
Minato 0:e4bf955dac58 111
Minato 0:e4bf955dac58 112 //Serial2.printf("%d,%d,%.2f,%.2f\r\n",data.a,data.b,data.analog[0],data.analog[1]);
Minato 0:e4bf955dac58 113 /*Serial2.printf("%d,%d,%d,%.2f,%f,%f,%d,%d,%d,%d,%d,%.2f,%.2f,%.2f,%.2f,%.2f\r\n"
Minato 0:e4bf955dac58 114 ,t2.read_ms(),data.height,data.rot_count,data.air_speed
Minato 0:e4bf955dac58 115 ,data.analog[0],data.analog[1],data.date,data.time
Minato 0:e4bf955dac58 116 ,data.lat,data.lon,data.course,data.fmps,data.falt,data.angle[0],data.angle[1],data.angle[2]);*/
Minato 0:e4bf955dac58 117 }
Minato 0:e4bf955dac58 118
Minato 0:e4bf955dac58 119 void send2(){//log
Minato 0:e4bf955dac58 120 Serial1.printf("%d,%d,%d,%.2f,%f,%f,%d,%d,%d,%d,%d,%.2f,%.2f,%.2f,%.2f,%.2f\r\n"
Minato 0:e4bf955dac58 121 ,t2.read_ms(),data.height,data.rot_count,data.air_speed
Minato 0:e4bf955dac58 122 ,data.analog[0],data.analog[1],data.date,data.time
Minato 0:e4bf955dac58 123 ,data.lat,data.lon,data.course,data.fmps,data.falt,data.angle[0],data.angle[1],data.angle[2]);
Minato 0:e4bf955dac58 124 }
Minato 0:e4bf955dac58 125
Minato 0:e4bf955dac58 126
Minato 0:e4bf955dac58 127 void send2_f(){
Minato 0:e4bf955dac58 128 send_f2=1;
Minato 0:e4bf955dac58 129 }
Minato 0:e4bf955dac58 130
Minato 0:e4bf955dac58 131
Minato 0:e4bf955dac58 132 int main() {
Minato 0:e4bf955dac58 133 char a[10];//バッファ
Minato 0:e4bf955dac58 134 int i=0;//count用
Minato 0:e4bf955dac58 135 int prev_m=0;
Minato 0:e4bf955dac58 136 ini();
Minato 0:e4bf955dac58 137 t.start();
Minato 0:e4bf955dac58 138 t2.start();
Minato 0:e4bf955dac58 139 alt.rise(&alt1);//立ち上がり
Minato 0:e4bf955dac58 140 alt.fall(&alt2);//立ち下がり
Minato 0:e4bf955dac58 141 rot.rise(&rot1);//立ち上がり
Minato 0:e4bf955dac58 142 rot.fall(&rot1);//立ち下がり
Minato 0:e4bf955dac58 143 //mid.fall(&mid1);//立ち上がり
Minato 0:e4bf955dac58 144
Minato 0:e4bf955dac58 145 flipper.attach(&rot2, 0.5);//0.5sごとに回転数更新＆送信
Minato 0:e4bf955dac58 146 i2c.calibrateAll(3000);
Minato 0:e4bf955dac58 147 flipper2.attach(&send2_f, 0.2);//0.2sごとに送信
Minato 0:e4bf955dac58 148 while(1) {
Minato 0:e4bf955dac58 149 while(Serial1.readable()>0){
Minato 0:e4bf955dac58 150 a[i]=Serial1.getc();
Minato 0:e4bf955dac58 151 if(a[i]=='\r'){
Minato 0:e4bf955dac58 152 a[i]=Serial1.getc();//\n
Minato 0:e4bf955dac58 153 a[i]='\0';
Minato 0:e4bf955dac58 154 i=0;
Minato 0:e4bf955dac58 155 data.air_speed=atof(a);
Minato 0:e4bf955dac58 156 }else{
Minato 0:e4bf955dac58 157 i++;
Minato 0:e4bf955dac58 158 }
Minato 0:e4bf955dac58 159 }
Minato 0:e4bf955dac58 160
Minato 0:e4bf955dac58 161 while(Serial2.readable()>0){
Minato 0:e4bf955dac58 162 char c = Serial2.getc();
Minato 0:e4bf955dac58 163 if(gps.encode(c)){
Minato 0:e4bf955dac58 164 gps.get_position(&(data.lat), &(data.lon), &(data.fix_age));// retrieves +/- lat/long in 100000ths of a degree
Minato 0:e4bf955dac58 165 gps.get_datetime(&(data.date), &(data.time), &(data.fix_age));// time in hhmmsscc
Minato 0:e4bf955dac58 166 data.course = gps.course();// course in 100ths of a degree
Minato 0:e4bf955dac58 167 data.fmps = gps.f_speed_mps(); // speed in m/sec
Minato 0:e4bf955dac58 168 data.falt = gps.f_altitude(); // +/- altitude in meters
Minato 0:e4bf955dac58 169 }
Minato 0:e4bf955dac58 170 }
Minato 0:e4bf955dac58 171 if(send_f1==1){
Minato 0:e4bf955dac58 172 send_f1=0;
Minato 0:e4bf955dac58 173 send();
Minato 0:e4bf955dac58 174 }
Minato 0:e4bf955dac58 175 if(send_f2==1){
Minato 0:e4bf955dac58 176 read_gyro();
Minato 0:e4bf955dac58 177 read_analog();
Minato 0:e4bf955dac58 178 send_f2=0;
Minato 0:e4bf955dac58 179 send2();
Minato 0:e4bf955dac58 180 int a;
Minato 0:e4bf955dac58 181 a=mid;
Minato 0:e4bf955dac58 182 if(a==prev_m && !a && mid_t==0){
Minato 0:e4bf955dac58 183 mid1();
Minato 0:e4bf955dac58 184 Serial1.printf("##%f,%f\r\n",mid_a,mid_b);
Minato 0:e4bf955dac58 185 Serial2.printf("##%f,%f\r\n",mid_a,mid_b);
Minato 0:e4bf955dac58 186 }
Minato 0:e4bf955dac58 187 prev_m=a;
Minato 0:e4bf955dac58 188 }
Minato 0:e4bf955dac58 189 }
Minato 0:e4bf955dac58 190 }