CansatB_2021
/
B_test_gpsXbee
GPSとXbee統合試験用(作:皆川 )
main.cpp@0:44f6af5eecc5, 2021-10-11 (annotated)
- Committer:
- odaa
- Date:
- Mon Oct 11 18:37:43 2021 +0000
- Revision:
- 0:44f6af5eecc5
- Child:
- 1:8d166b7409af
xbee and gps
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
odaa | 0:44f6af5eecc5 | 1 | #include "mbed.h" |
odaa | 0:44f6af5eecc5 | 2 | #include"getGPS.h" |
odaa | 0:44f6af5eecc5 | 3 | |
odaa | 0:44f6af5eecc5 | 4 | |
odaa | 0:44f6af5eecc5 | 5 | |
odaa | 0:44f6af5eecc5 | 6 | Serial xbee(D1, D0); |
odaa | 0:44f6af5eecc5 | 7 | GPS gps(A2,A7); |
odaa | 0:44f6af5eecc5 | 8 | int end_flag=0; |
odaa | 0:44f6af5eecc5 | 9 | double bias_la=0,bias_lo=0; |
odaa | 0:44f6af5eecc5 | 10 | |
odaa | 0:44f6af5eecc5 | 11 | int main() |
odaa | 0:44f6af5eecc5 | 12 | { |
odaa | 0:44f6af5eecc5 | 13 | |
odaa | 0:44f6af5eecc5 | 14 | xbee.printf("XBee Connected\r\n"); |
odaa | 0:44f6af5eecc5 | 15 | |
odaa | 0:44f6af5eecc5 | 16 | while(1) { |
odaa | 0:44f6af5eecc5 | 17 | |
odaa | 0:44f6af5eecc5 | 18 | xbee.printf("start command\t(S or s)\r\nend command\t(E or e)"); |
odaa | 0:44f6af5eecc5 | 19 | |
odaa | 0:44f6af5eecc5 | 20 | int received_data = xbee.getc(); |
odaa | 0:44f6af5eecc5 | 21 | |
odaa | 0:44f6af5eecc5 | 22 | xbee.printf("\r\n---------------\r\nReceived Data: %c\r\n", received_data); |
odaa | 0:44f6af5eecc5 | 23 | |
odaa | 0:44f6af5eecc5 | 24 | if(received_data == 83 || received_data == 115) |
odaa | 0:44f6af5eecc5 | 25 | { //S or s |
odaa | 0:44f6af5eecc5 | 26 | double goal_latitude,goal_longtitude; |
odaa | 0:44f6af5eecc5 | 27 | xbee.printf("Mission Start\r\n"); |
odaa | 0:44f6af5eecc5 | 28 | xbee.printf("please enter goal latitude:"); |
odaa | 0:44f6af5eecc5 | 29 | xbee.scanf("%lf",&goal_latitude); |
odaa | 0:44f6af5eecc5 | 30 | xbee.printf("\r\nplease enter goal longtitude:"); |
odaa | 0:44f6af5eecc5 | 31 | xbee.scanf("%lf",&goal_longtitude); |
odaa | 0:44f6af5eecc5 | 32 | xbee.printf("\r\n--------------\r\nstart GPS\r\n------------------\r\n"); |
odaa | 0:44f6af5eecc5 | 33 | end_flag=0; |
odaa | 0:44f6af5eecc5 | 34 | |
odaa | 0:44f6af5eecc5 | 35 | |
odaa | 0:44f6af5eecc5 | 36 | while(end_flag==0) |
odaa | 0:44f6af5eecc5 | 37 | { |
odaa | 0:44f6af5eecc5 | 38 | if(gps.getgps()) |
odaa | 0:44f6af5eecc5 | 39 | { //現在地取得 |
odaa | 0:44f6af5eecc5 | 40 | xbee.printf("(latitude:%lf, longtitude:%lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力 |
odaa | 0:44f6af5eecc5 | 41 | } |
odaa | 0:44f6af5eecc5 | 42 | |
odaa | 0:44f6af5eecc5 | 43 | else |
odaa | 0:44f6af5eecc5 | 44 | { |
odaa | 0:44f6af5eecc5 | 45 | xbee.printf("No data\r\n");//データ取得に失敗した場合 |
odaa | 0:44f6af5eecc5 | 46 | } |
odaa | 0:44f6af5eecc5 | 47 | |
odaa | 0:44f6af5eecc5 | 48 | if(abs(gps.latitude-double(goal_latitude))<=bias_la&&abs(gps.latitude-double(goal_latitude))<=bias_lo) |
odaa | 0:44f6af5eecc5 | 49 | { |
odaa | 0:44f6af5eecc5 | 50 | xbee.printf("potion match\r\n"); |
odaa | 0:44f6af5eecc5 | 51 | end_flag=1; |
odaa | 0:44f6af5eecc5 | 52 | } |
odaa | 0:44f6af5eecc5 | 53 | if(abs(gps.latitude-double(goal_latitude))>bias_la||abs(gps.latitude-double(goal_latitude))>bias_lo) |
odaa | 0:44f6af5eecc5 | 54 | { |
odaa | 0:44f6af5eecc5 | 55 | xbee.printf("\r\n-----\r\n(latitude bias:%lf,longtitude bias:%lf)\r\n",gps.latitude-double(goal_latitude),gps.latitude-double(goal_latitude)); |
odaa | 0:44f6af5eecc5 | 56 | } |
odaa | 0:44f6af5eecc5 | 57 | wait(0.5); |
odaa | 0:44f6af5eecc5 | 58 | |
odaa | 0:44f6af5eecc5 | 59 | } |
odaa | 0:44f6af5eecc5 | 60 | |
odaa | 0:44f6af5eecc5 | 61 | |
odaa | 0:44f6af5eecc5 | 62 | |
odaa | 0:44f6af5eecc5 | 63 | } |
odaa | 0:44f6af5eecc5 | 64 | else if(received_data == 67 || received_data == 99) |
odaa | 0:44f6af5eecc5 | 65 | { //C or c |
odaa | 0:44f6af5eecc5 | 66 | xbee.printf("%c\r\n", received_data); |
odaa | 0:44f6af5eecc5 | 67 | xbee.printf("Mission Complete\r\n"); |
odaa | 0:44f6af5eecc5 | 68 | break; |
odaa | 0:44f6af5eecc5 | 69 | } |
odaa | 0:44f6af5eecc5 | 70 | break; |
odaa | 0:44f6af5eecc5 | 71 | } |
odaa | 0:44f6af5eecc5 | 72 | return 0; |
odaa | 0:44f6af5eecc5 | 73 | } |
odaa | 0:44f6af5eecc5 | 74 | |
odaa | 0:44f6af5eecc5 | 75 |