20150512 Nakazawa
Fork of AigamozuControlPackets by
AigamozuControlPackets.cpp@6:f164a716be46, 2014-06-09 (annotated)
- Committer:
- m5171135
- Date:
- Mon Jun 09 00:16:38 2014 +0000
- Revision:
- 6:f164a716be46
- Parent:
- 5:3f51eeb5aedc
- Child:
- 7:200ce5c1f486
ver1.3;
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 | |
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 | 2:3f2d4f53ceed | 10 | } |
m5171135 | 2:3f2d4f53ceed | 11 | |
m5171135 | 5:3f51eeb5aedc | 12 | |
m5171135 | 2:3f2d4f53ceed | 13 | ////////////////////////////// |
m5171135 | 2:3f2d4f53ceed | 14 | // Controller/Base -> Robot // |
m5171135 | 2:3f2d4f53ceed | 15 | ////////////////////////////// |
m5171135 | 4:04dadf67ecb6 | 16 | |
m5171135 | 2:3f2d4f53ceed | 17 | void AigamozuControlPackets::createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR) |
m5171135 | 2:3f2d4f53ceed | 18 | { |
m5171135 | 2:3f2d4f53ceed | 19 | uint8_t tmp[] = {'A','G','S','M','F',fromID,'T',toID,'L',directionL,pwmL,'R',directionR,pwmR,'A','G','E'}; |
m5171135 | 2:3f2d4f53ceed | 20 | for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; |
m5171135 | 2:3f2d4f53ceed | 21 | packetLength = RECEIVE_STATUS_COMMNAD_LENGTH; |
m5171135 | 2:3f2d4f53ceed | 22 | } |
m5171135 | 2:3f2d4f53ceed | 23 | |
m5171135 | 2:3f2d4f53ceed | 24 | void AigamozuControlPackets::createRequestCommand(uint8_t fromID,uint8_t toID,uint8_t) |
m5171135 | 0:2b8b56ac7a82 | 25 | { |
m5171135 | 2:3f2d4f53ceed | 26 | uint8_t tmp[] = {'A','G','S','S','F',fromID,'T',toID,'A','G','E'}; |
m5171135 | 2:3f2d4f53ceed | 27 | for(int i = 0; i < REQUEST_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; |
m5171135 | 2:3f2d4f53ceed | 28 | packetLength = REQUEST_COMMNAD_LENGTH; |
m5171135 | 2:3f2d4f53ceed | 29 | } |
m5171135 | 0:2b8b56ac7a82 | 30 | |
m5171135 | 2:3f2d4f53ceed | 31 | void AigamozuControlPackets::createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,MODE mode){ |
m5171135 | 2:3f2d4f53ceed | 32 | uint8_t tmp[] = {'A','G','S','C','F',fromID,'T',toID,mode,'A','G','E'}; |
m5171135 | 2:3f2d4f53ceed | 33 | for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; ++i) packetData[i] = tmp[i]; |
m5171135 | 2:3f2d4f53ceed | 34 | packetLength = CHANGE_MODE_COMMAND_LENGTH; |
m5171135 | 2:3f2d4f53ceed | 35 | } |
m5171135 | 2:3f2d4f53ceed | 36 | |
m5171135 | 2:3f2d4f53ceed | 37 | |
m5171135 | 2:3f2d4f53ceed | 38 | ////////////////////////////// |
m5171135 | 2:3f2d4f53ceed | 39 | // Robot -> Controller/Base // |
m5171135 | 2:3f2d4f53ceed | 40 | ////////////////////////////// |
m5171135 | 2:3f2d4f53ceed | 41 | |
m5171135 | 2:3f2d4f53ceed | 42 | void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID,uint8_t toID,long latitudeH,long latitudeL,long longitudeH,long longitudeL) |
m5171135 | 2:3f2d4f53ceed | 43 | { |
m5171135 | 0:2b8b56ac7a82 | 44 | //useing union's split::: long hoge.a(4byte) == uint8_t hoge.b[4] |
m5171135 | 0:2b8b56ac7a82 | 45 | TEST_T latH,latL,lonH,lonL; |
m5171135 | 0:2b8b56ac7a82 | 46 | |
m5171135 | 0:2b8b56ac7a82 | 47 | latH.a = latitudeH; |
m5171135 | 0:2b8b56ac7a82 | 48 | latL.a = latitudeL; |
m5171135 | 0:2b8b56ac7a82 | 49 | lonH.a = longitudeH; |
m5171135 | 0:2b8b56ac7a82 | 50 | lonL.a = longitudeL; |
m5171135 | 0:2b8b56ac7a82 | 51 | |
m5171135 | 1:80448565c15c | 52 | 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 | 53 | for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; |
m5171135 | 2:3f2d4f53ceed | 54 | packetLength = RECEIVE_STATUS_COMMNAD_LENGTH; |
m5171135 | 0:2b8b56ac7a82 | 55 | } |
m5171135 | 0:2b8b56ac7a82 | 56 | |
m5171135 | 2:3f2d4f53ceed | 57 | |
m5171135 | 2:3f2d4f53ceed | 58 | ////////////////////////////// |
m5171135 | 2:3f2d4f53ceed | 59 | // Using createPacket // |
m5171135 | 2:3f2d4f53ceed | 60 | ////////////////////////////// |
m5171135 | 4:04dadf67ecb6 | 61 | uint8_t AigamozuControlPackets::checkCommnadType(uint8_t* buf){ |
m5171135 | 4:04dadf67ecb6 | 62 | return buf[14]; |
m5171135 | 4:04dadf67ecb6 | 63 | } |
m5171135 | 2:3f2d4f53ceed | 64 | |
m5171135 | 2:3f2d4f53ceed | 65 | uint8_t* AigamozuControlPackets::getPacketData(){ |
m5171135 | 2:3f2d4f53ceed | 66 | return packetData; |
m5171135 | 2:3f2d4f53ceed | 67 | } |
m5171135 | 2:3f2d4f53ceed | 68 | |
m5171135 | 2:3f2d4f53ceed | 69 | int AigamozuControlPackets::getPacketLength(){ |
m5171135 | 2:3f2d4f53ceed | 70 | return packetLength; |
m5171135 | 0:2b8b56ac7a82 | 71 | } |
m5171135 | 0:2b8b56ac7a82 | 72 | |
m5171135 | 5:3f51eeb5aedc | 73 | void AigamozuControlPackets::changeSpeed(uint8_t* buf){ |
m5171135 | 5:3f51eeb5aedc | 74 | if(nowMode == MANUAL_MODE){ |
m5171135 | 5:3f51eeb5aedc | 75 | manualCount =0; |
m5171135 | 5:3f51eeb5aedc | 76 | _agzSheild.changeSpeed(buf[20],buf[21],buf[23],buf[24]); |
m5171135 | 5:3f51eeb5aedc | 77 | } |
m5171135 | 5:3f51eeb5aedc | 78 | } |
m5171135 | 4:04dadf67ecb6 | 79 | |
m5171135 | 6:f164a716be46 | 80 | ////////////////////////////// |
m5171135 | 6:f164a716be46 | 81 | // each mode interrupt // |
m5171135 | 6:f164a716be46 | 82 | ////////////////////////////// |
m5171135 | 6:f164a716be46 | 83 | |
m5171135 | 6:f164a716be46 | 84 | void AigamozuControlPackets::manualMode(){ |
m5171135 | 6:f164a716be46 | 85 | manualCount++; |
m5171135 | 6:f164a716be46 | 86 | if(manualCount > 10){ |
m5171135 | 6:f164a716be46 | 87 | _agzSheild.changeSpeed(0,0,0,0); |
m5171135 | 6:f164a716be46 | 88 | manualCount = 0; |
m5171135 | 6:f164a716be46 | 89 | } |
m5171135 | 6:f164a716be46 | 90 | } |
m5171135 | 6:f164a716be46 | 91 | |
m5171135 | 6:f164a716be46 | 92 | |
m5171135 | 5:3f51eeb5aedc | 93 | void AigamozuControlPackets::randomAuto(){ |
m5171135 | 2:3f2d4f53ceed | 94 | |
m5171135 | 5:3f51eeb5aedc | 95 | randomCount++; |
m5171135 | 5:3f51eeb5aedc | 96 | |
m5171135 | 5:3f51eeb5aedc | 97 | if(randomCount < 10) _agzSheild.changeSpeed(1,128,1,128); |
m5171135 | 5:3f51eeb5aedc | 98 | else if(randomCount < 11) _agzSheild.changeSpeed(2,128,2,128); |
m5171135 | 5:3f51eeb5aedc | 99 | else randomCount = 0; |
m5171135 | 5:3f51eeb5aedc | 100 | |
m5171135 | 5:3f51eeb5aedc | 101 | } |
m5171135 | 6:f164a716be46 | 102 | |
m5171135 | 6:f164a716be46 | 103 | void AigamozuControlPackets::gpsAuto(){ |
m5171135 | 5:3f51eeb5aedc | 104 | |
m5171135 | 5:3f51eeb5aedc | 105 | } |
m5171135 | 6:f164a716be46 | 106 | |
m5171135 | 5:3f51eeb5aedc | 107 | |
m5171135 | 6:f164a716be46 | 108 | ////////////////////////////// |
m5171135 | 6:f164a716be46 | 109 | // Mode change // |
m5171135 | 6:f164a716be46 | 110 | ////////////////////////////// |
m5171135 | 2:3f2d4f53ceed | 111 | bool AigamozuControlPackets::changeMode(uint8_t *buf){ |
m5171135 | 2:3f2d4f53ceed | 112 | |
m5171135 | 6:f164a716be46 | 113 | //reset |
m5171135 | 5:3f51eeb5aedc | 114 | _agzSheild.changeSpeed(0,0,0,0); |
m5171135 | 6:f164a716be46 | 115 | eachModeInt.detach(); |
m5171135 | 5:3f51eeb5aedc | 116 | |
m5171135 | 6:f164a716be46 | 117 | //Select Mode |
m5171135 | 2:3f2d4f53ceed | 118 | switch(buf[19]){ |
m5171135 | 2:3f2d4f53ceed | 119 | case 0: |
m5171135 | 2:3f2d4f53ceed | 120 | nowMode = STANDBY_MODE; |
m5171135 | 2:3f2d4f53ceed | 121 | break; |
m5171135 | 2:3f2d4f53ceed | 122 | |
m5171135 | 2:3f2d4f53ceed | 123 | case 1: |
m5171135 | 6:f164a716be46 | 124 | eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0); |
m5171135 | 2:3f2d4f53ceed | 125 | nowMode = MANUAL_MODE; |
m5171135 | 2:3f2d4f53ceed | 126 | break; |
m5171135 | 2:3f2d4f53ceed | 127 | |
m5171135 | 2:3f2d4f53ceed | 128 | case 2: |
m5171135 | 2:3f2d4f53ceed | 129 | nowMode = AUTO_MODE; |
m5171135 | 6:f164a716be46 | 130 | eachModeInt.attach(this,&AigamozuControlPackets::randomAuto,1.0); |
m5171135 | 2:3f2d4f53ceed | 131 | break; |
m5171135 | 4:04dadf67ecb6 | 132 | |
m5171135 | 4:04dadf67ecb6 | 133 | case 3: |
m5171135 | 6:f164a716be46 | 134 | eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,2.0); |
m5171135 | 4:04dadf67ecb6 | 135 | nowMode = AUTO_GPS_MODE; |
m5171135 | 4:04dadf67ecb6 | 136 | break; |
m5171135 | 4:04dadf67ecb6 | 137 | |
m5171135 | 6:f164a716be46 | 138 | default: |
m5171135 | 6:f164a716be46 | 139 | nowMode = STANDBY_MODE; |
m5171135 | 6:f164a716be46 | 140 | break; |
m5171135 | 5:3f51eeb5aedc | 141 | |
m5171135 | 2:3f2d4f53ceed | 142 | } |
m5171135 | 2:3f2d4f53ceed | 143 | return false; |
m5171135 | 2:3f2d4f53ceed | 144 | } |
m5171135 | 5:3f51eeb5aedc | 145 | |
m5171135 | 5:3f51eeb5aedc | 146 | |
m5171135 | 2:3f2d4f53ceed | 147 |