aigamozu / AigamozuControlPackets_2016

2016_05_19ver Auto mode 10sec forward, 2sec stop, 2sec right turn Please change test_mode's right turn ppm

Dependencies:   VNH5019 AigamozuControlPackets_2016

Dependents:   Aigamozu_Robot_2016_ver1 GPSLOG_program AigamozuControlPackets_2016

Fork of AigamozuControlPackets by aigamozu

AigamozuControlPackets.cpp@49:c69fcf98e6b7, 2016-08-09 (annotated)

Committer:
kityann
Date:
Tue Aug 09 13:04:06 2016 +0000
Revision:
49:c69fcf98e6b7
Parent:
46:246051b4f24d 
comitto

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