20150512 Nakazawa

Dependencies:   VNH5019

Committer:
s1200058
Date:
Tue May 12 10:40:07 2015 +0000
Revision:
0:806e9a434e81
Child:
1:6d7c854c927d
2015/05/12

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1200058 0:806e9a434e81 1 #include "AigamozuControlPackets.h"
s1200058 0:806e9a434e81 2 #include "VNH5019.h"
s1200058 0:806e9a434e81 3
s1200058 0:806e9a434e81 4 //////////////////////////////
s1200058 0:806e9a434e81 5 // Init //
s1200058 0:806e9a434e81 6 //////////////////////////////
s1200058 0:806e9a434e81 7 AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild){
s1200058 0:806e9a434e81 8 packetData = new uint8_t[50];
s1200058 0:806e9a434e81 9 packetLength = 0;
s1200058 0:806e9a434e81 10 //--init base point--//
s1200058 0:806e9a434e81 11 //basePoint[0].x = 13956.2655;
s1200058 0:806e9a434e81 12 //basePoint[0].y = 3731.5060;
s1200058 0:806e9a434e81 13 basePoint[0].x = 100.2655;
s1200058 0:806e9a434e81 14 basePoint[0].y = 30.5060;
s1200058 0:806e9a434e81 15
s1200058 0:806e9a434e81 16 //basePoint[1].x = 13956.2898;
s1200058 0:806e9a434e81 17 //basePoint[1].y = 3731.5055;
s1200058 0:806e9a434e81 18 basePoint[1].x = 200.2898;
s1200058 0:806e9a434e81 19 basePoint[1].y = 30.5055;
s1200058 0:806e9a434e81 20
s1200058 0:806e9a434e81 21 //basePoint[2].x = 13956.2915;
s1200058 0:806e9a434e81 22 //basePoint[2].y = 3731.5245;
s1200058 0:806e9a434e81 23 basePoint[2].x = 200.2915;
s1200058 0:806e9a434e81 24 basePoint[2].y = 40.5245;
s1200058 0:806e9a434e81 25
s1200058 0:806e9a434e81 26 //basePoint[3].x = 13956.2680;
s1200058 0:806e9a434e81 27 //basePoint[3].y = 3731.5248;
s1200058 0:806e9a434e81 28 basePoint[3].x = 100.2680;
s1200058 0:806e9a434e81 29 basePoint[3].y = 40.5248;
s1200058 0:806e9a434e81 30
s1200058 0:806e9a434e81 31 agzPoint.x=0;agzPoint.y=0;
s1200058 0:806e9a434e81 32 agzPointKalman.x=0;agzPointKalman.y=0;
s1200058 0:806e9a434e81 33 agzCov.x=0;agzCov.y=0;
s1200058 0:806e9a434e81 34 }
s1200058 0:806e9a434e81 35
s1200058 0:806e9a434e81 36 //////////////////////////////
s1200058 0:806e9a434e81 37 // get関数
s1200058 0:806e9a434e81 38 //////////////////////////////
s1200058 0:806e9a434e81 39 double AigamozuControlPackets::get_agzPoint_lati(){
s1200058 0:806e9a434e81 40 return agzPoint.x;
s1200058 0:806e9a434e81 41 }
s1200058 0:806e9a434e81 42 double AigamozuControlPackets::get_agzPoint_longi(){
s1200058 0:806e9a434e81 43 return agzPoint.y;
s1200058 0:806e9a434e81 44 }
s1200058 0:806e9a434e81 45 double AigamozuControlPackets::get_agzPointKalman_lati(){
s1200058 0:806e9a434e81 46 return agzPointKalman.x;
s1200058 0:806e9a434e81 47 }
s1200058 0:806e9a434e81 48 double AigamozuControlPackets::get_agzPointKalman_longi(){
s1200058 0:806e9a434e81 49 return agzPointKalman.y;
s1200058 0:806e9a434e81 50 }
s1200058 0:806e9a434e81 51 double AigamozuControlPackets::get_agzCov_lati(){
s1200058 0:806e9a434e81 52 return agzCov.x;
s1200058 0:806e9a434e81 53 }
s1200058 0:806e9a434e81 54 double AigamozuControlPackets::get_agzCov_longi(){
s1200058 0:806e9a434e81 55 return agzCov.y;
s1200058 0:806e9a434e81 56 }
s1200058 0:806e9a434e81 57
s1200058 0:806e9a434e81 58 double AigamozuControlPackets::get_basePointKalman_lati(int i){
s1200058 0:806e9a434e81 59 return basePointKalman[i].x;
s1200058 0:806e9a434e81 60 }
s1200058 0:806e9a434e81 61 double AigamozuControlPackets::get_basePointKalman_longi(int i){
s1200058 0:806e9a434e81 62 return basePointKalman[i].y;
s1200058 0:806e9a434e81 63 }
s1200058 0:806e9a434e81 64 //////////////////////////////
s1200058 0:806e9a434e81 65 // set関数
s1200058 0:806e9a434e81 66 //////////////////////////////
s1200058 0:806e9a434e81 67 void AigamozuControlPackets::set_agzCov(double cov_lati,double cov_longi){
s1200058 0:806e9a434e81 68 agzCov.x = cov_lati;
s1200058 0:806e9a434e81 69 agzCov.y = cov_longi;
s1200058 0:806e9a434e81 70 }
s1200058 0:806e9a434e81 71
s1200058 0:806e9a434e81 72
s1200058 0:806e9a434e81 73 void AigamozuControlPackets::set_agzAutoGPS(){
s1200058 0:806e9a434e81 74 agzPoint.x += 0.001;
s1200058 0:806e9a434e81 75 //agzPoint.y += 0.001;
s1200058 0:806e9a434e81 76 }
s1200058 0:806e9a434e81 77
s1200058 0:806e9a434e81 78 void AigamozuControlPackets::set_agzKalmanGPS(){
s1200058 0:806e9a434e81 79 agzPointKalman.x += 0.001;
s1200058 0:806e9a434e81 80 //agzPointKalman.y += 0.001;
s1200058 0:806e9a434e81 81
s1200058 0:806e9a434e81 82 }
s1200058 0:806e9a434e81 83
s1200058 0:806e9a434e81 84 //////////////////////////////
s1200058 0:806e9a434e81 85 // Controller/Base -> Robot //
s1200058 0:806e9a434e81 86 //////////////////////////////
s1200058 0:806e9a434e81 87
s1200058 0:806e9a434e81 88 void AigamozuControlPackets::createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR)
s1200058 0:806e9a434e81 89 {
s1200058 0:806e9a434e81 90 uint8_t tmp[] = {'A','G','S','M','F',fromID,'T',toID,'L',directionL,pwmL,'R',directionR,pwmR,'A','G','E'};
s1200058 0:806e9a434e81 91 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
s1200058 0:806e9a434e81 92 packetLength = RECEIVE_STATUS_COMMNAD_LENGTH;
s1200058 0:806e9a434e81 93 }
s1200058 0:806e9a434e81 94
s1200058 0:806e9a434e81 95 void AigamozuControlPackets::createRequestCommand(uint8_t fromID,uint8_t toID)
s1200058 0:806e9a434e81 96 {
s1200058 0:806e9a434e81 97 uint8_t tmp[] = {'A','G','S','S','F',fromID,'T',toID,'A','G','E'};
s1200058 0:806e9a434e81 98 for(int i = 0; i < REQUEST_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
s1200058 0:806e9a434e81 99 packetLength = REQUEST_COMMNAD_LENGTH;
s1200058 0:806e9a434e81 100 }
s1200058 0:806e9a434e81 101
s1200058 0:806e9a434e81 102 void AigamozuControlPackets::createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,MODE mode){
s1200058 0:806e9a434e81 103 uint8_t tmp[] = {'A','G','S','C','F',fromID,'T',toID,mode,'A','G','E'};
s1200058 0:806e9a434e81 104 for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; ++i) packetData[i] = tmp[i];
s1200058 0:806e9a434e81 105 packetLength = CHANGE_MODE_COMMAND_LENGTH;
s1200058 0:806e9a434e81 106 }
s1200058 0:806e9a434e81 107
s1200058 0:806e9a434e81 108
s1200058 0:806e9a434e81 109 //////////////////////////////
s1200058 0:806e9a434e81 110 // Robot -> Controller/Base //
s1200058 0:806e9a434e81 111 //////////////////////////////
s1200058 0:806e9a434e81 112
s1200058 0:806e9a434e81 113 void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID,uint8_t toID,double latitude,double longitude,double latitudeKalman,double longitudeKalman,double covarLati,double covarLongi)
s1200058 0:806e9a434e81 114 {
s1200058 0:806e9a434e81 115 UNION_double_char latitude_data,longitude_data, latitudeKalman_data, longitudeKalman_data;
s1200058 0:806e9a434e81 116 UNION_double_char covarLati_data,covarLongi_data;
s1200058 0:806e9a434e81 117
s1200058 0:806e9a434e81 118 latitude_data.double_value=latitude;
s1200058 0:806e9a434e81 119 longitude_data.double_value=longitude;
s1200058 0:806e9a434e81 120 longitude_data.double_value=latitudeKalman;
s1200058 0:806e9a434e81 121 longitudeKalman_data.double_value=longitudeKalman;
s1200058 0:806e9a434e81 122 covarLati_data.double_value=covarLati;
s1200058 0:806e9a434e81 123 covarLongi_data.double_value=covarLongi;
s1200058 0:806e9a434e81 124
s1200058 0:806e9a434e81 125 uint8_t tmp[] = {'A','G','S','R','F',fromID,'T',toID,'S',nowMode,71,80,83,
s1200058 0:806e9a434e81 126 latitude_data.char_value[0],latitude_data.char_value[1],latitude_data.char_value[2],latitude_data.char_value[3],latitude_data.char_value[4],latitude_data.char_value[5],latitude_data.char_value[6],latitude_data.char_value[7],
s1200058 0:806e9a434e81 127 longitude_data.char_value[0],longitude_data.char_value[1],longitude_data.char_value[2],longitude_data.char_value[3],longitude_data.char_value[4],longitude_data.char_value[5],longitude_data.char_value[6],longitude_data.char_value[7],
s1200058 0:806e9a434e81 128 latitudeKalman_data.char_value[0],latitudeKalman_data.char_value[1],latitudeKalman_data.char_value[2],latitudeKalman_data.char_value[3],latitudeKalman_data.char_value[4],latitudeKalman_data.char_value[5],latitudeKalman_data.char_value[6],latitudeKalman_data.char_value[7],
s1200058 0:806e9a434e81 129 longitudeKalman_data.char_value[0],longitudeKalman_data.char_value[1],longitudeKalman_data.char_value[2],longitudeKalman_data.char_value[3],longitudeKalman_data.char_value[4],longitudeKalman_data.char_value[5],longitudeKalman_data.char_value[6],longitudeKalman_data.char_value[7],
s1200058 0:806e9a434e81 130 covarLati_data.char_value[0],covarLati_data.char_value[1],covarLati_data.char_value[2],covarLati_data.char_value[3],covarLati_data.char_value[4],covarLati_data.char_value[5],covarLati_data.char_value[6],covarLati_data.char_value[7],
s1200058 0:806e9a434e81 131 covarLongi_data.char_value[0],covarLongi_data.char_value[1],covarLongi_data.char_value[2],covarLongi_data.char_value[3],covarLongi_data.char_value[4],covarLongi_data.char_value[5],covarLongi_data.char_value[6],covarLongi_data.char_value[7],
s1200058 0:806e9a434e81 132 65,71,69};
s1200058 0:806e9a434e81 133 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
s1200058 0:806e9a434e81 134 packetLength = RECEIVE_STATUS_COMMNAD_LENGTH;
s1200058 0:806e9a434e81 135 }
s1200058 0:806e9a434e81 136
s1200058 0:806e9a434e81 137
s1200058 0:806e9a434e81 138
s1200058 0:806e9a434e81 139 //////////////////////////////
s1200058 0:806e9a434e81 140 // Using createPacket //
s1200058 0:806e9a434e81 141 //////////////////////////////
s1200058 0:806e9a434e81 142 uint8_t AigamozuControlPackets::checkCommnadType(uint8_t* buf){
s1200058 0:806e9a434e81 143 return buf[14];
s1200058 0:806e9a434e81 144 }
s1200058 0:806e9a434e81 145
s1200058 0:806e9a434e81 146 uint8_t* AigamozuControlPackets::getPacketData(){
s1200058 0:806e9a434e81 147 return packetData;
s1200058 0:806e9a434e81 148 }
s1200058 0:806e9a434e81 149
s1200058 0:806e9a434e81 150 int AigamozuControlPackets::getPacketLength(){
s1200058 0:806e9a434e81 151 return packetLength;
s1200058 0:806e9a434e81 152 }
s1200058 0:806e9a434e81 153
s1200058 0:806e9a434e81 154 void AigamozuControlPackets::changeSpeed(uint8_t* buf){
s1200058 0:806e9a434e81 155 if(nowMode == MANUAL_MODE){
s1200058 0:806e9a434e81 156 manualCount =0;
s1200058 0:806e9a434e81 157 _agzSheild.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
s1200058 0:806e9a434e81 158 }
s1200058 0:806e9a434e81 159 }
s1200058 0:806e9a434e81 160
s1200058 0:806e9a434e81 161 //////////////////////////////
s1200058 0:806e9a434e81 162 // each mode interrupt //
s1200058 0:806e9a434e81 163 //////////////////////////////
s1200058 0:806e9a434e81 164
s1200058 0:806e9a434e81 165 void AigamozuControlPackets::manualMode(){
s1200058 0:806e9a434e81 166 manualCount++;
s1200058 0:806e9a434e81 167 if(manualCount > 10){
s1200058 0:806e9a434e81 168 _agzSheild.changeSpeed(0,0,0,0);
s1200058 0:806e9a434e81 169 manualCount = 0;
s1200058 0:806e9a434e81 170 }
s1200058 0:806e9a434e81 171 }
s1200058 0:806e9a434e81 172
s1200058 0:806e9a434e81 173
s1200058 0:806e9a434e81 174 void AigamozuControlPackets::randomAuto(){
s1200058 0:806e9a434e81 175
s1200058 0:806e9a434e81 176 randomCount++;
s1200058 0:806e9a434e81 177
s1200058 0:806e9a434e81 178 if(randomCount < 10) _agzSheild.changeSpeed(1,128,1,128);
s1200058 0:806e9a434e81 179 else if(randomCount < 11) _agzSheild.changeSpeed(2,128,2,128);
s1200058 0:806e9a434e81 180 else randomCount = 0;
s1200058 0:806e9a434e81 181
s1200058 0:806e9a434e81 182 }
s1200058 0:806e9a434e81 183
s1200058 0:806e9a434e81 184 bool AigamozuControlPackets::gpsAuto(){
s1200058 0:806e9a434e81 185
s1200058 0:806e9a434e81 186 /*
s1200058 0:806e9a434e81 187 _agzSheild.changeSpeed(2,128,2,128):for moving robot
s1200058 0:806e9a434e81 188 */
s1200058 0:806e9a434e81 189
s1200058 0:806e9a434e81 190 Timer Automove_Timer;
s1200058 0:806e9a434e81 191
s1200058 0:806e9a434e81 192 bool out_flag = true;
s1200058 0:806e9a434e81 193 static bool out_count_flag = false;
s1200058 0:806e9a434e81 194
s1200058 0:806e9a434e81 195 if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)){
s1200058 0:806e9a434e81 196 out_flag = false;
s1200058 0:806e9a434e81 197 out_count_flag = false;
s1200058 0:806e9a434e81 198 }else if(AigamozuControlPackets::checkGpsHit(basePointKalman[2],basePointKalman[3],basePointKalman[0],agzPointKalman)){
s1200058 0:806e9a434e81 199 out_flag = false;
s1200058 0:806e9a434e81 200 out_count_flag = false;
s1200058 0:806e9a434e81 201 }else{//if robot is out
s1200058 0:806e9a434e81 202 out_flag = true;
s1200058 0:806e9a434e81 203 }
s1200058 0:806e9a434e81 204
s1200058 0:806e9a434e81 205 //if robot is out:
s1200058 0:806e9a434e81 206 if(out_flag == true || out_count_flag == false){
s1200058 0:806e9a434e81 207 Automove_Timer.reset();
s1200058 0:806e9a434e81 208 out_count_flag = true;
s1200058 0:806e9a434e81 209 }
s1200058 0:806e9a434e81 210 if(out_flag){
s1200058 0:806e9a434e81 211 if(Automove_Timer.read_ms() < 5000){
s1200058 0:806e9a434e81 212 _agzSheild.changeSpeed(2,64,2,64);//back
s1200058 0:806e9a434e81 213 }else if(Automove_Timer.read_ms() < 6000){
s1200058 0:806e9a434e81 214 _agzSheild.changeSpeed(1,64,2,64);//straight
s1200058 0:806e9a434e81 215 }else if(Automove_Timer.read_ms() >= 6000){
s1200058 0:806e9a434e81 216 out_count_flag=false;
s1200058 0:806e9a434e81 217 out_flag=false;
s1200058 0:806e9a434e81 218 }
s1200058 0:806e9a434e81 219 }else{
s1200058 0:806e9a434e81 220 //if robot is inner
s1200058 0:806e9a434e81 221 _agzSheild.changeSpeed(1,64,1,64);//straight
s1200058 0:806e9a434e81 222 }
s1200058 0:806e9a434e81 223
s1200058 0:806e9a434e81 224 return out_flag;
s1200058 0:806e9a434e81 225
s1200058 0:806e9a434e81 226 }
s1200058 0:806e9a434e81 227
s1200058 0:806e9a434e81 228
s1200058 0:806e9a434e81 229
s1200058 0:806e9a434e81 230
s1200058 0:806e9a434e81 231
s1200058 0:806e9a434e81 232 //Update Robot Point
s1200058 0:806e9a434e81 233 void AigamozuControlPackets::reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
s1200058 0:806e9a434e81 234 agzPoint.x = (double)latitudeH + (double)(latitudeL / 1000000.0/60.0);
s1200058 0:806e9a434e81 235 agzPoint.y = (double)longitudeH +(double)(longitudeL / 1000000.0/60.0);
s1200058 0:806e9a434e81 236
s1200058 0:806e9a434e81 237 }
s1200058 0:806e9a434e81 238
s1200058 0:806e9a434e81 239 //Updata Base Point
s1200058 0:806e9a434e81 240 void AigamozuControlPackets::reNewBasePoint(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL){
s1200058 0:806e9a434e81 241 basePoint[id].x = (double)latitudeH + (double)(latitudeL / 1000000.0);
s1200058 0:806e9a434e81 242 basePoint[id].y = (double)longitudeH +(double)(longitudeL / 1000000.0);
s1200058 0:806e9a434e81 243 }
s1200058 0:806e9a434e81 244
s1200058 0:806e9a434e81 245 //Update Robot Point
s1200058 0:806e9a434e81 246 void AigamozuControlPackets::reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
s1200058 0:806e9a434e81 247 agzPointKalman.x = (double)latitudeH + (double)(latitudeL / 1000000.0/60.0);
s1200058 0:806e9a434e81 248 agzPointKalman.y = (double)longitudeH +(double)(longitudeL / 1000000.0/60.0);
s1200058 0:806e9a434e81 249
s1200058 0:806e9a434e81 250 }
s1200058 0:806e9a434e81 251
s1200058 0:806e9a434e81 252 //Updata Base Point
s1200058 0:806e9a434e81 253 void AigamozuControlPackets::reNewBasePointKalman(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL){
s1200058 0:806e9a434e81 254 basePointKalman[id].x = (double)latitudeH + (double)(latitudeL / 1000000.0);
s1200058 0:806e9a434e81 255 basePointKalman[id].y = (double)longitudeH +(double)(longitudeL / 1000000.0);
s1200058 0:806e9a434e81 256 }
s1200058 0:806e9a434e81 257
s1200058 0:806e9a434e81 258
s1200058 0:806e9a434e81 259 //Check Hit Point Area
s1200058 0:806e9a434e81 260 bool AigamozuControlPackets::checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P){
s1200058 0:806e9a434e81 261 vector2D AB = AigamozuControlPackets::sub_vector(B, A);
s1200058 0:806e9a434e81 262 vector2D BP = AigamozuControlPackets::sub_vector(P, B);
s1200058 0:806e9a434e81 263
s1200058 0:806e9a434e81 264 vector2D BC = AigamozuControlPackets::sub_vector(C, B);
s1200058 0:806e9a434e81 265 vector2D CP = AigamozuControlPackets::sub_vector(P, C);
s1200058 0:806e9a434e81 266
s1200058 0:806e9a434e81 267 vector2D CA = AigamozuControlPackets::sub_vector(A, C);
s1200058 0:806e9a434e81 268 vector2D AP = AigamozuControlPackets::sub_vector(P, A);
s1200058 0:806e9a434e81 269
s1200058 0:806e9a434e81 270 double c1 = AB.x * BP.y - AB.y * BP.x;
s1200058 0:806e9a434e81 271 double c2 = BC.x * CP.y - BC.y * CP.x;
s1200058 0:806e9a434e81 272 double c3 = CA.x * AP.y - CA.y * AP.x;
s1200058 0:806e9a434e81 273
s1200058 0:806e9a434e81 274 if( ( c1 > 0 && c2 > 0 && c3 > 0 ) || ( c1 < 0 && c2 < 0 && c3 < 0 ) ) {
s1200058 0:806e9a434e81 275 return true;
s1200058 0:806e9a434e81 276 }
s1200058 0:806e9a434e81 277
s1200058 0:806e9a434e81 278 return false;
s1200058 0:806e9a434e81 279
s1200058 0:806e9a434e81 280 }
s1200058 0:806e9a434e81 281
s1200058 0:806e9a434e81 282 vector2D AigamozuControlPackets::sub_vector( const vector2D& a, const vector2D& b )
s1200058 0:806e9a434e81 283 {
s1200058 0:806e9a434e81 284 vector2D ret;
s1200058 0:806e9a434e81 285 ret.x = a.x - b.x;
s1200058 0:806e9a434e81 286 ret.y = a.y - b.y;
s1200058 0:806e9a434e81 287 return ret;
s1200058 0:806e9a434e81 288 }
s1200058 0:806e9a434e81 289
s1200058 0:806e9a434e81 290
s1200058 0:806e9a434e81 291
s1200058 0:806e9a434e81 292
s1200058 0:806e9a434e81 293 //////////////////////////////
s1200058 0:806e9a434e81 294 // Mode change //
s1200058 0:806e9a434e81 295 //////////////////////////////
s1200058 0:806e9a434e81 296 bool AigamozuControlPackets::changeMode(uint8_t *buf){
s1200058 0:806e9a434e81 297
s1200058 0:806e9a434e81 298 //reset
s1200058 0:806e9a434e81 299 _agzSheild.changeSpeed(0,0,0,0);
s1200058 0:806e9a434e81 300 eachModeInt.detach();
s1200058 0:806e9a434e81 301
s1200058 0:806e9a434e81 302 //Select Mode
s1200058 0:806e9a434e81 303 switch(buf[19]){
s1200058 0:806e9a434e81 304 case 0:
s1200058 0:806e9a434e81 305 nowMode = STANDBY_MODE;
s1200058 0:806e9a434e81 306 break;
s1200058 0:806e9a434e81 307
s1200058 0:806e9a434e81 308 case 1:
s1200058 0:806e9a434e81 309 eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0);
s1200058 0:806e9a434e81 310 nowMode = MANUAL_MODE;
s1200058 0:806e9a434e81 311 break;
s1200058 0:806e9a434e81 312
s1200058 0:806e9a434e81 313 case 2:
s1200058 0:806e9a434e81 314 //nowMode = AUTO_MODE;
s1200058 0:806e9a434e81 315 //eachModeInt.attach(this,&AigamozuControlPackets::randomAuto,1.0);
s1200058 0:806e9a434e81 316 // eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,2.0);
s1200058 0:806e9a434e81 317 nowMode = AUTO_GPS_MODE;
s1200058 0:806e9a434e81 318 break;
s1200058 0:806e9a434e81 319
s1200058 0:806e9a434e81 320 case 3:
s1200058 0:806e9a434e81 321 //eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,0.2);
s1200058 0:806e9a434e81 322 nowMode = AUTO_GPS_MODE;
s1200058 0:806e9a434e81 323 break;
s1200058 0:806e9a434e81 324
s1200058 0:806e9a434e81 325 default:
s1200058 0:806e9a434e81 326 nowMode = STANDBY_MODE;
s1200058 0:806e9a434e81 327 break;
s1200058 0:806e9a434e81 328
s1200058 0:806e9a434e81 329 }
s1200058 0:806e9a434e81 330 return false;
s1200058 0:806e9a434e81 331 }
s1200058 0:806e9a434e81 332
s1200058 0:806e9a434e81 333
s1200058 0:806e9a434e81 334