aigamozu / AigamozuControlPackets_for_auto

forward

Dependencies:   VNH5019

Fork of AigamozuControlPackets by aigamozu

AigamozuControlPackets.cpp@2:3f2d4f53ceed, 2014-06-08 (annotated)

Committer:
m5171135
Date:
Sun Jun 08 09:44:19 2014 +0000
Revision:
2:3f2d4f53ceed
Parent:
1:80448565c15c
Child:
3:282dad679fca
Child:
4:04dadf67ecb6
AiagmozuControlPackets ver1.2 Atsunori Maruyama

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