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@49:3f097e4ad60f, 2017-03-17 (annotated)

Committer:
s1200058
Date:
Fri Mar 17 10:12:14 2017 +0000
Revision:
49:3f097e4ad60f
Parent:
48:4695e72853c6 
???????

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 49:3f097e4ad60f 4 #define HIGH_SPEED 96
s1200058 49:3f097e4ad60f 5 #define LOW_SPEED 32
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 49:3f097e4ad60f 39 // Robot -> PC
s1200058 46:c35184266e00 40 // ジャイロ・加速度データを送信する //
s1200058 45:2f47c25a3562 41 //////////////////////////////
s1200058 45:2f47c25a3562 42
s1200058 47:36fdf8cd4336 43 void AigamozuControlPackets::SendDataCommand(uint8_t fromID, uint8_t toID, int16_t gx,int16_t gy, int16_t gz, int16_t ax, int16_t ay,int16_t az, uint8_t flag)
s1200058 45:2f47c25a3562 44 {
s1200058 47:36fdf8cd4336 45
s1200058 47:36fdf8cd4336 46 UNION_int_char gx_data, gy_data, gz_data;
s1200058 47:36fdf8cd4336 47 UNION_int_char ax_data,ay_data, az_data;
s1200058 47:36fdf8cd4336 48
s1200058 47:36fdf8cd4336 49 gx_data.int_value=gx;
s1200058 47:36fdf8cd4336 50 gy_data.int_value=gy;
s1200058 47:36fdf8cd4336 51 gz_data.int_value=gz;
s1200058 47:36fdf8cd4336 52 ax_data.int_value=ax;
s1200058 47:36fdf8cd4336 53 ay_data.int_value=ay;
s1200058 47:36fdf8cd4336 54 az_data.int_value=az;
s1200058 45:2f47c25a3562 55
s1200058 48:4695e72853c6 56 uint8_t tmp[] = {'A','G','S','R','F',fromID,'T',toID,
s1200058 47:36fdf8cd4336 57 gx_data.char_value[0],gx_data.char_value[1],
s1200058 47:36fdf8cd4336 58 gy_data.char_value[0],gy_data.char_value[1],
s1200058 47:36fdf8cd4336 59 gz_data.char_value[0],gz_data.char_value[1],
s1200058 47:36fdf8cd4336 60 ax_data.char_value[0],ax_data.char_value[1],
s1200058 47:36fdf8cd4336 61 ay_data.char_value[0],ay_data.char_value[1],
s1200058 47:36fdf8cd4336 62 az_data.char_value[0],az_data.char_value[1],
s1200058 47:36fdf8cd4336 63 flag,'A','G','E'};
s1200058 47:36fdf8cd4336 64
s1200058 45:2f47c25a3562 65 for(int i = 0; i < SEND_DATA_COMMNAD_LENGTH; i++) packetData[i] = tmp[i];
s1200058 45:2f47c25a3562 66 packetLength = SEND_DATA_COMMNAD_LENGTH;
s1200058 45:2f47c25a3562 67
s1200058 45:2f47c25a3562 68 }
s1200058 45:2f47c25a3562 69
s1200058 45:2f47c25a3562 70
s1200058 48:4695e72853c6 71 //////////////////////////////
s1200058 49:3f097e4ad60f 72 // PC -> Robot
s1200058 48:4695e72853c6 73 // 受信したジャイロ・加速度データを変換する //
s1200058 48:4695e72853c6 74 //////////////////////////////
s1200058 48:4695e72853c6 75 //Updata Base Point
s1200058 48:4695e72853c6 76 void AigamozuControlPackets::GetData(uint8_t *gx,uint8_t *gy, uint8_t *gz,uint8_t *ax, uint8_t *ay,uint8_t *az){
s1200058 48:4695e72853c6 77 UNION_int_char t_gx, t_gy, t_gz, t_ax, t_ay, t_az;
s1200058 48:4695e72853c6 78 for(int i = 0;i < 2;i++){
s1200058 48:4695e72853c6 79 t_gx.char_value[i]=gx[i];
s1200058 48:4695e72853c6 80 t_gy.char_value[i]=gy[i];
s1200058 48:4695e72853c6 81 t_gz.char_value[i]=gz[i];
s1200058 48:4695e72853c6 82 }
s1200058 48:4695e72853c6 83 for(int i = 0;i < 8;i++){
s1200058 48:4695e72853c6 84 t_ax.char_value[i]=ax[i];
s1200058 48:4695e72853c6 85 t_ay.char_value[i]=ay[i];
s1200058 48:4695e72853c6 86 t_az.char_value[i]=az[i];
s1200058 48:4695e72853c6 87 }
s1200058 48:4695e72853c6 88
s1200058 48:4695e72853c6 89 r_gx = t_gx.int_value;
s1200058 48:4695e72853c6 90 r_gy = t_gy.int_value;
s1200058 48:4695e72853c6 91 r_gz = t_gz.int_value;
s1200058 48:4695e72853c6 92 r_ax = t_ax.int_value;
s1200058 48:4695e72853c6 93 r_ay = t_ay.int_value;
s1200058 48:4695e72853c6 94 r_az = t_az.int_value;
s1200058 48:4695e72853c6 95 }
s1200058 45:2f47c25a3562 96
m5171135 6:f164a716be46 97 //////////////////////////////
s1200058 49:3f097e4ad60f 98 // PC -> Robot
s1200058 49:3f097e4ad60f 99 // ジャイロ・加速度データを送信する //
s1200058 49:3f097e4ad60f 100 //////////////////////////////
s1200058 49:3f097e4ad60f 101 void AigamozuControlPackets::CreateManualCommand(uint8_t fromID,uint8_t toID,char c)
s1200058 49:3f097e4ad60f 102 {
s1200058 49:3f097e4ad60f 103 uint8_t directionL, directionR, pwmL, pwmR;
s1200058 49:3f097e4ad60f 104 if(c == 'a'){
s1200058 49:3f097e4ad60f 105 directionL = LOW_SPEED;
s1200058 49:3f097e4ad60f 106 pwmL = 1;
s1200058 49:3f097e4ad60f 107 directionR = HIGH_SPEED;
s1200058 49:3f097e4ad60f 108 pwmR = 1;
s1200058 49:3f097e4ad60f 109 }
s1200058 49:3f097e4ad60f 110 else if(c == 'w'){
s1200058 49:3f097e4ad60f 111 directionL = FORWARD_SPEED;
s1200058 49:3f097e4ad60f 112 pwmL = 1;
s1200058 49:3f097e4ad60f 113 directionR = FORWARD_SPEED;
s1200058 49:3f097e4ad60f 114 pwmR = 1;
s1200058 49:3f097e4ad60f 115 }
s1200058 49:3f097e4ad60f 116 if(c == 'd'){
s1200058 49:3f097e4ad60f 117 directionL = HIGH_SPEED;
s1200058 49:3f097e4ad60f 118 pwmL = 1;
s1200058 49:3f097e4ad60f 119 directionR = LOW_SPEED;
s1200058 49:3f097e4ad60f 120 pwmR = 1;
s1200058 49:3f097e4ad60f 121 }
s1200058 49:3f097e4ad60f 122 if(c == 'z'){
s1200058 49:3f097e4ad60f 123 directionL = FORWARD_SPEED;
s1200058 49:3f097e4ad60f 124 pwmL = 2;
s1200058 49:3f097e4ad60f 125 directionR = FORWARD_SPEED;
s1200058 49:3f097e4ad60f 126 pwmR = 2;
s1200058 49:3f097e4ad60f 127 }
s1200058 49:3f097e4ad60f 128 if(c == 's'){
s1200058 49:3f097e4ad60f 129 directionL = FORWARD_SPEED;
s1200058 49:3f097e4ad60f 130 pwmL = 0;
s1200058 49:3f097e4ad60f 131 directionR = FORWARD_SPEED;
s1200058 49:3f097e4ad60f 132 pwmR = 0;
s1200058 49:3f097e4ad60f 133 }
s1200058 49:3f097e4ad60f 134
s1200058 49:3f097e4ad60f 135
s1200058 49:3f097e4ad60f 136 uint8_t tmp[] = {'A','G','S','M','F',fromID,'T',toID,'L',directionL,pwmL,'R',directionR,pwmR,'A','G','E'};
s1200058 49:3f097e4ad60f 137 for(int i = 0; i < MANUAL_COMMAND_LENGTH; ++i) packetData[i] = tmp[i];
s1200058 49:3f097e4ad60f 138 packetLength = MANUAL_COMMAND_LENGTH;
s1200058 49:3f097e4ad60f 139 }
s1200058 49:3f097e4ad60f 140
s1200058 49:3f097e4ad60f 141 //////////////////////////////
m5171135 6:f164a716be46 142 // each mode interrupt //
m5171135 6:f164a716be46 143 //////////////////////////////
m5171135 6:f164a716be46 144
m5171135 6:f164a716be46 145 void AigamozuControlPackets::manualMode(){
m5171135 6:f164a716be46 146 manualCount++;
m5171135 6:f164a716be46 147 if(manualCount > 10){
m5171135 6:f164a716be46 148 _agzSheild.changeSpeed(0,0,0,0);
m5171135 6:f164a716be46 149 manualCount = 0;
m5171135 6:f164a716be46 150 }
m5171135 6:f164a716be46 151 }
m5171135 6:f164a716be46 152
s1200058 41:fbf8f617a6ab 153 bool AigamozuControlPackets::gpsAuto(/*const int autobase[]*/){
m5171135 5:3f51eeb5aedc 154
kityann 10:e77c664ee5e2 155 /*
kityann 10:e77c664ee5e2 156 _agzSheild.changeSpeed(2,128,2,128):for moving robot
kityann 10:e77c664ee5e2 157 */
kityann 10:e77c664ee5e2 158
s1200058 41:fbf8f617a6ab 159 if(nowMode == AUTO_GPS_MODE){
s1200058 45:2f47c25a3562 160 //if(Move_Timer.read_ms() < 5000){
s1200058 49:3f097e4ad60f 161 _agzSheild.changeSpeed(1,HIGH_SPEED,1,LOW_SPEED);//turn
s1200058 45:2f47c25a3562 162 // }else if(Move_Timer.read_ms() >= 5000 && Move_Timer.read_ms() < 6000){
s1200058 45:2f47c25a3562 163 // _agzSheild.changeSpeed(0,0,0,0);//stop
s1200058 45:2f47c25a3562 164 // Move_Timer.reset();
s1200058 45:2f47c25a3562 165 //}
s1200058 43:e011cf13dbb2 166 /*
s1200058 42:04ca5242eaf2 167 if(Move_Timer.read_ms() < 4000){
s1200058 42:04ca5242eaf2 168 _agzSheild.changeSpeed(1,LEFT_SPEED,1,RIGHT_SPEED);//turn
s1200058 42:04ca5242eaf2 169 }else if(Move_Timer.read_ms() >= 4000 && Move_Timer.read_ms() < 5000){
s1200058 42:04ca5242eaf2 170 _agzSheild.changeSpeed(0,0,0,0);//stop
s1200058 42:04ca5242eaf2 171 }else if(Move_Timer.read_ms() >= 5000 && Move_Timer.read_ms() < 6000){
s1200058 42:04ca5242eaf2 172 _agzSheild.changeSpeed(1,FORWARD_SPEED,1,FORWARD_SPEED);//forward
s1200058 42:04ca5242eaf2 173 }else if(Move_Timer.read_ms() >= 6000 && Move_Timer.read_ms() < 7000){
s1200058 42:04ca5242eaf2 174 _agzSheild.changeSpeed(0,0,0,0);//stop
s1200058 42:04ca5242eaf2 175 }else if(Move_Timer.read_ms() >= 7000 && Move_Timer.read_ms() < 12000){
s1200058 42:04ca5242eaf2 176 _agzSheild.changeSpeed(1,RIGHT_SPEED,1,LEFT_SPEED);//turn
s1200058 42:04ca5242eaf2 177 }else if(Move_Timer.read_ms() >=12000 && Move_Timer.read_ms() < 13000){
s1200058 42:04ca5242eaf2 178 _agzSheild.changeSpeed(0,0,0,0);//stop
s1200058 42:04ca5242eaf2 179 }else if(Move_Timer.read_ms() >= 13000 && Move_Timer.read_ms() < 14000){
s1200058 42:04ca5242eaf2 180 _agzSheild.changeSpeed(1,FORWARD_SPEED,1,FORWARD_SPEED);//forward
s1200058 42:04ca5242eaf2 181 }else if(Move_Timer.read_ms() >= 14000 && Move_Timer.read_ms() < 15000){
s1200058 42:04ca5242eaf2 182 _agzSheild.changeSpeed(0,0,0,0);//stop
s1200058 42:04ca5242eaf2 183 Move_Timer.reset();
kityann 10:e77c664ee5e2 184 }
s1200058 43:e011cf13dbb2 185 */
s1200058 32:cd68a37d1ee1 186 }
s1200058 41:fbf8f617a6ab 187
s1200058 41:fbf8f617a6ab 188 return 1;
kityann 10:e77c664ee5e2 189 }
s1200058 41:fbf8f617a6ab 190
m5171135 7:200ce5c1f486 191
m5171135 7:200ce5c1f486 192
m5171135 7:200ce5c1f486 193
m5171135 5:3f51eeb5aedc 194
m5171135 6:f164a716be46 195 //////////////////////////////
m5171135 6:f164a716be46 196 // Mode change //
m5171135 6:f164a716be46 197 //////////////////////////////
m5171135 2:3f2d4f53ceed 198 bool AigamozuControlPackets::changeMode(uint8_t *buf){
m5171135 2:3f2d4f53ceed 199
m5171135 6:f164a716be46 200 //reset
m5171135 5:3f51eeb5aedc 201 _agzSheild.changeSpeed(0,0,0,0);
m5171135 6:f164a716be46 202 eachModeInt.detach();
m5171135 5:3f51eeb5aedc 203
m5171135 6:f164a716be46 204 //Select Mode
s1200058 29:3a2d87c10e43 205 switch(buf[19]){
m5171135 2:3f2d4f53ceed 206 case 0:
m5171135 2:3f2d4f53ceed 207 nowMode = STANDBY_MODE;
m5171135 2:3f2d4f53ceed 208 break;
m5171135 2:3f2d4f53ceed 209
m5171135 2:3f2d4f53ceed 210 case 1:
m5171135 6:f164a716be46 211 eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0);
m5171135 2:3f2d4f53ceed 212 nowMode = MANUAL_MODE;
m5171135 2:3f2d4f53ceed 213 break;
m5171135 2:3f2d4f53ceed 214
m5171135 2:3f2d4f53ceed 215 case 2:
kityann 11:4d71c9cc3b4a 216 nowMode = AUTO_GPS_MODE;
s1200058 33:c11a9cb35840 217 Move_Timer.reset();
m5171135 2:3f2d4f53ceed 218 break;
m5171135 4:04dadf67ecb6 219
m5171135 4:04dadf67ecb6 220 case 3:
m5171135 4:04dadf67ecb6 221 nowMode = AUTO_GPS_MODE;
s1200058 33:c11a9cb35840 222 Move_Timer.reset();
m5171135 4:04dadf67ecb6 223 break;
m5171135 4:04dadf67ecb6 224
m5171135 6:f164a716be46 225 default:
m5171135 6:f164a716be46 226 nowMode = STANDBY_MODE;
m5171135 6:f164a716be46 227 break;
m5171135 5:3f51eeb5aedc 228
m5171135 2:3f2d4f53ceed 229 }
m5171135 2:3f2d4f53ceed 230 return false;
m5171135 2:3f2d4f53ceed 231 }
m5171135 5:3f51eeb5aedc 232
m5171135 5:3f51eeb5aedc 233
m5171135 2:3f2d4f53ceed 234