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