aigamozu / AigamozuControlPackets_for_auto

forward

Dependencies:   VNH5019

Fork of AigamozuControlPackets by aigamozu

AigamozuControlPackets.cpp@16:0e815cca2cc7, 2015-05-11 (annotated)

Committer:
s1200058
Date:
Mon May 11 13:09:32 2015 +0000
Revision:
16:0e815cca2cc7
Parent:
15:ac839aff80bc 
yokokawa

Who changed what in which revision?

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