Mami Yokokawa / AigamozuControlPackets

test

Dependencies:   VNH5019

Fork of AigamozuControlPackets by Haruna Nakazawa

AigamozuControlPackets.cpp@3:6c7084121bd6, 2015-05-12 (annotated)

Committer:
s1200058
Date:
Tue May 12 12:17:37 2015 +0000
Revision:
3:6c7084121bd6
Parent:
2:30e4ea7de224 
test

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