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@51:67f01969f709, 2017-03-31 (annotated)

Committer:
s1200058
Date:
Fri Mar 31 06:26:28 2017 +0000
Revision:
51:67f01969f709
Parent:
50:61d72f617aa5
Child:
52:d4d739f01b38
????

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