aigamozu / Mbed 2 deprecated Aigamozu_Robot_2016_ver1

2016_05_19 Auto mode: 10sec forward, 2sec stop, 2sec turn right Please change test Auto pwm

Dependencies:   ADXL345 AigamozuControlPackets_2016 HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST_2016 mbed

Fork of Aigamozu_Robot_ver4 by CanSat2015aizu

main.cpp@40:aa600a5ba02c, 2016-05-25 (annotated)

Committer:
s1210160
Date:
Wed May 25 11:26:20 2016 +0000
Revision:
40:aa600a5ba02c
Parent:
39:ff6819c7a066
Child:
41:d6350b57f02a
2016/5/25

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kityann 0:daab5accfd83 1 /**********************************************/
s1210160 39:ff6819c7a066 2 //
s1210160 39:ff6819c7a066 3 //
kityann 0:daab5accfd83 4 //
kityann 0:daab5accfd83 5 // Program name: Aigamozu ROBOT
kityann 0:daab5accfd83 6 // Author: Mineta Kizuku
s1210160 39:ff6819c7a066 7 //
s1210160 39:ff6819c7a066 8 //
kityann 0:daab5accfd83 9 //
kityann 0:daab5accfd83 10 /**********************************************/
kityann 0:daab5accfd83 11
kityann 0:daab5accfd83 12 /**********************************************/
kityann 0:daab5accfd83 13 //更新情報
kityann 0:daab5accfd83 14 //2015/05/11
kityann 0:daab5accfd83 15 //ロボットプログラムの作成
kityann 0:daab5accfd83 16 //
kityann 4:f36986ceb73d 17 //2015/05/13
kityann 4:f36986ceb73d 18 //カルマンフィルタの共分散の値を0.0001以下にならないようにした
kityann 4:f36986ceb73d 19 //共分散の値を10進数に変換するようにした
kityann 0:daab5accfd83 20 //
s1200058 21:76e7f36df4a9 21 //2015/05/13 Yokokawa
s1200058 6:13f212b75e71 22 //何回目のGPSでとられたGPSか確認するようにしました。
s1200058 6:13f212b75e71 23 //
s1200058 17:e563cf8e6bcf 24 //2015/05/15
s1200058 17:e563cf8e6bcf 25 //プログラムcreateReceiveStatusCommand()にて
s1200058 17:e563cf8e6bcf 26 //Aigamozu_collect_dataにinかoutかを送るためにstate関連をいじったので必要に応じて直してください。
s1200058 17:e563cf8e6bcf 27 //
kityann 23:c5071bf93db1 28 //2015/05//17
kityann 23:c5071bf93db1 29 //Send_Status関数内を変更：基地局への送信データのみ現在のモードを入れるパケットの部分に内外判定の結果を入れるようにした
kityann 23:c5071bf93db1 30 //基地局以外には現在のモードを送信するようにしてある
kityann 23:c5071bf93db1 31 //要確認！！！！
s1200058 28:4ccd7cfc6b1b 32 //
s1200058 28:4ccd7cfc6b1b 33 //2015/05/17
s1200058 28:4ccd7cfc6b1b 34 //Get_GPS()の中身longitudeの範囲138〜140に変更
s1200058 28:4ccd7cfc6b1b 35 //
s1200058 33:3025b16bccd2 36 //2015/05/19
s1200058 33:3025b16bccd2 37 //autoモードのとき、三十秒前進・三秒右に転回に変更した。
s1200058 33:3025b16bccd2 38 //
s1200058 32:6ba2e5402f00 39 //2015/05/24
s1200058 32:6ba2e5402f00 40 //Kalmanフィルターを十進数で計算するようにした。
s1200058 32:6ba2e5402f00 41 //Kalmanフィルターの計算式を変更した。
s1200058 32:6ba2e5402f00 42 //set_kalmanを追加した。
s1200058 32:6ba2e5402f00 43 //
kityann 0:daab5accfd83 44 /**********************************************/
kityann 0:daab5accfd83 45
kityann 0:daab5accfd83 46 #include "mbed.h"
kityann 0:daab5accfd83 47 #include "XBee.h"
kityann 0:daab5accfd83 48 #include "MBed_Adafruit_GPS.h"
kityann 0:daab5accfd83 49 #include "AigamozuControlPackets.h"
kityann 0:daab5accfd83 50 #include "agzIDLIST.h"
kityann 0:daab5accfd83 51 #include "aigamozuSetting.h"
kityann 0:daab5accfd83 52 #include "Kalman.h"
s1200058 34:9c3192c00e41 53 #include "math.h"
kityann 0:daab5accfd83 54
kityann 4:f36986ceb73d 55 #define SIGMA_MIN 0.0001
s1210160 40:aa600a5ba02c 56 #define KALMAN_START_TIME 5
kityann 4:f36986ceb73d 57
kityann 0:daab5accfd83 58 //************ID Number*****************
kityann 0:daab5accfd83 59 //Robot ID: 'A' ~ 'Z'
kityann 0:daab5accfd83 60 //Base ID: 'a' ~ 'a'
kityann 0:daab5accfd83 61 //manager ID: '0'(数字のゼロ)
kityann 0:daab5accfd83 62 //
s1210160 39:ff6819c7a066 63 const char MyID = 'C';
s1210160 39:ff6819c7a066 64 const int other_robot = 7;
kityann 0:daab5accfd83 65 //************ID Number*****************
kityann 0:daab5accfd83 66
kityann 0:daab5accfd83 67 /////////////////////////////////////////
kityann 0:daab5accfd83 68 //
kityann 0:daab5accfd83 69 //Pin Setting
kityann 0:daab5accfd83 70 //
kityann 0:daab5accfd83 71 /////////////////////////////////////////
kityann 0:daab5accfd83 72 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:daab5accfd83 73
kityann 0:daab5accfd83 74
kityann 0:daab5accfd83 75 /////////////////////////////////////////
kityann 0:daab5accfd83 76 //
kityann 0:daab5accfd83 77 //Connection Setting
kityann 0:daab5accfd83 78 //
kityann 0:daab5accfd83 79 /////////////////////////////////////////
kityann 0:daab5accfd83 80
kityann 0:daab5accfd83 81 //Serial Connect Setting: PC <--> mbed
s1210160 39:ff6819c7a066 82 Serial pc(USBTX, USBRX);
kityann 0:daab5accfd83 83
kityann 0:daab5accfd83 84 //Serial Connect Setting: GPS <--> mbed
kityann 0:daab5accfd83 85 Serial * gps_Serial;
kityann 0:daab5accfd83 86
kityann 0:daab5accfd83 87 //Serial Connect Setting: XBEE <--> mbed
kityann 0:daab5accfd83 88 XBee xbee(p13,p14);
kityann 0:daab5accfd83 89 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:daab5accfd83 90
kityann 0:daab5accfd83 91 //set up GPS module
kityann 0:daab5accfd83 92
kityann 0:daab5accfd83 93 //set up AigamozuControlPackets library
kityann 0:daab5accfd83 94 AigamozuControlPackets agz(agz_motorShield);
kityann 0:daab5accfd83 95
kityann 0:daab5accfd83 96
kityann 0:daab5accfd83 97 /////////////////////////////////////////
kityann 0:daab5accfd83 98 //
kityann 0:daab5accfd83 99 //For Kalman data
kityann 0:daab5accfd83 100 //
kityann 0:daab5accfd83 101 /////////////////////////////////////////
s1200058 32:6ba2e5402f00 102 #define FIRST_S2 0.000001
s1200058 32:6ba2e5402f00 103 #define COUNTER_MAX 10000
s1200058 32:6ba2e5402f00 104 double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
s1200058 32:6ba2e5402f00 105 double s2x_cur=FIRST_S2,s2x_prev=FIRST_S2,s2y_cur=FIRST_S2,s2y_prev=FIRST_S2;//緯度経度のの時刻tと時刻t-1での共分散
s1200058 32:6ba2e5402f00 106 double s2_R=FIRST_S2;//GPSセンサの分散
s1200058 32:6ba2e5402f00 107 double Kx=0,Ky=0;//カルマンゲイン
s1200058 32:6ba2e5402f00 108 double zx,zy;//観測値
s1200058 32:6ba2e5402f00 109 void Kalman(double Latitude,double Longitude);
s1200058 34:9c3192c00e41 110 int change = 0;
s1200058 34:9c3192c00e41 111
s1200058 37:19a9a37a5658 112 /*
s1200058 34:9c3192c00e41 113 LocalFileSystem local("local"); // マウントポイントを定義（ディレクトリパスになる）
s1200058 35:c18c96b92121 114 FILE *fp;
s1200058 35:c18c96b92121 115 char filename[16] = "/local/out0.txt";
s1200058 37:19a9a37a5658 116 */
s1200058 2:886fac7f4399 117 /////////////////////////////////////////
s1200058 2:886fac7f4399 118 //
s1200058 2:886fac7f4399 119 //Plus Speed
s1200058 2:886fac7f4399 120 //
s1200058 2:886fac7f4399 121 //MNUAL_MODEの時にスピードを変える
s1200058 2:886fac7f4399 122 /////////////////////////////////////////
s1210160 39:ff6819c7a066 123 void Plus_Speed(uint8_t *packetdata)
s1210160 39:ff6819c7a066 124 {
s1210160 39:ff6819c7a066 125
s1210160 39:ff6819c7a066 126 if(agz.nowMode == MANUAL_MODE) {
s1200058 2:886fac7f4399 127 agz.changeSpeed(packetdata);
s1200058 2:886fac7f4399 128 }
s1210160 39:ff6819c7a066 129
s1200058 2:886fac7f4399 130 }
kityann 0:daab5accfd83 131
s1210160 39:ff6819c7a066 132
kityann 0:daab5accfd83 133 /////////////////////////////////////////
kityann 0:daab5accfd83 134 //
s1200058 2:886fac7f4399 135 //Send Status
kityann 0:daab5accfd83 136 //
kityann 0:daab5accfd83 137 //リクエストがきたとき、自分の位置情報などを返信する
kityann 0:daab5accfd83 138 /////////////////////////////////////////
s1210160 39:ff6819c7a066 139 void Send_Status(char SenderIDc)
s1210160 39:ff6819c7a066 140 {
kityann 0:daab5accfd83 141 XBeeAddress64 send_Address;
s1210160 39:ff6819c7a066 142 if(SenderIDc == '0') {
kityann 0:daab5accfd83 143 send_Address = manager_Address;
kityann 23:c5071bf93db1 144 agz.createReceiveStatusCommand(MyID,SenderIDc, agz.nowMode,
s1210160 39:ff6819c7a066 145 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1210160 39:ff6819c7a066 146 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1210160 39:ff6819c7a066 147 agz.get_agzCov_lati(),agz.get_agzCov_longi());
kityann 0:daab5accfd83 148 }
s1210160 39:ff6819c7a066 149 if(SenderIDc >= 'A' && SenderIDc <= 'Z') {
kityann 0:daab5accfd83 150 send_Address = robot_Address[SenderIDc - 'A'];
s1200058 25:ae5fab0946f2 151 //Create GPS Infomation Packet
kityann 24:698d4e920d33 152 agz.createReceiveStatusCommand(MyID,SenderIDc, agz.nowMode,
s1210160 39:ff6819c7a066 153 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1210160 39:ff6819c7a066 154 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1210160 39:ff6819c7a066 155 agz.get_agzCov_lati(),agz.get_agzCov_longi());
kityann 0:daab5accfd83 156 }
s1210160 39:ff6819c7a066 157 if(SenderIDc >= 'a' && SenderIDc <= 'z') {
kityann 0:daab5accfd83 158 send_Address = base_Address[SenderIDc - 'a'];
kityann 24:698d4e920d33 159
kityann 24:698d4e920d33 160 agz.createReceiveStatusCommand(MyID,SenderIDc, (int)agz.gpsAuto(),
s1210160 39:ff6819c7a066 161 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1210160 39:ff6819c7a066 162 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1210160 39:ff6819c7a066 163 agz.get_agzCov_lati(),agz.get_agzCov_longi());
kityann 0:daab5accfd83 164 }
kityann 0:daab5accfd83 165 //send normal data
kityann 23:c5071bf93db1 166
s1210160 39:ff6819c7a066 167 /*
s1210160 39:ff6819c7a066 168 //debug***************************************************
s1210160 39:ff6819c7a066 169 printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
s1210160 39:ff6819c7a066 170 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1210160 39:ff6819c7a066 171 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1210160 39:ff6819c7a066 172 agz.get_agzCov_lati(),agz.get_agzCov_longi()
s1210160 39:ff6819c7a066 173 );
s1210160 39:ff6819c7a066 174 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
s1210160 39:ff6819c7a066 175 printf("\n");
s1210160 39:ff6819c7a066 176 //debug end***************************************************
s1210160 39:ff6819c7a066 177 */
kityann 0:daab5accfd83 178 //Select Destination
kityann 0:daab5accfd83 179 ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
kityann 0:daab5accfd83 180 //Send -> Base
kityann 0:daab5accfd83 181 xbee.send(tx64request);
s1210160 39:ff6819c7a066 182
kityann 0:daab5accfd83 183 }
kityann 0:daab5accfd83 184
kityann 0:daab5accfd83 185 /////////////////////////////////////////
kityann 0:daab5accfd83 186 //
kityann 0:daab5accfd83 187 //Get GPS function
kityann 0:daab5accfd83 188 //
kityann 0:daab5accfd83 189 /////////////////////////////////////////
kityann 0:daab5accfd83 190
s1210160 39:ff6819c7a066 191 void Get_GPS(Adafruit_GPS *myGPS)
s1210160 39:ff6819c7a066 192 {
s1210160 39:ff6819c7a066 193 printf("test");
s1200058 32:6ba2e5402f00 194 static int flag = 0;
s1200058 37:19a9a37a5658 195 // static int save_counter = 0;
s1210160 39:ff6819c7a066 196
kityann 0:daab5accfd83 197 if (myGPS->fix) {
s1210160 39:ff6819c7a066 198
kityann 0:daab5accfd83 199 agz.nowStatus = GPS_AVAIL;
s1200058 32:6ba2e5402f00 200 agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
s1210160 39:ff6819c7a066 201
s1210160 39:ff6819c7a066 202 if(flag < COUNTER_MAX) {
s1210160 39:ff6819c7a066 203 flag++;
s1200058 32:6ba2e5402f00 204 }
s1210160 40:aa600a5ba02c 205 if(flag == KALMAN_START_TIME && agz.nowMode == AUTO_GPS_MODE) {
s1200058 32:6ba2e5402f00 206 x_prev = agz.get_agzPoint_lati();
s1200058 32:6ba2e5402f00 207 y_prev = agz.get_agzPoint_longi();
kityann 0:daab5accfd83 208 }
s1210160 39:ff6819c7a066 209
s1210160 40:aa600a5ba02c 210 if(flag >= KALMAN_START_TIME+1 && agz.nowMode == AUTO_GPS_MODE) {
s1210160 39:ff6819c7a066 211 if(abs(x_prev - agz.get_agzPoint_lati()) < 0.001 && abs(y_prev - agz.get_agzPoint_longi()) < 0.001) {
s1200058 34:9c3192c00e41 212 Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi());
s1200058 34:9c3192c00e41 213 change = 1;
s1210160 39:ff6819c7a066 214 } else {
s1200058 34:9c3192c00e41 215 change = 0;
s1200058 34:9c3192c00e41 216 }
s1210160 39:ff6819c7a066 217 /* if(save_counter < 10){
s1210160 39:ff6819c7a066 218 fp = fopen(filename, "a");
s1210160 39:ff6819c7a066 219 fprintf(fp, "%d %.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
s1210160 39:ff6819c7a066 220 change, agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1210160 39:ff6819c7a066 221 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1210160 39:ff6819c7a066 222 agz.get_agzCov_lati(),agz.get_agzCov_longi());
s1210160 39:ff6819c7a066 223 fclose(fp);
s1210160 39:ff6819c7a066 224
s1210160 39:ff6819c7a066 225 if((flag - 16) % 500 == 0){
s1210160 39:ff6819c7a066 226 filename[10]++;
s1210160 39:ff6819c7a066 227 save_counter++;
s1210160 39:ff6819c7a066 228 }
s1210160 39:ff6819c7a066 229 }
s1210160 39:ff6819c7a066 230 */
s1210160 39:ff6819c7a066 231 }
s1210160 39:ff6819c7a066 232
s1210160 39:ff6819c7a066 233 printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
s1210160 39:ff6819c7a066 234 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1210160 39:ff6819c7a066 235 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1210160 39:ff6819c7a066 236 agz.get_agzCov_lati(),agz.get_agzCov_longi());
s1210160 39:ff6819c7a066 237
s1210160 39:ff6819c7a066 238 agz.createReceiveStatusCommand(MyID, other_robot, agz.nowMode,
s1210160 39:ff6819c7a066 239 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1210160 39:ff6819c7a066 240 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1210160 39:ff6819c7a066 241 agz.get_agzCov_lati(),agz.get_agzCov_longi());
s1210160 39:ff6819c7a066 242 //Select Destination
s1210160 39:ff6819c7a066 243 ZBTxRequest tx64request(robot_Address[other_robot],agz.packetData,agz.getPacketLength());
s1210160 39:ff6819c7a066 244 //Send -> Base
s1210160 39:ff6819c7a066 245 xbee.send(tx64request);
s1210160 39:ff6819c7a066 246 } else agz.nowStatus = GPS_UNAVAIL;
s1210160 39:ff6819c7a066 247
s1210160 39:ff6819c7a066 248 }
s1200058 2:886fac7f4399 249
s1200058 2:886fac7f4399 250 /////////////////////////////////////////
s1200058 2:886fac7f4399 251 //
s1200058 2:886fac7f4399 252 //New Mode
s1200058 2:886fac7f4399 253 //
s1200058 2:886fac7f4399 254 /////////////////////////////////////////
s1200058 2:886fac7f4399 255
s1210160 39:ff6819c7a066 256 void New_Mode(uint8_t *packetdata)
s1210160 39:ff6819c7a066 257 {
s1210160 39:ff6819c7a066 258
s1210160 39:ff6819c7a066 259 //bool result;
s1210160 39:ff6819c7a066 260 agz.changeMode(packetdata);
s1210160 39:ff6819c7a066 261
s1200058 2:886fac7f4399 262 }
s1210160 39:ff6819c7a066 263
kityann 1:b2b950b916ce 264 /////////////////////////////////////////
kityann 1:b2b950b916ce 265 //
kityann 1:b2b950b916ce 266 //Get Status
kityann 1:b2b950b916ce 267 //
kityann 1:b2b950b916ce 268 /////////////////////////////////////////
s1210160 39:ff6819c7a066 269 void Get_Status(char SenderIDc,uint8_t *packetdata)
s1210160 39:ff6819c7a066 270 {
s1210160 39:ff6819c7a066 271 printf("get status\n");
s1210160 39:ff6819c7a066 272
kityann 1:b2b950b916ce 273 //マネージャからデータが来たとき
s1210160 39:ff6819c7a066 274 if(SenderIDc == '0') {
kityann 1:b2b950b916ce 275 printf("get manager Status\n");
kityann 1:b2b950b916ce 276 }
kityann 1:b2b950b916ce 277 //他のロボットからデータが来たとき
s1210160 39:ff6819c7a066 278 if(SenderIDc >= 'A' && SenderIDc <= 'Z') {
s1210160 39:ff6819c7a066 279 int id = SenderIDc - 'A';
s1210160 39:ff6819c7a066 280 printf("get other robots Status %d\n", id+1);
s1210160 39:ff6819c7a066 281
kityann 1:b2b950b916ce 282 }
kityann 1:b2b950b916ce 283 //基地局からデータが来たとき
s1210160 39:ff6819c7a066 284 if(SenderIDc >= 'a' && SenderIDc <= 'z') {
kityann 1:b2b950b916ce 285 int id = SenderIDc - 'a';
s1210160 39:ff6819c7a066 286 printf("Get Base data %d\n", id+1);
kityann 1:b2b950b916ce 287 agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
s1210160 39:ff6819c7a066 288 //agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
s1210160 39:ff6819c7a066 289
kityann 1:b2b950b916ce 290 //debug
s1210160 39:ff6819c7a066 291 printf("latitude:%f,longitude:%f\n",
s1210160 39:ff6819c7a066 292 agz.get_basePoint_lati(id),agz.get_basePoint_longi(id)
s1210160 39:ff6819c7a066 293 );
kityann 1:b2b950b916ce 294 }
kityann 1:b2b950b916ce 295 }
kityann 1:b2b950b916ce 296
s1210160 39:ff6819c7a066 297 void Get_Status_Kalman(char SenderIDc,uint8_t *packetdata)
s1210160 39:ff6819c7a066 298 {
s1210160 39:ff6819c7a066 299
s1200058 2:886fac7f4399 300 //マネージャからデータが来たとき
s1210160 39:ff6819c7a066 301 if(SenderIDc == '0') {
s1200058 2:886fac7f4399 302 printf("get manager Status Kalman\n");
s1200058 2:886fac7f4399 303 }
s1200058 2:886fac7f4399 304 //他のロボットからデータが来たとき
s1210160 39:ff6819c7a066 305 if(SenderIDc >= 'A' && SenderIDc <= 'Z') {
s1200058 2:886fac7f4399 306 printf("get other robots Status Kalman\n");
s1210160 39:ff6819c7a066 307 int id = SenderIDc - 'A';
s1200058 2:886fac7f4399 308 }
s1200058 2:886fac7f4399 309 //基地局からデータが来たとき
s1210160 39:ff6819c7a066 310 if(SenderIDc >= 'a' && SenderIDc <= 'z') {
s1200058 2:886fac7f4399 311 printf("Get Base data Kalman\n");
s1200058 2:886fac7f4399 312 int id = SenderIDc - 'a';
s1200058 2:886fac7f4399 313 agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
s1210160 39:ff6819c7a066 314
s1200058 2:886fac7f4399 315 //debug
s1210160 39:ff6819c7a066 316 printf("BASE %d: latitudeK:%f,longitudeK:%f\n",
s1210160 39:ff6819c7a066 317 id, agz.get_basePointKalman_lati(id),agz.get_basePointKalman_longi(id));
s1200058 2:886fac7f4399 318 }
s1200058 2:886fac7f4399 319 }
s1200058 2:886fac7f4399 320
kityann 1:b2b950b916ce 321 /////////////////////////////////////////
kityann 1:b2b950b916ce 322 //
kityann 1:b2b950b916ce 323 //Send_Request_to_base
kityann 1:b2b950b916ce 324 //
kityann 1:b2b950b916ce 325 /////////////////////////////////////////
s1210160 39:ff6819c7a066 326 void Send_Request_Base(int basenumber)
s1210160 39:ff6819c7a066 327 {
s1210160 39:ff6819c7a066 328 //printf("send\n");
kityann 1:b2b950b916ce 329 agz.createRequestCommand(MyID, basenumber);
kityann 1:b2b950b916ce 330 //Select Destination
kityann 1:b2b950b916ce 331 ZBTxRequest tx64request(base_Address[basenumber],agz.packetData,agz.getPacketLength());
kityann 1:b2b950b916ce 332 //Send -> Base
kityann 1:b2b950b916ce 333 xbee.send(tx64request);
kityann 1:b2b950b916ce 334 }
kityann 0:daab5accfd83 335
s1200058 3:1ac506a96fd6 336 /////////////////////////////////////////
s1200058 3:1ac506a96fd6 337 //
s1200058 3:1ac506a96fd6 338 //auto_Move
s1200058 3:1ac506a96fd6 339 //
s1200058 3:1ac506a96fd6 340 //InAreaかOutAreaの判定
s1200058 3:1ac506a96fd6 341 //Kalmanを通した値を出力(Baseと自分)
s1200058 3:1ac506a96fd6 342 /////////////////////////////////////////
s1200058 3:1ac506a96fd6 343
s1210160 39:ff6819c7a066 344 void auto_Move()
s1210160 39:ff6819c7a066 345 {
s1210160 39:ff6819c7a066 346
s1210160 39:ff6819c7a066 347 bool result;
s1210160 39:ff6819c7a066 348 int i;
s1210160 39:ff6819c7a066 349
s1210160 39:ff6819c7a066 350 result = agz.gpsAuto();
s1210160 39:ff6819c7a066 351 //agz.set_agzAutoGPS();
s1210160 39:ff6819c7a066 352 //agz.set_agzKalmanGPS();
s1210160 39:ff6819c7a066 353
s1210160 39:ff6819c7a066 354 if(result == true) {
s1200058 3:1ac506a96fd6 355 printf("Out Area\n");
s1210160 39:ff6819c7a066 356 } else if(result == false) {
s1200058 3:1ac506a96fd6 357 printf("In Area\n");
s1210160 39:ff6819c7a066 358 }
s1210160 39:ff6819c7a066 359 for(i = 0; i < 4; i++) {
s1210160 39:ff6819c7a066 360 printf("%d: %f, %f\n", i, agz.get_basePointKalman_lati(i), agz.get_basePointKalman_longi(i));
s1210160 39:ff6819c7a066 361 }
s1210160 39:ff6819c7a066 362 printf("robot: %f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
s1210160 39:ff6819c7a066 363
s1200058 3:1ac506a96fd6 364 }
s1200058 3:1ac506a96fd6 365
s1210160 39:ff6819c7a066 366 void print_gps(int count)
s1210160 39:ff6819c7a066 367 {
s1210160 39:ff6819c7a066 368
s1200058 5:522c47c78401 369 printf("%d times:\n", count);
s1200058 5:522c47c78401 370 printf("%f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
s1210160 39:ff6819c7a066 371
s1200058 5:522c47c78401 372 }
kityann 0:daab5accfd83 373
s1200058 32:6ba2e5402f00 374
kityann 0:daab5accfd83 375 /////////////////////////////////////////
kityann 0:daab5accfd83 376 //
kityann 0:daab5accfd83 377 //Kalman Processing
kityann 0:daab5accfd83 378 //
kityann 0:daab5accfd83 379 /////////////////////////////////////////
s1210160 39:ff6819c7a066 380 void calc_Kalman()
s1210160 39:ff6819c7a066 381 {
s1210160 39:ff6819c7a066 382 //calc Kalman gain
s1210160 39:ff6819c7a066 383 Kx = s2x_prev/(s2x_prev+s2_R);
s1210160 39:ff6819c7a066 384 Ky = s2y_prev/(s2y_prev+s2_R);
s1210160 39:ff6819c7a066 385 //estimate
s1210160 39:ff6819c7a066 386 x_cur = x_prev + Kx*(zx-x_prev);
s1210160 39:ff6819c7a066 387 y_cur = y_prev + Ky*(zy-y_prev);
s1210160 39:ff6819c7a066 388 //calc sigma
s1210160 39:ff6819c7a066 389 s2x_cur = s2x_prev-Kx*s2x_prev;
s1210160 39:ff6819c7a066 390 s2y_cur = s2y_prev-Ky*s2y_prev;
kityann 0:daab5accfd83 391
kityann 0:daab5accfd83 392 }
kityann 0:daab5accfd83 393
s1210160 39:ff6819c7a066 394 void Kalman(double Latitude,double Longitude)
s1210160 39:ff6819c7a066 395 {
s1200058 32:6ba2e5402f00 396
s1200058 32:6ba2e5402f00 397 zx = Latitude;
s1200058 32:6ba2e5402f00 398 zy = Longitude;
s1200058 32:6ba2e5402f00 399
s1200058 32:6ba2e5402f00 400 calc_Kalman();
s1210160 39:ff6819c7a066 401
s1200058 32:6ba2e5402f00 402 //更新
s1200058 32:6ba2e5402f00 403 x_prev = x_cur;
s1200058 32:6ba2e5402f00 404 y_prev = y_cur;
s1200058 32:6ba2e5402f00 405 s2x_prev = s2x_cur;
s1200058 32:6ba2e5402f00 406 s2y_prev = s2y_cur;
s1210160 39:ff6819c7a066 407
s1200058 32:6ba2e5402f00 408 //agzPontKalmanとagzCovに格納する
s1200058 32:6ba2e5402f00 409 agz.set_agzPointKalman_lati(x_cur);
s1200058 32:6ba2e5402f00 410 agz.set_agzPointKalman_longi(y_cur);
s1200058 32:6ba2e5402f00 411 agz.set_agzCov(s2x_cur,s2y_cur);
s1210160 39:ff6819c7a066 412
kityann 0:daab5accfd83 413 }
kityann 0:daab5accfd83 414
kityann 0:daab5accfd83 415 /////////////////////////////////////////
kityann 0:daab5accfd83 416 //
kityann 0:daab5accfd83 417 //Main Processing
kityann 0:daab5accfd83 418 //
kityann 0:daab5accfd83 419 /////////////////////////////////////////
s1210160 39:ff6819c7a066 420 int main()
s1210160 39:ff6819c7a066 421 {
kityann 0:daab5accfd83 422 //start up time
kityann 0:daab5accfd83 423 wait(3);
s1210160 39:ff6819c7a066 424 //set pc frequency to 57600bps
s1210160 39:ff6819c7a066 425 pc.baud(PC_BAUD_RATE);
kityann 0:daab5accfd83 426 //set xbee frequency to 57600bps
s1210160 39:ff6819c7a066 427 xbee.begin(XBEE_BAUD_RATE);
s1210160 39:ff6819c7a066 428
kityann 0:daab5accfd83 429
kityann 0:daab5accfd83 430 //GPS setting
kityann 0:daab5accfd83 431 gps_Serial = new Serial(p28,p27);
s1210160 39:ff6819c7a066 432 Adafruit_GPS myGPS(gps_Serial);
kityann 0:daab5accfd83 433 Timer refresh_Timer;
s1200058 7:a9a1acc7673b 434 const int refresh_Time = 1000; //refresh time in ms
s1200058 3:1ac506a96fd6 435 Timer auto_Timer;
s1200058 3:1ac506a96fd6 436 const int auto_Time = 2000; //refresh time in ms
s1200058 5:522c47c78401 437 int count = 0;
s1210160 39:ff6819c7a066 438 Timer startGPS_Timer;
s1210160 39:ff6819c7a066 439 const int startGPS_Time = 180000;
s1210160 39:ff6819c7a066 440
s1200058 38:e2aa1165d28d 441 const int straight = 8000, turning = 12000, wait = 10000;
s1210160 39:ff6819c7a066 442
s1210160 39:ff6819c7a066 443 myGPS.begin(GPS_BAUD_RATE);
s1210160 39:ff6819c7a066 444
s1200058 14:5deb7a4f1cd4 445 Timer collect_Timer;
s1210160 39:ff6819c7a066 446 const int collect_Time = 2000; //when we collect 4 GPS point, we use
s1200058 14:5deb7a4f1cd4 447 int collect_flag = 0;
s1210160 39:ff6819c7a066 448
kityann 0:daab5accfd83 449 char SenderIDc;
kityann 0:daab5accfd83 450 //GPS Send Command
s1210160 39:ff6819c7a066 451 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:daab5accfd83 452 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:daab5accfd83 453 myGPS.sendCommand(PGCMD_ANTENNA);
s1210160 39:ff6819c7a066 454
s1200058 29:524684a1198f 455 wait_ms(2000);
s1210160 39:ff6819c7a066 456
kityann 0:daab5accfd83 457 //interrupt start
kityann 0:daab5accfd83 458 refresh_Timer.start();
s1200058 3:1ac506a96fd6 459 auto_Timer.start();
s1200058 30:7f6ebe2121d9 460 agz.Move_Timer.start();
s1200058 16:a07350b3eb64 461 collect_Timer.start();
s1210160 39:ff6819c7a066 462 startGPS_Timer.start();
s1200058 29:524684a1198f 463 printf("start\n");
s1210160 39:ff6819c7a066 464
s1210160 39:ff6819c7a066 465
kityann 0:daab5accfd83 466 while (true) {
s1210160 39:ff6819c7a066 467
kityann 0:daab5accfd83 468 //Check Xbee Buffer Available
kityann 0:daab5accfd83 469 xbee.readPacket();
s1210160 39:ff6819c7a066 470
kityann 0:daab5accfd83 471 if (xbee.getResponse().isAvailable()) {
kityann 0:daab5accfd83 472 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:daab5accfd83 473 uint8_t *buf = zbRx.getFrameData();
s1210160 39:ff6819c7a066 474
kityann 0:daab5accfd83 475 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:daab5accfd83 476 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:daab5accfd83 477 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
kityann 0:daab5accfd83 478 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
kityann 0:daab5accfd83 479 SenderIDc = buf1[5];//送信元のIDを取得する
s1200058 21:76e7f36df4a9 480 char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
s1210160 39:ff6819c7a066 481
s1210160 39:ff6819c7a066 482 //Check Command Type
s1210160 39:ff6819c7a066 483 switch(Command_type) {
s1210160 39:ff6819c7a066 484 //Get Request command
s1210160 39:ff6819c7a066 485 case MANUAL: {
s1200058 12:48ef69b4f0e4 486 Plus_Speed(buf);
s1200058 2:886fac7f4399 487 break;
s1200058 2:886fac7f4399 488 }
s1210160 39:ff6819c7a066 489 case STATUS_REQUEST: {
s1210160 39:ff6819c7a066 490 Send_Status(SenderIDc);
s1210160 39:ff6819c7a066 491 printf("%c\n", SenderIDc);
s1210160 39:ff6819c7a066 492 break;
kityann 0:daab5accfd83 493 }
s1210160 39:ff6819c7a066 494 case CHANGE_MODE: {
s1200058 12:48ef69b4f0e4 495 New_Mode(buf);
s1200058 2:886fac7f4399 496 break;
s1200058 2:886fac7f4399 497 }
s1210160 39:ff6819c7a066 498 case RECEIVE_STATUS: {
kityann 1:b2b950b916ce 499 Get_Status(SenderIDc,buf1);
kityann 1:b2b950b916ce 500 break;
kityann 1:b2b950b916ce 501 }
s1210160 39:ff6819c7a066 502 default: {
kityann 0:daab5accfd83 503 break;
kityann 0:daab5accfd83 504 }
kityann 0:daab5accfd83 505 }//endswitch
kityann 0:daab5accfd83 506 }//endifZB_RX_RESPONSE
kityann 0:daab5accfd83 507 }//endifisAvailable
s1210160 39:ff6819c7a066 508
kityann 0:daab5accfd83 509 myGPS.read();
kityann 0:daab5accfd83 510 //recive gps module
kityann 0:daab5accfd83 511 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:daab5accfd83 512 if ( myGPS.newNMEAreceived() ) {
kityann 0:daab5accfd83 513 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
s1210160 39:ff6819c7a066 514 continue;
s1210160 39:ff6819c7a066 515 } else {
s1200058 5:522c47c78401 516 count++;
s1210160 39:ff6819c7a066 517 }
kityann 0:daab5accfd83 518 }
kityann 1:b2b950b916ce 519 //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
kityann 0:daab5accfd83 520 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:daab5accfd83 521 refresh_Timer.reset();
s1200058 12:48ef69b4f0e4 522 //print_gps(count);
s1210160 39:ff6819c7a066 523 if(startGPS_Timer.read_ms() >= startGPS_Time) {
s1210160 39:ff6819c7a066 524 Get_GPS(&myGPS);
s1210160 39:ff6819c7a066 525 }
kityann 0:daab5accfd83 526 }
s1210160 39:ff6819c7a066 527
s1210160 39:ff6819c7a066 528 //get base GPS
s1210160 39:ff6819c7a066 529 if( collect_Timer.read_ms() >= collect_Time) {
s1210160 39:ff6819c7a066 530 collect_Timer.reset();
s1210160 39:ff6819c7a066 531
s1200058 15:f3d01f37f00d 532 Send_Request_Base(collect_flag);
s1210160 39:ff6819c7a066 533
s1200058 14:5deb7a4f1cd4 534 collect_flag++;
s1200058 14:5deb7a4f1cd4 535
s1210160 39:ff6819c7a066 536 if(collect_flag == 4) {
s1200058 14:5deb7a4f1cd4 537 collect_flag = 0;
s1210160 39:ff6819c7a066 538 }
s1210160 39:ff6819c7a066 539 }
s1210160 39:ff6819c7a066 540
s1210160 39:ff6819c7a066 541 if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time) {
s1210160 39:ff6819c7a066 542 auto_Timer.reset();
s1210160 39:ff6819c7a066 543 auto_Move();
s1200058 29:524684a1198f 544 }
s1210160 39:ff6819c7a066 545
s1210160 39:ff6819c7a066 546 if(agz.nowMode == AUTO_GPS_MODE) {
s1210160 39:ff6819c7a066 547 if(agz.Move_Timer.read_ms() < straight) {
s1210160 39:ff6819c7a066 548 agz.test_Auto(0); //straight
s1210160 39:ff6819c7a066 549 }
s1210160 39:ff6819c7a066 550 if(agz.Move_Timer.read_ms() > straight && agz.Move_Timer.read_ms() < wait) {
s1210160 39:ff6819c7a066 551 agz.test_Auto(1);
s1210160 39:ff6819c7a066 552 }
s1210160 39:ff6819c7a066 553 if(agz.Move_Timer.read_ms() > wait && agz.Move_Timer.read_ms() < turning) {
s1210160 39:ff6819c7a066 554 agz.test_Auto(2); //Turn Right
s1210160 39:ff6819c7a066 555 }
s1210160 39:ff6819c7a066 556 if(agz.Move_Timer.read_ms() > turning) {
s1210160 39:ff6819c7a066 557 agz.test_Auto(3);
s1210160 39:ff6819c7a066 558 wait_ms(200);
s1210160 39:ff6819c7a066 559 agz.Move_Timer.reset();
s1210160 39:ff6819c7a066 560 }
kityann 1:b2b950b916ce 561 }
kityann 0:daab5accfd83 562 }
s1210160 39:ff6819c7a066 563
kityann 0:daab5accfd83 564 }