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