to Mineta
Dependencies: VNH5019
Dependents: agz_base_ver2 agz_base_ver2 get_GPS_data_ver1 aigamozu_program_ver2 ... more
AigamozuControlPackets.cpp@3:282dad679fca, 2014-06-18 (annotated)
- Committer:
- kityann
- Date:
- Wed Jun 18 13:48:02 2014 +0000
- Revision:
- 3:282dad679fca
- Parent:
- 2:3f2d4f53ceed
aigamo get data;
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 | |
m5171135 | 0:2b8b56ac7a82 | 3 | |
m5171135 | 2:3f2d4f53ceed | 4 | AigamozuControlPackets::AigamozuControlPackets(){ |
m5171135 | 2:3f2d4f53ceed | 5 | packetData = new uint8_t[50]; |
m5171135 | 2:3f2d4f53ceed | 6 | packetLength = 0; |
m5171135 | 2:3f2d4f53ceed | 7 | |
m5171135 | 2:3f2d4f53ceed | 8 | } |
m5171135 | 2:3f2d4f53ceed | 9 | |
m5171135 | 1:80448565c15c | 10 | |
m5171135 | 1:80448565c15c | 11 | |
m5171135 | 2:3f2d4f53ceed | 12 | ////////////////////////////// |
m5171135 | 2:3f2d4f53ceed | 13 | // Controller/Base -> Robot // |
m5171135 | 2:3f2d4f53ceed | 14 | ////////////////////////////// |
m5171135 | 2:3f2d4f53ceed | 15 | void AigamozuControlPackets::createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR) |
m5171135 | 2:3f2d4f53ceed | 16 | { |
m5171135 | 2:3f2d4f53ceed | 17 | uint8_t tmp[] = {'A','G','S','M','F',fromID,'T',toID,'L',directionL,pwmL,'R',directionR,pwmR,'A','G','E'}; |
m5171135 | 2:3f2d4f53ceed | 18 | for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; |
m5171135 | 2:3f2d4f53ceed | 19 | packetLength = RECEIVE_STATUS_COMMNAD_LENGTH; |
m5171135 | 2:3f2d4f53ceed | 20 | } |
m5171135 | 2:3f2d4f53ceed | 21 | |
kityann | 3:282dad679fca | 22 | void AigamozuControlPackets::createRequestCommand(uint8_t fromID,uint8_t toID) |
m5171135 | 0:2b8b56ac7a82 | 23 | { |
m5171135 | 2:3f2d4f53ceed | 24 | uint8_t tmp[] = {'A','G','S','S','F',fromID,'T',toID,'A','G','E'}; |
m5171135 | 2:3f2d4f53ceed | 25 | for(int i = 0; i < REQUEST_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; |
m5171135 | 2:3f2d4f53ceed | 26 | packetLength = REQUEST_COMMNAD_LENGTH; |
m5171135 | 2:3f2d4f53ceed | 27 | } |
m5171135 | 0:2b8b56ac7a82 | 28 | |
kityann | 3:282dad679fca | 29 | void AigamozuControlPackets::createChangeModeCommand(uint8_t fromID,uint8_t toID,MODE mode){ |
m5171135 | 2:3f2d4f53ceed | 30 | uint8_t tmp[] = {'A','G','S','C','F',fromID,'T',toID,mode,'A','G','E'}; |
m5171135 | 2:3f2d4f53ceed | 31 | for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; ++i) packetData[i] = tmp[i]; |
m5171135 | 2:3f2d4f53ceed | 32 | packetLength = CHANGE_MODE_COMMAND_LENGTH; |
m5171135 | 2:3f2d4f53ceed | 33 | } |
m5171135 | 2:3f2d4f53ceed | 34 | |
m5171135 | 2:3f2d4f53ceed | 35 | |
m5171135 | 2:3f2d4f53ceed | 36 | ////////////////////////////// |
m5171135 | 2:3f2d4f53ceed | 37 | // Robot -> Controller/Base // |
m5171135 | 2:3f2d4f53ceed | 38 | ////////////////////////////// |
m5171135 | 2:3f2d4f53ceed | 39 | |
m5171135 | 2:3f2d4f53ceed | 40 | void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID,uint8_t toID,long latitudeH,long latitudeL,long longitudeH,long longitudeL) |
m5171135 | 2:3f2d4f53ceed | 41 | { |
m5171135 | 0:2b8b56ac7a82 | 42 | //useing union's split::: long hoge.a(4byte) == uint8_t hoge.b[4] |
m5171135 | 0:2b8b56ac7a82 | 43 | TEST_T latH,latL,lonH,lonL; |
m5171135 | 0:2b8b56ac7a82 | 44 | |
m5171135 | 0:2b8b56ac7a82 | 45 | latH.a = latitudeH; |
m5171135 | 0:2b8b56ac7a82 | 46 | latL.a = latitudeL; |
m5171135 | 0:2b8b56ac7a82 | 47 | lonH.a = longitudeH; |
m5171135 | 0:2b8b56ac7a82 | 48 | lonL.a = longitudeL; |
m5171135 | 0:2b8b56ac7a82 | 49 | |
m5171135 | 1:80448565c15c | 50 | 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 | 51 | for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; |
m5171135 | 2:3f2d4f53ceed | 52 | packetLength = RECEIVE_STATUS_COMMNAD_LENGTH; |
m5171135 | 0:2b8b56ac7a82 | 53 | } |
m5171135 | 0:2b8b56ac7a82 | 54 | |
m5171135 | 2:3f2d4f53ceed | 55 | |
m5171135 | 2:3f2d4f53ceed | 56 | ////////////////////////////// |
m5171135 | 2:3f2d4f53ceed | 57 | // Using createPacket // |
m5171135 | 2:3f2d4f53ceed | 58 | ////////////////////////////// |
m5171135 | 2:3f2d4f53ceed | 59 | |
m5171135 | 2:3f2d4f53ceed | 60 | uint8_t* AigamozuControlPackets::getPacketData(){ |
m5171135 | 2:3f2d4f53ceed | 61 | return packetData; |
m5171135 | 2:3f2d4f53ceed | 62 | } |
m5171135 | 2:3f2d4f53ceed | 63 | |
m5171135 | 2:3f2d4f53ceed | 64 | int AigamozuControlPackets::getPacketLength(){ |
m5171135 | 2:3f2d4f53ceed | 65 | return packetLength; |
m5171135 | 0:2b8b56ac7a82 | 66 | } |
m5171135 | 0:2b8b56ac7a82 | 67 | |
m5171135 | 0:2b8b56ac7a82 | 68 | uint8_t AigamozuControlPackets::checkCommnadType(uint8_t* buf){ |
m5171135 | 0:2b8b56ac7a82 | 69 | return buf[14]; |
m5171135 | 0:2b8b56ac7a82 | 70 | } |
m5171135 | 2:3f2d4f53ceed | 71 | |
m5171135 | 2:3f2d4f53ceed | 72 | bool AigamozuControlPackets::changeMode(uint8_t *buf){ |
m5171135 | 2:3f2d4f53ceed | 73 | |
m5171135 | 2:3f2d4f53ceed | 74 | switch(buf[19]){ |
m5171135 | 2:3f2d4f53ceed | 75 | |
m5171135 | 2:3f2d4f53ceed | 76 | case 0: |
m5171135 | 2:3f2d4f53ceed | 77 | nowMode = STANDBY_MODE; |
m5171135 | 2:3f2d4f53ceed | 78 | break; |
m5171135 | 2:3f2d4f53ceed | 79 | |
m5171135 | 2:3f2d4f53ceed | 80 | case 1: |
m5171135 | 2:3f2d4f53ceed | 81 | nowMode = MANUAL_MODE; |
m5171135 | 2:3f2d4f53ceed | 82 | break; |
m5171135 | 2:3f2d4f53ceed | 83 | |
m5171135 | 2:3f2d4f53ceed | 84 | case 2: |
m5171135 | 2:3f2d4f53ceed | 85 | nowMode = AUTO_MODE; |
m5171135 | 2:3f2d4f53ceed | 86 | break; |
m5171135 | 2:3f2d4f53ceed | 87 | } |
m5171135 | 2:3f2d4f53ceed | 88 | return false; |
m5171135 | 2:3f2d4f53ceed | 89 | } |
m5171135 | 2:3f2d4f53ceed | 90 |