![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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
Diff: main.cpp
- Revision:
- 41:d6350b57f02a
- Parent:
- 40:aa600a5ba02c
- Child:
- 42:5cdd92577dfe
diff -r aa600a5ba02c -r d6350b57f02a main.cpp --- a/main.cpp Wed May 25 11:26:20 2016 +0000 +++ b/main.cpp Wed May 25 14:34:05 2016 +0000 @@ -60,8 +60,7 @@ //Base ID: 'a' ~ 'a' //manager ID: '0'(数字のゼロ) // -const char MyID = 'C'; -const int other_robot = 7; +const char MyID = 'A'; //************ID Number***************** ///////////////////////////////////////// @@ -202,12 +201,13 @@ if(flag < COUNTER_MAX) { flag++; } - if(flag == KALMAN_START_TIME && agz.nowMode == AUTO_GPS_MODE) { + if(/*flag == KALMAN_START_TIME */agz.nowMode == AUTO_GPS_MODE && agz.auto_count == 0) { x_prev = agz.get_agzPoint_lati(); y_prev = agz.get_agzPoint_longi(); + agz.auto_count++; } - if(flag >= KALMAN_START_TIME+1 && agz.nowMode == AUTO_GPS_MODE) { + if(/*flag >= KALMAN_START_TIME+1 && agz.nowMode == AUTO_GPS_MODE && */agz.auto_count > 0) { if(abs(x_prev - agz.get_agzPoint_lati()) < 0.001 && abs(y_prev - agz.get_agzPoint_longi()) < 0.001) { Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi()); change = 1; @@ -235,14 +235,6 @@ agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(), agz.get_agzCov_lati(),agz.get_agzCov_longi()); - agz.createReceiveStatusCommand(MyID, other_robot, agz.nowMode, - agz.get_agzPoint_lati(),agz.get_agzPoint_longi(), - agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(), - agz.get_agzCov_lati(),agz.get_agzCov_longi()); - //Select Destination - ZBTxRequest tx64request(robot_Address[other_robot],agz.packetData,agz.getPacketLength()); - //Send -> Base - xbee.send(tx64request); } else agz.nowStatus = GPS_UNAVAIL; } @@ -285,7 +277,7 @@ int id = SenderIDc - 'a'; printf("Get Base data %d\n", id+1); agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]); - //agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]); + agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]); //debug printf("latitude:%f,longitude:%f\n", @@ -435,10 +427,8 @@ Timer auto_Timer; const int auto_Time = 2000; //refresh time in ms int count = 0; - Timer startGPS_Timer; - const int startGPS_Time = 180000; - const int straight = 8000, turning = 12000, wait = 10000; +// const int straight = 8000, turning = 12000, wait = 10000; myGPS.begin(GPS_BAUD_RATE); @@ -459,9 +449,9 @@ auto_Timer.start(); agz.Move_Timer.start(); collect_Timer.start(); - startGPS_Timer.start(); printf("start\n"); + agz.auto_count = 0; while (true) { @@ -520,9 +510,7 @@ if (refresh_Timer.read_ms() >= refresh_Time) { refresh_Timer.reset(); //print_gps(count); - if(startGPS_Timer.read_ms() >= startGPS_Time) { - Get_GPS(&myGPS); - } + Get_GPS(&myGPS); } //get base GPS @@ -542,23 +530,7 @@ auto_Timer.reset(); auto_Move(); } - - if(agz.nowMode == AUTO_GPS_MODE) { - if(agz.Move_Timer.read_ms() < straight) { - agz.test_Auto(0); //straight - } - if(agz.Move_Timer.read_ms() > straight && agz.Move_Timer.read_ms() < wait) { - agz.test_Auto(1); - } - if(agz.Move_Timer.read_ms() > wait && agz.Move_Timer.read_ms() < turning) { - agz.test_Auto(2); //Turn Right - } - if(agz.Move_Timer.read_ms() > turning) { - agz.test_Auto(3); - wait_ms(200); - agz.Move_Timer.reset(); - } - } + } } \ No newline at end of file