aigamozu / AigamozuControlPackets

to Mineta

Dependencies:   VNH5019

Dependents:   agz_base_ver2 agz_base_ver2 get_GPS_data_ver1 aigamozu_program_ver2 ... more

AigamozuControlPackets.cpp@17:a6fa8cc96d94, 2015-05-12 (annotated)

Committer:
kityann
Date:
Tue May 12 01:02:47 2015 +0000
Revision:
17:a6fa8cc96d94
Parent:
14:e441331aa4a1
Child:
18:01882120e6cf
test

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