aigamozu / AigamozuControlPackets_2016

2016_05_19ver Auto mode 10sec forward, 2sec stop, 2sec right turn Please change test_mode's right turn ppm

Dependencies:   VNH5019 AigamozuControlPackets_2016

Dependents:   Aigamozu_Robot_2016_ver1 GPSLOG_program AigamozuControlPackets_2016

Fork of AigamozuControlPackets by aigamozu

AigamozuControlPackets.cpp@5:3f51eeb5aedc, 2014-06-08 (annotated)

Committer:
m5171135
Date:
Sun Jun 08 15:09:02 2014 +0000
Revision:
5:3f51eeb5aedc
Parent:
4:04dadf67ecb6
Child:
6:f164a716be46
ver 2,0

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 5:3f51eeb5aedc 5 AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild){
m5171135 2:3f2d4f53ceed 6 packetData = new uint8_t[50];
m5171135 2:3f2d4f53ceed 7 packetLength = 0;
m5171135 2:3f2d4f53ceed 8 }
m5171135 2:3f2d4f53ceed 9
m5171135 5:3f51eeb5aedc 10
m5171135 2:3f2d4f53ceed 11 //////////////////////////////
m5171135 2:3f2d4f53ceed 12 // Controller/Base -> Robot //
m5171135 2:3f2d4f53ceed 13 //////////////////////////////
m5171135 4:04dadf67ecb6 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
m5171135 2:3f2d4f53ceed 22 void AigamozuControlPackets::createRequestCommand(uint8_t fromID,uint8_t toID,uint8_t)
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
m5171135 2:3f2d4f53ceed 29 void AigamozuControlPackets::createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,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 4:04dadf67ecb6 59 uint8_t AigamozuControlPackets::checkCommnadType(uint8_t* buf){
m5171135 4:04dadf67ecb6 60 return buf[14];
m5171135 4:04dadf67ecb6 61 }
m5171135 2:3f2d4f53ceed 62
m5171135 2:3f2d4f53ceed 63 uint8_t* AigamozuControlPackets::getPacketData(){
m5171135 2:3f2d4f53ceed 64 return packetData;
m5171135 2:3f2d4f53ceed 65 }
m5171135 2:3f2d4f53ceed 66
m5171135 2:3f2d4f53ceed 67 int AigamozuControlPackets::getPacketLength(){
m5171135 2:3f2d4f53ceed 68 return packetLength;
m5171135 0:2b8b56ac7a82 69 }
m5171135 0:2b8b56ac7a82 70
m5171135 5:3f51eeb5aedc 71 void AigamozuControlPackets::changeSpeed(uint8_t* buf){
m5171135 5:3f51eeb5aedc 72 if(nowMode == MANUAL_MODE){
m5171135 5:3f51eeb5aedc 73 manualCount =0;
m5171135 5:3f51eeb5aedc 74 _agzSheild.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
m5171135 5:3f51eeb5aedc 75 }
m5171135 5:3f51eeb5aedc 76 }
m5171135 4:04dadf67ecb6 77
m5171135 5:3f51eeb5aedc 78 void AigamozuControlPackets::randomAuto(){
m5171135 2:3f2d4f53ceed 79
m5171135 5:3f51eeb5aedc 80 randomCount++;
m5171135 5:3f51eeb5aedc 81
m5171135 5:3f51eeb5aedc 82 if(randomCount < 10) _agzSheild.changeSpeed(1,128,1,128);
m5171135 5:3f51eeb5aedc 83 else if(randomCount < 11) _agzSheild.changeSpeed(2,128,2,128);
m5171135 5:3f51eeb5aedc 84 else randomCount = 0;
m5171135 5:3f51eeb5aedc 85
m5171135 5:3f51eeb5aedc 86 }
m5171135 5:3f51eeb5aedc 87
m5171135 5:3f51eeb5aedc 88 void AigamozuControlPackets::manualMode(){
m5171135 5:3f51eeb5aedc 89 manualCount++;
m5171135 5:3f51eeb5aedc 90 if(manualCount > 10){
m5171135 5:3f51eeb5aedc 91 _agzSheild.changeSpeed(0,0,0,0);
m5171135 5:3f51eeb5aedc 92 manualCount = 0;
m5171135 5:3f51eeb5aedc 93 }
m5171135 5:3f51eeb5aedc 94 }
m5171135 5:3f51eeb5aedc 95
m5171135 2:3f2d4f53ceed 96 bool AigamozuControlPackets::changeMode(uint8_t *buf){
m5171135 2:3f2d4f53ceed 97
m5171135 5:3f51eeb5aedc 98 _agzSheild.changeSpeed(0,0,0,0);
m5171135 5:3f51eeb5aedc 99 hoge.detach();
m5171135 5:3f51eeb5aedc 100
m5171135 2:3f2d4f53ceed 101 switch(buf[19]){
m5171135 2:3f2d4f53ceed 102
m5171135 2:3f2d4f53ceed 103 case 0:
m5171135 2:3f2d4f53ceed 104 nowMode = STANDBY_MODE;
m5171135 2:3f2d4f53ceed 105 break;
m5171135 2:3f2d4f53ceed 106
m5171135 2:3f2d4f53ceed 107 case 1:
m5171135 5:3f51eeb5aedc 108 hoge.attach(this,&AigamozuControlPackets::manualMode,1.0);
m5171135 2:3f2d4f53ceed 109 nowMode = MANUAL_MODE;
m5171135 2:3f2d4f53ceed 110 break;
m5171135 2:3f2d4f53ceed 111
m5171135 2:3f2d4f53ceed 112 case 2:
m5171135 2:3f2d4f53ceed 113 nowMode = AUTO_MODE;
m5171135 5:3f51eeb5aedc 114 hoge.attach(this,&AigamozuControlPackets::randomAuto,1.0);
m5171135 5:3f51eeb5aedc 115
m5171135 2:3f2d4f53ceed 116 break;
m5171135 4:04dadf67ecb6 117
m5171135 4:04dadf67ecb6 118 case 3:
m5171135 4:04dadf67ecb6 119 nowMode = AUTO_GPS_MODE;
m5171135 4:04dadf67ecb6 120 break;
m5171135 4:04dadf67ecb6 121
m5171135 5:3f51eeb5aedc 122
m5171135 5:3f51eeb5aedc 123
m5171135 2:3f2d4f53ceed 124 }
m5171135 2:3f2d4f53ceed 125 return false;
m5171135 2:3f2d4f53ceed 126 }
m5171135 5:3f51eeb5aedc 127
m5171135 5:3f51eeb5aedc 128
m5171135 2:3f2d4f53ceed 129