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@50:3511be172d81, 2017-10-29 (annotated)
- Committer:
- s1210160
- Date:
- Sun Oct 29 00:56:51 2017 +0000
- Revision:
- 50:3511be172d81
- Parent:
- 48:ee5a6906273e
commit;
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 | |
s1210160 | 50:3511be172d81 | 3 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 4 | // |
s1210160 | 50:3511be172d81 | 5 | // Constructor |
s1210160 | 50:3511be172d81 | 6 | // |
s1210160 | 50:3511be172d81 | 7 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 8 | AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild) |
s1210160 | 50:3511be172d81 | 9 | { |
m5171135 | 2:3f2d4f53ceed | 10 | packetData = new uint8_t[50]; |
m5171135 | 2:3f2d4f53ceed | 11 | packetLength = 0; |
s1210160 | 50:3511be172d81 | 12 | |
s1210160 | 50:3511be172d81 | 13 | agzPoint.x = 0.0; |
s1210160 | 50:3511be172d81 | 14 | agzPoint.y = 0.0; |
s1210160 | 50:3511be172d81 | 15 | |
s1210160 | 50:3511be172d81 | 16 | for(int i=0; i<BASENUMBER; i++) { |
s1210160 | 50:3511be172d81 | 17 | basePoint[i].x = 0.0; |
s1210160 | 50:3511be172d81 | 18 | basePoint[i].y = 0.0; |
s1210160 | 50:3511be172d81 | 19 | } |
s1210160 | 50:3511be172d81 | 20 | } |
s1210160 | 50:3511be172d81 | 21 | |
s1210160 | 50:3511be172d81 | 22 | AigamozuControlPackets::vector2D AigamozuControlPackets::sub_vector(vector2D A, vector2D B) |
s1210160 | 50:3511be172d81 | 23 | { |
s1210160 | 50:3511be172d81 | 24 | vector2D ret; |
s1210160 | 50:3511be172d81 | 25 | ret.x = B.x - A.x; |
s1210160 | 50:3511be172d81 | 26 | ret.y = B.y - A.y; |
s1210160 | 50:3511be172d81 | 27 | return ret; |
s1210160 | 50:3511be172d81 | 28 | } |
s1210160 | 50:3511be172d81 | 29 | |
s1210160 | 50:3511be172d81 | 30 | bool AigamozuControlPackets::checkInOut(vector2D A, vector2D B, vector2D C, vector2D P) |
s1210160 | 50:3511be172d81 | 31 | { |
s1210160 | 50:3511be172d81 | 32 | vector2D BA = sub_vector(B, A); |
s1210160 | 50:3511be172d81 | 33 | vector2D BP = sub_vector(B, P); |
s1210160 | 50:3511be172d81 | 34 | vector2D BC = sub_vector(B, C); |
s1210160 | 50:3511be172d81 | 35 | |
s1210160 | 50:3511be172d81 | 36 | double c1 = BA.x * BP.y - BA.y * BP.x; |
s1210160 | 50:3511be172d81 | 37 | double c2 = BP.x * BC.y - BP.y * BC.x; |
s1210160 | 50:3511be172d81 | 38 | |
s1210160 | 50:3511be172d81 | 39 | if((c1>0 && c2>0) || (c1<0 && c2<0)) return true; |
s1210160 | 50:3511be172d81 | 40 | else return false; |
s1210160 | 50:3511be172d81 | 41 | } |
s1210160 | 50:3511be172d81 | 42 | |
s1210160 | 50:3511be172d81 | 43 | void AigamozuControlPackets::autoMoving(void) |
s1210160 | 50:3511be172d81 | 44 | { |
s1210160 | 50:3511be172d81 | 45 | |
s1210160 | 50:3511be172d81 | 46 | if(checkInOut(basePoint[0], basePoint[1], basePoint[2], agzPoint) |
s1210160 | 50:3511be172d81 | 47 | && checkInOut(basePoint[2], basePoint[3], basePoint[0], agzPoint)) { |
s1210160 | 50:3511be172d81 | 48 | out_flag = false; |
s1210160 | 50:3511be172d81 | 49 | } else { |
s1210160 | 50:3511be172d81 | 50 | out_flag = true; |
m5171135 | 2:3f2d4f53ceed | 51 | } |
m5171135 | 2:3f2d4f53ceed | 52 | |
kityann | 17:a6fa8cc96d94 | 53 | } |
s1210160 | 50:3511be172d81 | 54 | |
s1210160 | 50:3511be172d81 | 55 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 56 | // |
s1210160 | 50:3511be172d81 | 57 | // Create Packet: Robot -> Base, |
s1210160 | 50:3511be172d81 | 58 | // Manager -> Robot/Base |
s1210160 | 50:3511be172d81 | 59 | // |
s1210160 | 50:3511be172d81 | 60 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 61 | void AigamozuControlPackets::createRequestCommand(uint8_t fromID, uint8_t toID) |
s1210160 | 50:3511be172d81 | 62 | { |
s1210160 | 50:3511be172d81 | 63 | uint8_t tmp[] = {'A','G','S','S','F',fromID,'T',toID,'A','G','E'}; |
s1210160 | 50:3511be172d81 | 64 | for(int i = 0; i < REQUEST_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; |
s1210160 | 50:3511be172d81 | 65 | packetLength = REQUEST_COMMNAD_LENGTH; |
kityann | 17:a6fa8cc96d94 | 66 | } |
s1200058 | 20:fec2d6dec897 | 67 | |
s1210160 | 50:3511be172d81 | 68 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 69 | // |
s1210160 | 50:3511be172d81 | 70 | // Create Packet: Base -> Robot/Manager, |
s1210160 | 50:3511be172d81 | 71 | // Robot -> Manager |
s1210160 | 50:3511be172d81 | 72 | // |
s1210160 | 50:3511be172d81 | 73 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 74 | void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID, uint8_t toID, int state) |
m5171135 | 0:2b8b56ac7a82 | 75 | { |
s1210160 | 50:3511be172d81 | 76 | UNION_double_char latitude_data,longitude_data; |
m5171135 | 2:3f2d4f53ceed | 77 | |
s1210160 | 50:3511be172d81 | 78 | latitude_data.double_value=agzPoint.y; |
s1210160 | 50:3511be172d81 | 79 | longitude_data.double_value=agzPoint.x; |
s1210160 | 50:3511be172d81 | 80 | |
m5171135 | 2:3f2d4f53ceed | 81 | |
s1210160 | 50:3511be172d81 | 82 | uint8_t tmp[] = {'A', 'G', 'S', 'R', 'F', fromID, 'T', toID, 'S', state,'G', 'P', 'S', |
s1210160 | 50:3511be172d81 | 83 | latitude_data.char_value[0], latitude_data.char_value[1], latitude_data.char_value[2], latitude_data.char_value[3], |
s1210160 | 50:3511be172d81 | 84 | latitude_data.char_value[4], latitude_data.char_value[5], latitude_data.char_value[6], latitude_data.char_value[7], |
s1210160 | 50:3511be172d81 | 85 | longitude_data.char_value[0], longitude_data.char_value[1], longitude_data.char_value[2], longitude_data.char_value[3], |
s1210160 | 50:3511be172d81 | 86 | longitude_data.char_value[4], longitude_data.char_value[5], longitude_data.char_value[6], longitude_data.char_value[7], |
s1210160 | 50:3511be172d81 | 87 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, |
s1210160 | 50:3511be172d81 | 88 | 'A', 'G', 'E' |
s1210160 | 50:3511be172d81 | 89 | }; |
m5171135 | 2:3f2d4f53ceed | 90 | for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; |
m5171135 | 2:3f2d4f53ceed | 91 | packetLength = RECEIVE_STATUS_COMMNAD_LENGTH; |
m5171135 | 0:2b8b56ac7a82 | 92 | } |
m5171135 | 0:2b8b56ac7a82 | 93 | |
s1210160 | 50:3511be172d81 | 94 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 95 | // |
s1210160 | 50:3511be172d81 | 96 | // Update data |
s1210160 | 50:3511be172d81 | 97 | // |
s1210160 | 50:3511be172d81 | 98 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 99 | void AigamozuControlPackets::reNewRobotPoint(long latitudeH, long latitudeL, long longitudeH, long longitudeL) |
s1210160 | 50:3511be172d81 | 100 | { |
s1210160 | 50:3511be172d81 | 101 | agzPoint.x = (double)longitudeH + (double)(longitudeL / 10000.0/60.0); |
s1210160 | 50:3511be172d81 | 102 | agzPoint.y = (double)latitudeH +(double)(latitudeL / 10000.0/60.0); |
s1200058 | 37:5654e31ad452 | 103 | } |
m5171135 | 7:200ce5c1f486 | 104 | |
s1210160 | 50:3511be172d81 | 105 | void AigamozuControlPackets::reNewBasePoint(int id, uint8_t *latitude, uint8_t *longitude) |
m5171135 | 7:200ce5c1f486 | 106 | { |
s1210160 | 50:3511be172d81 | 107 | UNION_double_char latitude_data,longitude_data; |
s1210160 | 50:3511be172d81 | 108 | for(int i = 0; i < 8; i++) { |
s1210160 | 50:3511be172d81 | 109 | latitude_data.char_value[i]=latitude[i]; |
s1210160 | 50:3511be172d81 | 110 | } |
s1210160 | 50:3511be172d81 | 111 | for(int i = 0; i < 8; i++) { |
s1210160 | 50:3511be172d81 | 112 | longitude_data.char_value[i]=longitude[i]; |
s1210160 | 50:3511be172d81 | 113 | } |
s1210160 | 50:3511be172d81 | 114 | basePoint[id].x = longitude_data.double_value; |
s1210160 | 50:3511be172d81 | 115 | basePoint[id].y = latitude_data.double_value; |
s1210160 | 50:3511be172d81 | 116 | } |
m5171135 | 7:200ce5c1f486 | 117 | |
s1210160 | 50:3511be172d81 | 118 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 119 | // |
s1210160 | 50:3511be172d81 | 120 | // Manual Mode |
s1210160 | 50:3511be172d81 | 121 | // |
s1210160 | 50:3511be172d81 | 122 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 123 | void AigamozuControlPackets::manualMode() {} |
m5171135 | 5:3f51eeb5aedc | 124 | |
s1210160 | 50:3511be172d81 | 125 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 126 | // |
s1210160 | 50:3511be172d81 | 127 | // Change Mode |
s1210160 | 50:3511be172d81 | 128 | // |
s1210160 | 50:3511be172d81 | 129 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 130 | void AigamozuControlPackets::changeMode(uint8_t *buf) |
s1210160 | 50:3511be172d81 | 131 | { |
s1210160 | 50:3511be172d81 | 132 | |
m5171135 | 6:f164a716be46 | 133 | //reset |
m5171135 | 5:3f51eeb5aedc | 134 | _agzSheild.changeSpeed(0,0,0,0); |
m5171135 | 6:f164a716be46 | 135 | eachModeInt.detach(); |
m5171135 | 5:3f51eeb5aedc | 136 | |
m5171135 | 6:f164a716be46 | 137 | //Select Mode |
s1210160 | 50:3511be172d81 | 138 | switch(buf[8]) { |
m5171135 | 2:3f2d4f53ceed | 139 | case 0: |
m5171135 | 2:3f2d4f53ceed | 140 | nowMode = STANDBY_MODE; |
s1210160 | 50:3511be172d81 | 141 | break; |
s1210160 | 50:3511be172d81 | 142 | |
m5171135 | 2:3f2d4f53ceed | 143 | case 1: |
m5171135 | 6:f164a716be46 | 144 | eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0); |
m5171135 | 2:3f2d4f53ceed | 145 | nowMode = MANUAL_MODE; |
s1210160 | 50:3511be172d81 | 146 | break; |
s1210160 | 50:3511be172d81 | 147 | |
m5171135 | 2:3f2d4f53ceed | 148 | case 2: |
s1210160 | 50:3511be172d81 | 149 | nowMode = AUTO_MODE; |
s1210160 | 50:3511be172d81 | 150 | //Move_Timer.reset(); |
s1210160 | 50:3511be172d81 | 151 | break; |
s1210160 | 50:3511be172d81 | 152 | |
m5171135 | 6:f164a716be46 | 153 | default: |
m5171135 | 6:f164a716be46 | 154 | nowMode = STANDBY_MODE; |
s1210160 | 50:3511be172d81 | 155 | break; |
s1210160 | 50:3511be172d81 | 156 | |
s1210160 | 50:3511be172d81 | 157 | } |
s1210160 | 50:3511be172d81 | 158 | } |
s1210160 | 50:3511be172d81 | 159 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 160 | // |
s1210160 | 50:3511be172d81 | 161 | // Set |
s1210160 | 50:3511be172d81 | 162 | // |
s1210160 | 50:3511be172d81 | 163 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 164 | void AigamozuControlPackets::set_nowStatus(STATUS s) |
s1210160 | 50:3511be172d81 | 165 | { |
s1210160 | 50:3511be172d81 | 166 | nowStatus = s; |
m5171135 | 2:3f2d4f53ceed | 167 | } |
m5171135 | 5:3f51eeb5aedc | 168 | |
m5171135 | 5:3f51eeb5aedc | 169 | |
s1210160 | 50:3511be172d81 | 170 | |
s1210160 | 50:3511be172d81 | 171 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 172 | // |
s1210160 | 50:3511be172d81 | 173 | // Get |
s1210160 | 50:3511be172d81 | 174 | // |
s1210160 | 50:3511be172d81 | 175 | ///////////////////////////////////////// |
s1210160 | 50:3511be172d81 | 176 | MODE AigamozuControlPackets::get_nowMode(void) |
s1210160 | 50:3511be172d81 | 177 | { |
s1210160 | 50:3511be172d81 | 178 | return nowMode; |
s1210160 | 50:3511be172d81 | 179 | } |
s1210160 | 50:3511be172d81 | 180 | uint8_t* AigamozuControlPackets::get_packetData(void) |
s1210160 | 50:3511be172d81 | 181 | { |
s1210160 | 50:3511be172d81 | 182 | return packetData; |
s1210160 | 50:3511be172d81 | 183 | } |
s1210160 | 50:3511be172d81 | 184 | int AigamozuControlPackets::get_packetLength(void) |
s1210160 | 50:3511be172d81 | 185 | { |
s1210160 | 50:3511be172d81 | 186 | return packetLength; |
s1210160 | 50:3511be172d81 | 187 | } |
s1210160 | 50:3511be172d81 | 188 | bool AigamozuControlPackets::get_out_flag(void) |
s1210160 | 50:3511be172d81 | 189 | { |
s1210160 | 50:3511be172d81 | 190 | return out_flag; |
s1210160 | 50:3511be172d81 | 191 | } |
s1210160 | 50:3511be172d81 | 192 | double AigamozuControlPackets::get_agzPoint_latitude(void) |
s1210160 | 50:3511be172d81 | 193 | { |
s1210160 | 50:3511be172d81 | 194 | return agzPoint.y; |
s1210160 | 50:3511be172d81 | 195 | } |
s1210160 | 50:3511be172d81 | 196 | double AigamozuControlPackets::get_agzPoint_longitude(void) |
s1210160 | 50:3511be172d81 | 197 | { |
s1210160 | 50:3511be172d81 | 198 | return agzPoint.x; |
s1210160 | 50:3511be172d81 | 199 | } |
s1210160 | 50:3511be172d81 | 200 | double AigamozuControlPackets::get_basePoint_latitude(int i) |
s1210160 | 50:3511be172d81 | 201 | { |
s1210160 | 50:3511be172d81 | 202 | return basePoint[i].y; |
s1210160 | 50:3511be172d81 | 203 | } |
s1210160 | 50:3511be172d81 | 204 | double AigamozuControlPackets::get_basePoint_longitude(int i) |
s1210160 | 50:3511be172d81 | 205 | { |
s1210160 | 50:3511be172d81 | 206 | return basePoint[i].x; |
s1210160 | 50:3511be172d81 | 207 | } |