aigamozu / AigamozuControlPackets_展示会

展示会用です

Dependencies:   VNH5019

Dependents:   Aigamozu_Robot_展示会 Aigamozu_Robot_March Aigamozu_Robot_templete

Fork of AigamozuControlPackets by aigamozu

AigamozuControlPackets.cpp@42:04ca5242eaf2, 2016-10-24 (annotated)

Committer:
s1200058
Date:
Mon Oct 24 09:12:37 2016 +0000
Revision:
42:04ca5242eaf2
Parent:
41:fbf8f617a6ab
Child:
43:e011cf13dbb2
Child:
44:da580a931f28
8??

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 42:04ca5242eaf2 4 #define RIGHT_SPEED 0
s1200058 42:04ca5242eaf2 5 #define LEFT_SPEED 32
s1200058 42:04ca5242eaf2 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){
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
m5171135 5:3f51eeb5aedc 29 void AigamozuControlPackets::changeSpeed(uint8_t* buf){
m5171135 5:3f51eeb5aedc 30 if(nowMode == MANUAL_MODE){
m5171135 5:3f51eeb5aedc 31 manualCount =0;
m5171135 5:3f51eeb5aedc 32 _agzSheild.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
m5171135 5:3f51eeb5aedc 33 }
m5171135 5:3f51eeb5aedc 34 }
m5171135 4:04dadf67ecb6 35
m5171135 6:f164a716be46 36 //////////////////////////////
m5171135 6:f164a716be46 37 // each mode interrupt //
m5171135 6:f164a716be46 38 //////////////////////////////
m5171135 6:f164a716be46 39
m5171135 6:f164a716be46 40 void AigamozuControlPackets::manualMode(){
m5171135 6:f164a716be46 41 manualCount++;
m5171135 6:f164a716be46 42 if(manualCount > 10){
m5171135 6:f164a716be46 43 _agzSheild.changeSpeed(0,0,0,0);
m5171135 6:f164a716be46 44 manualCount = 0;
m5171135 6:f164a716be46 45 }
m5171135 6:f164a716be46 46 }
m5171135 6:f164a716be46 47
s1200058 41:fbf8f617a6ab 48 bool AigamozuControlPackets::gpsAuto(/*const int autobase[]*/){
m5171135 5:3f51eeb5aedc 49
kityann 10:e77c664ee5e2 50 /*
kityann 10:e77c664ee5e2 51 _agzSheild.changeSpeed(2,128,2,128):for moving robot
kityann 10:e77c664ee5e2 52 */
kityann 10:e77c664ee5e2 53
s1200058 41:fbf8f617a6ab 54 if(nowMode == AUTO_GPS_MODE){
s1200058 41:fbf8f617a6ab 55
s1200058 42:04ca5242eaf2 56 if(Move_Timer.read_ms() < 4000){
s1200058 42:04ca5242eaf2 57 _agzSheild.changeSpeed(1,LEFT_SPEED,1,RIGHT_SPEED);//turn
s1200058 42:04ca5242eaf2 58 }else if(Move_Timer.read_ms() >= 4000 && Move_Timer.read_ms() < 5000){
s1200058 42:04ca5242eaf2 59 _agzSheild.changeSpeed(0,0,0,0);//stop
s1200058 42:04ca5242eaf2 60 }else if(Move_Timer.read_ms() >= 5000 && Move_Timer.read_ms() < 6000){
s1200058 42:04ca5242eaf2 61 _agzSheild.changeSpeed(1,FORWARD_SPEED,1,FORWARD_SPEED);//forward
s1200058 42:04ca5242eaf2 62 }else if(Move_Timer.read_ms() >= 6000 && Move_Timer.read_ms() < 7000){
s1200058 42:04ca5242eaf2 63 _agzSheild.changeSpeed(0,0,0,0);//stop
s1200058 42:04ca5242eaf2 64 }else if(Move_Timer.read_ms() >= 7000 && Move_Timer.read_ms() < 12000){
s1200058 42:04ca5242eaf2 65 _agzSheild.changeSpeed(1,RIGHT_SPEED,1,LEFT_SPEED);//turn
s1200058 42:04ca5242eaf2 66 }else if(Move_Timer.read_ms() >=12000 && Move_Timer.read_ms() < 13000){
s1200058 42:04ca5242eaf2 67 _agzSheild.changeSpeed(0,0,0,0);//stop
s1200058 42:04ca5242eaf2 68 }else if(Move_Timer.read_ms() >= 13000 && Move_Timer.read_ms() < 14000){
s1200058 42:04ca5242eaf2 69 _agzSheild.changeSpeed(1,FORWARD_SPEED,1,FORWARD_SPEED);//forward
s1200058 42:04ca5242eaf2 70 }else if(Move_Timer.read_ms() >= 14000 && Move_Timer.read_ms() < 15000){
s1200058 42:04ca5242eaf2 71 _agzSheild.changeSpeed(0,0,0,0);//stop
s1200058 42:04ca5242eaf2 72 Move_Timer.reset();
kityann 10:e77c664ee5e2 73 }
s1200058 32:cd68a37d1ee1 74 }
s1200058 41:fbf8f617a6ab 75
s1200058 41:fbf8f617a6ab 76 return 1;
kityann 10:e77c664ee5e2 77 }
s1200058 41:fbf8f617a6ab 78
m5171135 7:200ce5c1f486 79
m5171135 7:200ce5c1f486 80
m5171135 7:200ce5c1f486 81
m5171135 5:3f51eeb5aedc 82
m5171135 6:f164a716be46 83 //////////////////////////////
m5171135 6:f164a716be46 84 // Mode change //
m5171135 6:f164a716be46 85 //////////////////////////////
m5171135 2:3f2d4f53ceed 86 bool AigamozuControlPackets::changeMode(uint8_t *buf){
m5171135 2:3f2d4f53ceed 87
m5171135 6:f164a716be46 88 //reset
m5171135 5:3f51eeb5aedc 89 _agzSheild.changeSpeed(0,0,0,0);
m5171135 6:f164a716be46 90 eachModeInt.detach();
m5171135 5:3f51eeb5aedc 91
m5171135 6:f164a716be46 92 //Select Mode
s1200058 29:3a2d87c10e43 93 switch(buf[19]){
m5171135 2:3f2d4f53ceed 94 case 0:
m5171135 2:3f2d4f53ceed 95 nowMode = STANDBY_MODE;
m5171135 2:3f2d4f53ceed 96 break;
m5171135 2:3f2d4f53ceed 97
m5171135 2:3f2d4f53ceed 98 case 1:
m5171135 6:f164a716be46 99 eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0);
m5171135 2:3f2d4f53ceed 100 nowMode = MANUAL_MODE;
m5171135 2:3f2d4f53ceed 101 break;
m5171135 2:3f2d4f53ceed 102
m5171135 2:3f2d4f53ceed 103 case 2:
kityann 11:4d71c9cc3b4a 104 nowMode = AUTO_GPS_MODE;
s1200058 33:c11a9cb35840 105 Move_Timer.reset();
m5171135 2:3f2d4f53ceed 106 break;
m5171135 4:04dadf67ecb6 107
m5171135 4:04dadf67ecb6 108 case 3:
m5171135 4:04dadf67ecb6 109 nowMode = AUTO_GPS_MODE;
s1200058 33:c11a9cb35840 110 Move_Timer.reset();
m5171135 4:04dadf67ecb6 111 break;
m5171135 4:04dadf67ecb6 112
m5171135 6:f164a716be46 113 default:
m5171135 6:f164a716be46 114 nowMode = STANDBY_MODE;
m5171135 6:f164a716be46 115 break;
m5171135 5:3f51eeb5aedc 116
m5171135 2:3f2d4f53ceed 117 }
m5171135 2:3f2d4f53ceed 118 return false;
m5171135 2:3f2d4f53ceed 119 }
m5171135 5:3f51eeb5aedc 120
m5171135 5:3f51eeb5aedc 121
m5171135 2:3f2d4f53ceed 122