aigamozu / AigamozuControlPackets_展示会

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   VNH5019

Dependents:   Aigamozu_Robot_展示会 Aigamozu_Robot_March Aigamozu_Robot_templete

Fork of AigamozuControlPackets by aigamozu

AigamozuControlPackets.cpp@52:d4d739f01b38, 2017-04-20 (annotated)

Committer:
s1200058
Date:
Thu Apr 20 10:24:16 2017 +0000
Revision:
52:d4d739f01b38
Parent:
51:67f01969f709 
for next study group

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
s1200058 50:61d72f617aa5 4 #define LOW_SPEED 16
s1200058 50:61d72f617aa5 5 #define HIGH_SPEED 64
s1200058 52:d4d739f01b38 6 #define FORWARD_SPEED 32
s1200058 41:fbf8f617a6ab 7
m5171135 6:f164a716be46 8 //////////////////////////////
m5171135 6:f164a716be46 9 // Init //
m5171135 6:f164a716be46 10 //////////////////////////////
m5171135 5:3f51eeb5aedc 11 AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild){
s1200058 52:d4d739f01b38 12 packetData = new uint8_t[50];
m5171135 2:3f2d4f53ceed 13 }
m5171135 2:3f2d4f53ceed 14
kityann 17:a6fa8cc96d94 15 //////////////////////////////
m5171135 2:3f2d4f53ceed 16 // Using createPacket //
m5171135 2:3f2d4f53ceed 17 //////////////////////////////
m5171135 4:04dadf67ecb6 18 uint8_t AigamozuControlPackets::checkCommnadType(uint8_t* buf){
m5171135 4:04dadf67ecb6 19 return buf[14];
m5171135 4:04dadf67ecb6 20 }
m5171135 2:3f2d4f53ceed 21
m5171135 2:3f2d4f53ceed 22 uint8_t* AigamozuControlPackets::getPacketData(){
m5171135 2:3f2d4f53ceed 23 return packetData;
m5171135 2:3f2d4f53ceed 24 }
m5171135 2:3f2d4f53ceed 25
m5171135 2:3f2d4f53ceed 26 int AigamozuControlPackets::getPacketLength(){
m5171135 2:3f2d4f53ceed 27 return packetLength;
m5171135 0:2b8b56ac7a82 28 }
m5171135 0:2b8b56ac7a82 29
s1200058 50:61d72f617aa5 30 //changeSpeed()
s1200058 50:61d72f617aa5 31 //ロボットを操縦する
m5171135 5:3f51eeb5aedc 32 void AigamozuControlPackets::changeSpeed(uint8_t* buf){
m5171135 5:3f51eeb5aedc 33 if(nowMode == MANUAL_MODE){
m5171135 5:3f51eeb5aedc 34 manualCount =0;
m5171135 5:3f51eeb5aedc 35 _agzSheild.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
m5171135 5:3f51eeb5aedc 36 }
s1200058 52:d4d739f01b38 37 }
s1200058 52:d4d739f01b38 38
s1200058 52:d4d739f01b38 39
m5171135 4:04dadf67ecb6 40
s1200058 52:d4d739f01b38 41 void AigamozuControlPackets::createChangeModeCommand(uint8_t fromID,uint8_t toID,int mode){
s1200058 52:d4d739f01b38 42 uint8_t tmp[] = {'A','G','S','C','F',fromID,'T',toID,(uint8_t)mode,'A','G','E'};
s1200058 52:d4d739f01b38 43 for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; ++i) packetData[i] = tmp[i];
s1200058 52:d4d739f01b38 44 packetLength = CHANGE_MODE_COMMAND_LENGTH;
s1200058 52:d4d739f01b38 45 }
s1200058 52:d4d739f01b38 46
s1200058 52:d4d739f01b38 47
s1200058 50:61d72f617aa5 48 ///////////////////////////////////////
s1200058 50:61d72f617aa5 49 // each mode interrupt //
s1200058 50:61d72f617aa5 50 ///////////////////////////////////////
s1200058 50:61d72f617aa5 51
s1200058 50:61d72f617aa5 52 //manualMode()
s1200058 50:61d72f617aa5 53 //マニュアルコマンドを連続で
s1200058 50:61d72f617aa5 54 //10回以上処理した場合、一瞬停止する
m5171135 6:f164a716be46 55
m5171135 6:f164a716be46 56 void AigamozuControlPackets::manualMode(){
m5171135 6:f164a716be46 57 manualCount++;
m5171135 6:f164a716be46 58 if(manualCount > 10){
m5171135 6:f164a716be46 59 _agzSheild.changeSpeed(0,0,0,0);
m5171135 6:f164a716be46 60 manualCount = 0;
m5171135 6:f164a716be46 61 }
m5171135 6:f164a716be46 62 }
m5171135 6:f164a716be46 63
s1200058 50:61d72f617aa5 64 //autoMove()
s1200058 50:61d72f617aa5 65 //ロボットがオートモードの際に
s1200058 50:61d72f617aa5 66 //シーケンス動作させるために使用する
s1200058 50:61d72f617aa5 67 bool AigamozuControlPackets::autoMove(/*const int autobase[]*/){
m5171135 5:3f51eeb5aedc 68
kityann 10:e77c664ee5e2 69 /*
kityann 10:e77c664ee5e2 70 _agzSheild.changeSpeed(2,128,2,128):for moving robot
s1200058 50:61d72f617aa5 71 VNH5019参照
kityann 10:e77c664ee5e2 72 */
s1200058 51:67f01969f709 73
s1200058 51:67f01969f709 74 if(nowMode == AUTO_GPS_MODE){
s1200058 41:fbf8f617a6ab 75
s1200058 52:d4d739f01b38 76 //if(moveTimer.read_ms() <= 400){
s1200058 52:d4d739f01b38 77 _agzSheild.changeSpeed(1,FORWARD_SPEED,1,FORWARD_SPEED);
s1200058 52:d4d739f01b38 78 //}else if(moveTimer.read_ms() > 400 && moveTimer.read_ms() <= 500){
s1200058 52:d4d739f01b38 79 // _agzSheild.changeSpeed(0,LOW_SPEED,0,LOW_SPEED); //stop
s1200058 52:d4d739f01b38 80 //}
s1200058 32:cd68a37d1ee1 81 }
s1200058 41:fbf8f617a6ab 82
s1200058 51:67f01969f709 83 return 1;
kityann 10:e77c664ee5e2 84 }
m5171135 5:3f51eeb5aedc 85
m5171135 6:f164a716be46 86 //////////////////////////////
m5171135 6:f164a716be46 87 // Mode change //
m5171135 6:f164a716be46 88 //////////////////////////////
m5171135 2:3f2d4f53ceed 89 bool AigamozuControlPackets::changeMode(uint8_t *buf){
m5171135 2:3f2d4f53ceed 90
m5171135 6:f164a716be46 91 //reset
m5171135 5:3f51eeb5aedc 92 _agzSheild.changeSpeed(0,0,0,0);
m5171135 6:f164a716be46 93 eachModeInt.detach();
m5171135 5:3f51eeb5aedc 94
m5171135 6:f164a716be46 95 //Select Mode
s1200058 29:3a2d87c10e43 96 switch(buf[19]){
s1200058 50:61d72f617aa5 97 case 0: //スタンバイ
m5171135 2:3f2d4f53ceed 98 nowMode = STANDBY_MODE;
m5171135 2:3f2d4f53ceed 99 break;
m5171135 2:3f2d4f53ceed 100
s1200058 50:61d72f617aa5 101 case 1: //マニュアル
m5171135 6:f164a716be46 102 eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0);
m5171135 2:3f2d4f53ceed 103 nowMode = MANUAL_MODE;
m5171135 2:3f2d4f53ceed 104 break;
m5171135 2:3f2d4f53ceed 105
s1200058 50:61d72f617aa5 106 case 2: //オートモード
kityann 11:4d71c9cc3b4a 107 nowMode = AUTO_GPS_MODE;
s1200058 50:61d72f617aa5 108 moveTimer.reset();
m5171135 2:3f2d4f53ceed 109 break;
m5171135 4:04dadf67ecb6 110
s1200058 50:61d72f617aa5 111 case 3: //オートモード
m5171135 4:04dadf67ecb6 112 nowMode = AUTO_GPS_MODE;
s1200058 50:61d72f617aa5 113 moveTimer.reset();
m5171135 4:04dadf67ecb6 114 break;
m5171135 4:04dadf67ecb6 115
m5171135 6:f164a716be46 116 default:
m5171135 6:f164a716be46 117 nowMode = STANDBY_MODE;
m5171135 6:f164a716be46 118 break;
m5171135 5:3f51eeb5aedc 119
m5171135 2:3f2d4f53ceed 120 }
m5171135 2:3f2d4f53ceed 121 return false;
m5171135 2:3f2d4f53ceed 122 }