20150512 Nakazawa
Fork of AigamozuControlPackets by
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:fa82a475ab95
test
Who changed what in which revision?
User | Revision | Line number | New 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 |