aigamozu / AigamozuControlPackets_展示会

展示会用です

Dependencies:   VNH5019

Dependents:   Aigamozu_Robot_展示会 Aigamozu_Robot_March Aigamozu_Robot_templete

Fork of AigamozuControlPackets by aigamozu

AigamozuControlPackets.cpp@53:99b977d16a6d, 2017-08-04 (annotated)

Committer:
s1200058
Date:
Fri Aug 04 12:49:32 2017 +0000
Revision:
53:99b977d16a6d
Parent:
50:61d72f617aa5 
for_OHARA

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