aigamozu / AigamozuControlPackets

to Mineta

Dependencies:   VNH5019

Dependents:   agz_base_ver2 agz_base_ver2 get_GPS_data_ver1 aigamozu_program_ver2 ... more

AigamozuControlPackets.cpp@26:195c054a0148, 2015-05-15 (annotated)

Committer:
s1200058
Date:
Fri May 15 09:23:45 2015 +0000
Revision:
26:195c054a0148
Parent:
25:119fd6c9ae80
Child:
30:abc6740124d4
2015/05/15

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