aigamozu / AigamozuControlPackets

to Mineta

Dependencies:   VNH5019

Dependents:   agz_base_ver2 agz_base_ver2 get_GPS_data_ver1 aigamozu_program_ver2 ... more

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?

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 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