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