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@46:c35184266e00, 2017-03-11 (annotated)

Committer:
s1200058
Date:
Sat Mar 11 15:01:37 2017 +0000
Revision:
46:c35184266e00
Parent:
45:2f47c25a3562
Child:
47:36fdf8cd4336
????????

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 45:2f47c25a3562 4 #define RIGHT_SPEED 96
s1200058 45:2f47c25a3562 5 #define LEFT_SPEED 96
s1200058 45:2f47c25a3562 6 #define FORWARD_SPEED 96
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 45:2f47c25a3562 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
m5171135 5:3f51eeb5aedc 30 void AigamozuControlPackets::changeSpeed(uint8_t* buf){
m5171135 5:3f51eeb5aedc 31 if(nowMode == MANUAL_MODE){
m5171135 5:3f51eeb5aedc 32 manualCount =0;
m5171135 5:3f51eeb5aedc 33 _agzSheild.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
m5171135 5:3f51eeb5aedc 34 }
m5171135 5:3f51eeb5aedc 35 }
m5171135 4:04dadf67ecb6 36
s1200058 45:2f47c25a3562 37
s1200058 45:2f47c25a3562 38 //////////////////////////////
s1200058 46:c35184266e00 39 // Robot -> Xbee
s1200058 46:c35184266e00 40 // ジャイロ・加速度データを送信する //
s1200058 45:2f47c25a3562 41 //////////////////////////////
s1200058 45:2f47c25a3562 42
s1200058 45:2f47c25a3562 43 void AigamozuControlPackets::SendDataCommand(uint8_t fromID, uint8_t toID, uint8_t gx,uint8_t gy, uint8_t gz, uint8_t ax, uint8_t ay,uint8_t az, uint8_t flag)
s1200058 45:2f47c25a3562 44 {
s1200058 45:2f47c25a3562 45
s1200058 45:2f47c25a3562 46 uint8_t tmp[] = {'A','G','S','M','F',fromID,'T',toID, gx, gy, gz, ax, ay, az,flag,'A','G','E'};
s1200058 45:2f47c25a3562 47 for(int i = 0; i < SEND_DATA_COMMNAD_LENGTH; i++) packetData[i] = tmp[i];
s1200058 45:2f47c25a3562 48 packetLength = SEND_DATA_COMMNAD_LENGTH;
s1200058 45:2f47c25a3562 49
s1200058 45:2f47c25a3562 50 }
s1200058 45:2f47c25a3562 51
s1200058 45:2f47c25a3562 52
s1200058 45:2f47c25a3562 53
m5171135 6:f164a716be46 54 //////////////////////////////
m5171135 6:f164a716be46 55 // each mode interrupt //
m5171135 6:f164a716be46 56 //////////////////////////////
m5171135 6:f164a716be46 57
m5171135 6:f164a716be46 58 void AigamozuControlPackets::manualMode(){
m5171135 6:f164a716be46 59 manualCount++;
m5171135 6:f164a716be46 60 if(manualCount > 10){
m5171135 6:f164a716be46 61 _agzSheild.changeSpeed(0,0,0,0);
m5171135 6:f164a716be46 62 manualCount = 0;
m5171135 6:f164a716be46 63 }
m5171135 6:f164a716be46 64 }
m5171135 6:f164a716be46 65
s1200058 41:fbf8f617a6ab 66 bool AigamozuControlPackets::gpsAuto(/*const int autobase[]*/){
m5171135 5:3f51eeb5aedc 67
kityann 10:e77c664ee5e2 68 /*
kityann 10:e77c664ee5e2 69 _agzSheild.changeSpeed(2,128,2,128):for moving robot
kityann 10:e77c664ee5e2 70 */
kityann 10:e77c664ee5e2 71
s1200058 41:fbf8f617a6ab 72 if(nowMode == AUTO_GPS_MODE){
s1200058 45:2f47c25a3562 73 //if(Move_Timer.read_ms() < 5000){
s1200058 43:e011cf13dbb2 74 _agzSheild.changeSpeed(1,LEFT_SPEED,1,RIGHT_SPEED);//turn
s1200058 45:2f47c25a3562 75 // }else if(Move_Timer.read_ms() >= 5000 && Move_Timer.read_ms() < 6000){
s1200058 45:2f47c25a3562 76 // _agzSheild.changeSpeed(0,0,0,0);//stop
s1200058 45:2f47c25a3562 77 // Move_Timer.reset();
s1200058 45:2f47c25a3562 78 //}
s1200058 43:e011cf13dbb2 79 /*
s1200058 42:04ca5242eaf2 80 if(Move_Timer.read_ms() < 4000){
s1200058 42:04ca5242eaf2 81 _agzSheild.changeSpeed(1,LEFT_SPEED,1,RIGHT_SPEED);//turn
s1200058 42:04ca5242eaf2 82 }else if(Move_Timer.read_ms() >= 4000 && Move_Timer.read_ms() < 5000){
s1200058 42:04ca5242eaf2 83 _agzSheild.changeSpeed(0,0,0,0);//stop
s1200058 42:04ca5242eaf2 84 }else if(Move_Timer.read_ms() >= 5000 && Move_Timer.read_ms() < 6000){
s1200058 42:04ca5242eaf2 85 _agzSheild.changeSpeed(1,FORWARD_SPEED,1,FORWARD_SPEED);//forward
s1200058 42:04ca5242eaf2 86 }else if(Move_Timer.read_ms() >= 6000 && Move_Timer.read_ms() < 7000){
s1200058 42:04ca5242eaf2 87 _agzSheild.changeSpeed(0,0,0,0);//stop
s1200058 42:04ca5242eaf2 88 }else if(Move_Timer.read_ms() >= 7000 && Move_Timer.read_ms() < 12000){
s1200058 42:04ca5242eaf2 89 _agzSheild.changeSpeed(1,RIGHT_SPEED,1,LEFT_SPEED);//turn
s1200058 42:04ca5242eaf2 90 }else if(Move_Timer.read_ms() >=12000 && Move_Timer.read_ms() < 13000){
s1200058 42:04ca5242eaf2 91 _agzSheild.changeSpeed(0,0,0,0);//stop
s1200058 42:04ca5242eaf2 92 }else if(Move_Timer.read_ms() >= 13000 && Move_Timer.read_ms() < 14000){
s1200058 42:04ca5242eaf2 93 _agzSheild.changeSpeed(1,FORWARD_SPEED,1,FORWARD_SPEED);//forward
s1200058 42:04ca5242eaf2 94 }else if(Move_Timer.read_ms() >= 14000 && Move_Timer.read_ms() < 15000){
s1200058 42:04ca5242eaf2 95 _agzSheild.changeSpeed(0,0,0,0);//stop
s1200058 42:04ca5242eaf2 96 Move_Timer.reset();
kityann 10:e77c664ee5e2 97 }
s1200058 43:e011cf13dbb2 98 */
s1200058 32:cd68a37d1ee1 99 }
s1200058 41:fbf8f617a6ab 100
s1200058 41:fbf8f617a6ab 101 return 1;
kityann 10:e77c664ee5e2 102 }
s1200058 41:fbf8f617a6ab 103
m5171135 7:200ce5c1f486 104
m5171135 7:200ce5c1f486 105
m5171135 7:200ce5c1f486 106
m5171135 5:3f51eeb5aedc 107
m5171135 6:f164a716be46 108 //////////////////////////////
m5171135 6:f164a716be46 109 // Mode change //
m5171135 6:f164a716be46 110 //////////////////////////////
m5171135 2:3f2d4f53ceed 111 bool AigamozuControlPackets::changeMode(uint8_t *buf){
m5171135 2:3f2d4f53ceed 112
m5171135 6:f164a716be46 113 //reset
m5171135 5:3f51eeb5aedc 114 _agzSheild.changeSpeed(0,0,0,0);
m5171135 6:f164a716be46 115 eachModeInt.detach();
m5171135 5:3f51eeb5aedc 116
m5171135 6:f164a716be46 117 //Select Mode
s1200058 29:3a2d87c10e43 118 switch(buf[19]){
m5171135 2:3f2d4f53ceed 119 case 0:
m5171135 2:3f2d4f53ceed 120 nowMode = STANDBY_MODE;
m5171135 2:3f2d4f53ceed 121 break;
m5171135 2:3f2d4f53ceed 122
m5171135 2:3f2d4f53ceed 123 case 1:
m5171135 6:f164a716be46 124 eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0);
m5171135 2:3f2d4f53ceed 125 nowMode = MANUAL_MODE;
m5171135 2:3f2d4f53ceed 126 break;
m5171135 2:3f2d4f53ceed 127
m5171135 2:3f2d4f53ceed 128 case 2:
kityann 11:4d71c9cc3b4a 129 nowMode = AUTO_GPS_MODE;
s1200058 33:c11a9cb35840 130 Move_Timer.reset();
m5171135 2:3f2d4f53ceed 131 break;
m5171135 4:04dadf67ecb6 132
m5171135 4:04dadf67ecb6 133 case 3:
m5171135 4:04dadf67ecb6 134 nowMode = AUTO_GPS_MODE;
s1200058 33:c11a9cb35840 135 Move_Timer.reset();
m5171135 4:04dadf67ecb6 136 break;
m5171135 4:04dadf67ecb6 137
m5171135 6:f164a716be46 138 default:
m5171135 6:f164a716be46 139 nowMode = STANDBY_MODE;
m5171135 6:f164a716be46 140 break;
m5171135 5:3f51eeb5aedc 141
m5171135 2:3f2d4f53ceed 142 }
m5171135 2:3f2d4f53ceed 143 return false;
m5171135 2:3f2d4f53ceed 144 }
m5171135 5:3f51eeb5aedc 145
m5171135 5:3f51eeb5aedc 146
m5171135 2:3f2d4f53ceed 147