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?
User | Revision | Line number | New 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 | } |