aigamozu / AigamozuControlPackets

to Mineta

Dependencies:   VNH5019

Dependents:   agz_base_ver2 agz_base_ver2 get_GPS_data_ver1 aigamozu_program_ver2 ... more

AigamozuControlPackets.cpp@9:4f675487f06b, 2015-04-10 (annotated)

Committer:
s1200058
Date:
Fri Apr 10 12:58:42 2015 +0000
Revision:
9:4f675487f06b
Parent:
7:200ce5c1f486
Child:
10:e77c664ee5e2
yokokawa;

Who changed what in which revision?

UserRevisionLine numberNew 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--//
m5171135 7:200ce5c1f486 11 basePoint[0].x = 13956.2655;
m5171135 7:200ce5c1f486 12 basePoint[0].y = 3731.5060;
m5171135 7:200ce5c1f486 13
m5171135 7:200ce5c1f486 14 basePoint[1].x = 13956.2898;
m5171135 7:200ce5c1f486 15 basePoint[1].y = 3731.5055;
m5171135 7:200ce5c1f486 16
m5171135 7:200ce5c1f486 17 basePoint[2].x = 13956.2915;
m5171135 7:200ce5c1f486 18 basePoint[2].y = 3731.5245;
m5171135 7:200ce5c1f486 19
m5171135 7:200ce5c1f486 20 basePoint[3].x = 13956.2680;
m5171135 7:200ce5c1f486 21 basePoint[3].y = 3731.5248;
m5171135 2:3f2d4f53ceed 22 }
m5171135 2:3f2d4f53ceed 23
m5171135 5:3f51eeb5aedc 24
m5171135 2:3f2d4f53ceed 25 //////////////////////////////
m5171135 2:3f2d4f53ceed 26 // Controller/Base -> Robot //
m5171135 2:3f2d4f53ceed 27 //////////////////////////////
m5171135 4:04dadf67ecb6 28
m5171135 2:3f2d4f53ceed 29 void AigamozuControlPackets::createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR)
m5171135 2:3f2d4f53ceed 30 {
m5171135 2:3f2d4f53ceed 31 uint8_t tmp[] = {'A','G','S','M','F',fromID,'T',toID,'L',directionL,pwmL,'R',directionR,pwmR,'A','G','E'};
m5171135 2:3f2d4f53ceed 32 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
m5171135 2:3f2d4f53ceed 33 packetLength = RECEIVE_STATUS_COMMNAD_LENGTH;
m5171135 2:3f2d4f53ceed 34 }
m5171135 2:3f2d4f53ceed 35
s1200058 9:4f675487f06b 36 void AigamozuControlPackets::createRequestCommand(uint8_t fromID,uint8_t toID)
m5171135 0:2b8b56ac7a82 37 {
m5171135 2:3f2d4f53ceed 38 uint8_t tmp[] = {'A','G','S','S','F',fromID,'T',toID,'A','G','E'};
m5171135 2:3f2d4f53ceed 39 for(int i = 0; i < REQUEST_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
m5171135 2:3f2d4f53ceed 40 packetLength = REQUEST_COMMNAD_LENGTH;
m5171135 2:3f2d4f53ceed 41 }
m5171135 0:2b8b56ac7a82 42
m5171135 2:3f2d4f53ceed 43 void AigamozuControlPackets::createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,MODE mode){
m5171135 2:3f2d4f53ceed 44 uint8_t tmp[] = {'A','G','S','C','F',fromID,'T',toID,mode,'A','G','E'};
m5171135 2:3f2d4f53ceed 45 for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; ++i) packetData[i] = tmp[i];
m5171135 2:3f2d4f53ceed 46 packetLength = CHANGE_MODE_COMMAND_LENGTH;
m5171135 2:3f2d4f53ceed 47 }
m5171135 2:3f2d4f53ceed 48
m5171135 2:3f2d4f53ceed 49
m5171135 2:3f2d4f53ceed 50 //////////////////////////////
m5171135 2:3f2d4f53ceed 51 // Robot -> Controller/Base //
m5171135 2:3f2d4f53ceed 52 //////////////////////////////
m5171135 2:3f2d4f53ceed 53
m5171135 2:3f2d4f53ceed 54 void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID,uint8_t toID,long latitudeH,long latitudeL,long longitudeH,long longitudeL)
m5171135 2:3f2d4f53ceed 55 {
m5171135 0:2b8b56ac7a82 56 //useing union's split::: long hoge.a(4byte) == uint8_t hoge.b[4]
m5171135 0:2b8b56ac7a82 57 TEST_T latH,latL,lonH,lonL;
m5171135 0:2b8b56ac7a82 58
m5171135 0:2b8b56ac7a82 59 latH.a = latitudeH;
m5171135 0:2b8b56ac7a82 60 latL.a = latitudeL;
m5171135 0:2b8b56ac7a82 61 lonH.a = longitudeH;
m5171135 0:2b8b56ac7a82 62 lonL.a = longitudeL;
m5171135 0:2b8b56ac7a82 63
m5171135 1:80448565c15c 64 uint8_t tmp[] = {'A','G','S','R','F',fromID,'T',toID,'S',nowStatus,71,80,83,latH.b[0],latH.b[1],latH.b[2],latH.b[3],latL.b[0],latL.b[1],latL.b[2],latL.b[3],lonH.b[0],lonH.b[1],lonH.b[2],lonH.b[3],lonL.b[0],lonL.b[1],lonL.b[2],lonL.b[3],65,71,69};
m5171135 2:3f2d4f53ceed 65 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
m5171135 2:3f2d4f53ceed 66 packetLength = RECEIVE_STATUS_COMMNAD_LENGTH;
m5171135 0:2b8b56ac7a82 67 }
m5171135 0:2b8b56ac7a82 68
m5171135 2:3f2d4f53ceed 69
m5171135 2:3f2d4f53ceed 70 //////////////////////////////
m5171135 2:3f2d4f53ceed 71 // Using createPacket //
m5171135 2:3f2d4f53ceed 72 //////////////////////////////
m5171135 4:04dadf67ecb6 73 uint8_t AigamozuControlPackets::checkCommnadType(uint8_t* buf){
m5171135 4:04dadf67ecb6 74 return buf[14];
m5171135 4:04dadf67ecb6 75 }
m5171135 2:3f2d4f53ceed 76
m5171135 2:3f2d4f53ceed 77 uint8_t* AigamozuControlPackets::getPacketData(){
m5171135 2:3f2d4f53ceed 78 return packetData;
m5171135 2:3f2d4f53ceed 79 }
m5171135 2:3f2d4f53ceed 80
m5171135 2:3f2d4f53ceed 81 int AigamozuControlPackets::getPacketLength(){
m5171135 2:3f2d4f53ceed 82 return packetLength;
m5171135 0:2b8b56ac7a82 83 }
m5171135 0:2b8b56ac7a82 84
m5171135 5:3f51eeb5aedc 85 void AigamozuControlPackets::changeSpeed(uint8_t* buf){
m5171135 5:3f51eeb5aedc 86 if(nowMode == MANUAL_MODE){
m5171135 5:3f51eeb5aedc 87 manualCount =0;
m5171135 5:3f51eeb5aedc 88 _agzSheild.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
m5171135 5:3f51eeb5aedc 89 }
m5171135 5:3f51eeb5aedc 90 }
m5171135 4:04dadf67ecb6 91
m5171135 6:f164a716be46 92 //////////////////////////////
m5171135 6:f164a716be46 93 // each mode interrupt //
m5171135 6:f164a716be46 94 //////////////////////////////
m5171135 6:f164a716be46 95
m5171135 6:f164a716be46 96 void AigamozuControlPackets::manualMode(){
m5171135 6:f164a716be46 97 manualCount++;
m5171135 6:f164a716be46 98 if(manualCount > 10){
m5171135 6:f164a716be46 99 _agzSheild.changeSpeed(0,0,0,0);
m5171135 6:f164a716be46 100 manualCount = 0;
m5171135 6:f164a716be46 101 }
m5171135 6:f164a716be46 102 }
m5171135 6:f164a716be46 103
m5171135 6:f164a716be46 104
m5171135 5:3f51eeb5aedc 105 void AigamozuControlPackets::randomAuto(){
m5171135 2:3f2d4f53ceed 106
m5171135 5:3f51eeb5aedc 107 randomCount++;
m5171135 5:3f51eeb5aedc 108
m5171135 5:3f51eeb5aedc 109 if(randomCount < 10) _agzSheild.changeSpeed(1,128,1,128);
m5171135 5:3f51eeb5aedc 110 else if(randomCount < 11) _agzSheild.changeSpeed(2,128,2,128);
m5171135 5:3f51eeb5aedc 111 else randomCount = 0;
m5171135 5:3f51eeb5aedc 112
m5171135 5:3f51eeb5aedc 113 }
m5171135 6:f164a716be46 114
m5171135 6:f164a716be46 115 void AigamozuControlPackets::gpsAuto(){
m5171135 5:3f51eeb5aedc 116
m5171135 7:200ce5c1f486 117 printf("Check Start\n");
m5171135 7:200ce5c1f486 118 printf("%f %f\n",agzPoint.x,agzPoint.y);
m5171135 7:200ce5c1f486 119 if(AigamozuControlPackets::checkGpsHit(basePoint[0],basePoint[1],basePoint[2],agzPoint)) printf("InArea1\n");
m5171135 7:200ce5c1f486 120 if(AigamozuControlPackets::checkGpsHit(basePoint[0],basePoint[2],basePoint[3],agzPoint)) printf("InArea2\n");
m5171135 7:200ce5c1f486 121
m5171135 7:200ce5c1f486 122
m5171135 7:200ce5c1f486 123 }
m5171135 7:200ce5c1f486 124
m5171135 7:200ce5c1f486 125
m5171135 7:200ce5c1f486 126
m5171135 7:200ce5c1f486 127
m5171135 7:200ce5c1f486 128
m5171135 7:200ce5c1f486 129
m5171135 7:200ce5c1f486 130 //Update Robot Point
m5171135 7:200ce5c1f486 131 void AigamozuControlPackets::reNewPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
m5171135 7:200ce5c1f486 132 agzPoint.x = (double)latitudeH + (double)(latitudeL / 10000.0);
m5171135 7:200ce5c1f486 133 agzPoint.y = (double)longitudeH +(double)(longitudeL / 10000.0);
m5171135 7:200ce5c1f486 134
m5171135 7:200ce5c1f486 135 }
m5171135 7:200ce5c1f486 136
m5171135 7:200ce5c1f486 137
m5171135 7:200ce5c1f486 138 //Check Hit Point Area
m5171135 7:200ce5c1f486 139 bool AigamozuControlPackets::checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P){
m5171135 7:200ce5c1f486 140 vector2D AB = AigamozuControlPackets::sub_vector(B, A);
m5171135 7:200ce5c1f486 141 vector2D BP = AigamozuControlPackets::sub_vector(P, B);
m5171135 7:200ce5c1f486 142
m5171135 7:200ce5c1f486 143 vector2D BC = AigamozuControlPackets::sub_vector(C, B);
m5171135 7:200ce5c1f486 144 vector2D CP = AigamozuControlPackets::sub_vector(P, C);
m5171135 7:200ce5c1f486 145
m5171135 7:200ce5c1f486 146 vector2D CA = AigamozuControlPackets::sub_vector(A, C);
m5171135 7:200ce5c1f486 147 vector2D AP = AigamozuControlPackets::sub_vector(P, A);
m5171135 7:200ce5c1f486 148
m5171135 7:200ce5c1f486 149 double c1 = AB.x * BP.y - AB.y * BP.x;
m5171135 7:200ce5c1f486 150 double c2 = BC.x * CP.y - BC.y * CP.x;
m5171135 7:200ce5c1f486 151 double c3 = CA.x * AP.y - CA.y * AP.x;
m5171135 7:200ce5c1f486 152
m5171135 7:200ce5c1f486 153 if( ( c1 > 0 && c2 > 0 && c3 > 0 ) || ( c1 < 0 && c2 < 0 && c3 < 0 ) ) {
m5171135 7:200ce5c1f486 154 return true;
m5171135 5:3f51eeb5aedc 155 }
m5171135 6:f164a716be46 156
m5171135 7:200ce5c1f486 157 return false;
m5171135 7:200ce5c1f486 158
m5171135 7:200ce5c1f486 159 }
m5171135 7:200ce5c1f486 160
m5171135 7:200ce5c1f486 161 vector2D AigamozuControlPackets::sub_vector( const vector2D& a, const vector2D& b )
m5171135 7:200ce5c1f486 162 {
m5171135 7:200ce5c1f486 163 vector2D ret;
m5171135 7:200ce5c1f486 164 ret.x = a.x - b.x;
m5171135 7:200ce5c1f486 165 ret.y = a.y - b.y;
m5171135 7:200ce5c1f486 166 return ret;
m5171135 7:200ce5c1f486 167 }
m5171135 7:200ce5c1f486 168
m5171135 7:200ce5c1f486 169
m5171135 7:200ce5c1f486 170
m5171135 5:3f51eeb5aedc 171
m5171135 6:f164a716be46 172 //////////////////////////////
m5171135 6:f164a716be46 173 // Mode change //
m5171135 6:f164a716be46 174 //////////////////////////////
m5171135 2:3f2d4f53ceed 175 bool AigamozuControlPackets::changeMode(uint8_t *buf){
m5171135 2:3f2d4f53ceed 176
m5171135 6:f164a716be46 177 //reset
m5171135 5:3f51eeb5aedc 178 _agzSheild.changeSpeed(0,0,0,0);
m5171135 6:f164a716be46 179 eachModeInt.detach();
m5171135 5:3f51eeb5aedc 180
m5171135 6:f164a716be46 181 //Select Mode
m5171135 2:3f2d4f53ceed 182 switch(buf[19]){
m5171135 2:3f2d4f53ceed 183 case 0:
m5171135 2:3f2d4f53ceed 184 nowMode = STANDBY_MODE;
m5171135 2:3f2d4f53ceed 185 break;
m5171135 2:3f2d4f53ceed 186
m5171135 2:3f2d4f53ceed 187 case 1:
m5171135 6:f164a716be46 188 eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0);
m5171135 2:3f2d4f53ceed 189 nowMode = MANUAL_MODE;
m5171135 2:3f2d4f53ceed 190 break;
m5171135 2:3f2d4f53ceed 191
m5171135 2:3f2d4f53ceed 192 case 2:
m5171135 2:3f2d4f53ceed 193 nowMode = AUTO_MODE;
m5171135 6:f164a716be46 194 eachModeInt.attach(this,&AigamozuControlPackets::randomAuto,1.0);
m5171135 2:3f2d4f53ceed 195 break;
m5171135 4:04dadf67ecb6 196
m5171135 4:04dadf67ecb6 197 case 3:
m5171135 7:200ce5c1f486 198 eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,5.0);
m5171135 4:04dadf67ecb6 199 nowMode = AUTO_GPS_MODE;
m5171135 4:04dadf67ecb6 200 break;
m5171135 4:04dadf67ecb6 201
m5171135 6:f164a716be46 202 default:
m5171135 6:f164a716be46 203 nowMode = STANDBY_MODE;
m5171135 6:f164a716be46 204 break;
m5171135 5:3f51eeb5aedc 205
m5171135 2:3f2d4f53ceed 206 }
m5171135 2:3f2d4f53ceed 207 return false;
m5171135 2:3f2d4f53ceed 208 }
m5171135 5:3f51eeb5aedc 209
m5171135 5:3f51eeb5aedc 210
m5171135 2:3f2d4f53ceed 211