![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2015/05/29
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed
Fork of Aigamozu_Robot_ver4_1 by
Revision 36:a11060f5199e, committed 2015-05-29
- Comitter:
- s1210160
- Date:
- Fri May 29 13:07:46 2015 +0000
- Parent:
- 35:3094c84a024b
- Commit message:
- 2015/05/29
Changed in this revision
AigamozuControlPackets.lib | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/AigamozuControlPackets.lib Wed May 27 12:10:20 2015 +0000 +++ b/AigamozuControlPackets.lib Fri May 29 13:07:46 2015 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/teams/aigamozu/code/AigamozuControlPackets/#b33b8b2a92b0 +http://mbed.org/teams/aigamozu/code/AigamozuControlPackets/#93ba352b0395
--- a/main.cpp Wed May 27 12:10:20 2015 +0000 +++ b/main.cpp Fri May 29 13:07:46 2015 +0000 @@ -41,6 +41,8 @@ //Kalmanフィルターの計算式を変更した。 //set_kalmanを追加した。 // +//2015/05/29 +//auto_Move関数の実装とAigamozuControlPackets内にcontrol_Mortor関数の実装 /**********************************************/ #include "mbed.h" @@ -59,7 +61,7 @@ //Base ID: 'a' ~ 'a' //manager ID: '0'(数字のゼロ) // -const char MyID = 'D'; +const char MyID = 'A'; //************ID Number***************** ///////////////////////////////////////// @@ -316,28 +318,64 @@ // //InAreaかOutAreaの判定 //Kalmanを通した値を出力(Baseと自分) +//2015/05/29 +//外側判定と処理の実装 +//内側判定:シーケンス動作 +//外側判定:10秒間バック、3秒間旋回を行い、その後シーケンス動作へ ///////////////////////////////////////// void auto_Move(){ - bool result; - int i; + bool result; // 毎回の内外判定の結果を格納 + bool out_flag = false; // 外側処理の実行フラグ + const int sequenceTime[4] = {30000, 31000, 34000, 34200}; + const int outSequenceTime[4] = {10000, 11000, 14000, 14200}; result = agz.gpsAuto(); //agz.set_agzAutoGPS(); //agz.set_agzKalmanGPS(); - if(result == true){ - printf("Out Area\n"); - } -else if(result == false){ - printf("In Area\n"); -} -for(i = 0; i < 4; i++){ - printf("%d: %f, %f\n", i, agz.get_basePointKalman_lati(i), agz.get_basePointKalman_longi(i)); - } - printf("robot: %f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi()); + if(out_flag == false && result == true){ + out_flag = true; + agz.Out_Timer.reset(); + } + if(out_flag == false){ + if(agz.Move_Timer.read_ms() < sequenceTime[0]){ + agz.control_Motor(0); //straight + } + if(agz.Move_Timer.read_ms() > sequenceTime[0] && agz.Move_Timer.read_ms() < sequenceTime[1]){ + agz.control_Motor(1); + } + if(agz.Move_Timer.read_ms() > sequenceTime[1] && agz.Move_Timer.read_ms() < sequenceTime[2]){ + agz.control_Motor(2); //Turn Right + } + if(agz.Move_Timer.read_ms() > sequenceTime[2] && agz.Move_Timer.read_ms() < sequenceTime[3]){ + agz.control_Motor(1); + } + if(agz.Move_Timer.read_ms() > sequenceTime[3]){ + agz.Move_Timer.reset(); + } + } + if(out_flag == true){ + if(agz.Out_Timer.read_ms() < outSequenceTime[0]){ + agz.control_Motor(3); //back + } + if(agz.Out_Timer.read_ms() > outSequenceTime[0] && agz.Out_Timer.read_ms() < outSequenceTime[1]){ + agz.control_Motor(1); + } + if(agz.Out_Timer.read_ms() > outSequenceTime[1] && agz.Out_Timer.read_ms() < outSequenceTime[2]){ + agz.control_Motor(2); //Turn Right + } + if(agz.Out_Timer.read_ms() > outSequenceTime[2] && agz.Out_Timer.read_ms() < outSequenceTime[3]){ + agz.control_Motor(1); + } + if(agz.Out_Timer.read_ms() > outSequenceTime[3]){ + agz.Out_Timer.reset(); + agz.Move_Timer.reset(); + out_flag = false; + } + } } void print_gps(int count){ @@ -406,10 +444,10 @@ Timer refresh_Timer; const int refresh_Time = 1000; //refresh time in ms Timer auto_Timer; - const int auto_Time = 2000; //refresh time in ms + const int auto_Time = 100; //refresh time in ms int count = 0; - const int straight = 30000, turning = 34000, wait = 31000; + myGPS.begin(GPS_BAUD_RATE); @@ -429,6 +467,7 @@ refresh_Timer.start(); auto_Timer.start(); agz.Move_Timer.start(); + agz.Out_Timer.start(); collect_Timer.start(); printf("start\n"); @@ -513,22 +552,6 @@ 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