展示会用です
Dependencies: VNH5019
Dependents: Aigamozu_Robot_展示会 Aigamozu_Robot_March Aigamozu_Robot_templete
Fork of AigamozuControlPackets by
AigamozuControlPackets.cpp
- Committer:
- s1200058
- Date:
- 2017-03-13
- Revision:
- 47:36fdf8cd4336
- Parent:
- 46:c35184266e00
- Child:
- 48:4695e72853c6
File content as of revision 47:36fdf8cd4336:
#include "AigamozuControlPackets.h" #include "VNH5019.h" #define RIGHT_SPEED 96 #define LEFT_SPEED 96 #define FORWARD_SPEED 96 ////////////////////////////// // Init // ////////////////////////////// AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild){ packetData = new uint8_t[50]; } ////////////////////////////// // Using createPacket // ////////////////////////////// uint8_t AigamozuControlPackets::checkCommnadType(uint8_t* buf){ return buf[14]; } uint8_t* AigamozuControlPackets::getPacketData(){ return packetData; } int AigamozuControlPackets::getPacketLength(){ return packetLength; } void AigamozuControlPackets::changeSpeed(uint8_t* buf){ if(nowMode == MANUAL_MODE){ manualCount =0; _agzSheild.changeSpeed(buf[20],buf[21],buf[23],buf[24]); } } ////////////////////////////// // Robot -> Xbee // ジャイロ・加速度データを送信する // ////////////////////////////// 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) { UNION_int_char gx_data, gy_data, gz_data; UNION_int_char ax_data,ay_data, az_data; gx_data.int_value=gx; gy_data.int_value=gy; gz_data.int_value=gz; ax_data.int_value=ax; ay_data.int_value=ay; az_data.int_value=az; uint8_t tmp[] = {'A','G','S','M','F',fromID,'T',toID, gx_data.char_value[0],gx_data.char_value[1], gy_data.char_value[0],gy_data.char_value[1], gz_data.char_value[0],gz_data.char_value[1], ax_data.char_value[0],ax_data.char_value[1], ay_data.char_value[0],ay_data.char_value[1], az_data.char_value[0],az_data.char_value[1], flag,'A','G','E'}; for(int i = 0; i < SEND_DATA_COMMNAD_LENGTH; i++) packetData[i] = tmp[i]; packetLength = SEND_DATA_COMMNAD_LENGTH; } ////////////////////////////// // each mode interrupt // ////////////////////////////// void AigamozuControlPackets::manualMode(){ manualCount++; if(manualCount > 10){ _agzSheild.changeSpeed(0,0,0,0); manualCount = 0; } } bool AigamozuControlPackets::gpsAuto(/*const int autobase[]*/){ /* _agzSheild.changeSpeed(2,128,2,128):for moving robot */ if(nowMode == AUTO_GPS_MODE){ //if(Move_Timer.read_ms() < 5000){ _agzSheild.changeSpeed(1,LEFT_SPEED,1,RIGHT_SPEED);//turn // }else if(Move_Timer.read_ms() >= 5000 && Move_Timer.read_ms() < 6000){ // _agzSheild.changeSpeed(0,0,0,0);//stop // Move_Timer.reset(); //} /* if(Move_Timer.read_ms() < 4000){ _agzSheild.changeSpeed(1,LEFT_SPEED,1,RIGHT_SPEED);//turn }else if(Move_Timer.read_ms() >= 4000 && Move_Timer.read_ms() < 5000){ _agzSheild.changeSpeed(0,0,0,0);//stop }else if(Move_Timer.read_ms() >= 5000 && Move_Timer.read_ms() < 6000){ _agzSheild.changeSpeed(1,FORWARD_SPEED,1,FORWARD_SPEED);//forward }else if(Move_Timer.read_ms() >= 6000 && Move_Timer.read_ms() < 7000){ _agzSheild.changeSpeed(0,0,0,0);//stop }else if(Move_Timer.read_ms() >= 7000 && Move_Timer.read_ms() < 12000){ _agzSheild.changeSpeed(1,RIGHT_SPEED,1,LEFT_SPEED);//turn }else if(Move_Timer.read_ms() >=12000 && Move_Timer.read_ms() < 13000){ _agzSheild.changeSpeed(0,0,0,0);//stop }else if(Move_Timer.read_ms() >= 13000 && Move_Timer.read_ms() < 14000){ _agzSheild.changeSpeed(1,FORWARD_SPEED,1,FORWARD_SPEED);//forward }else if(Move_Timer.read_ms() >= 14000 && Move_Timer.read_ms() < 15000){ _agzSheild.changeSpeed(0,0,0,0);//stop Move_Timer.reset(); } */ } return 1; } ////////////////////////////// // Mode change // ////////////////////////////// bool AigamozuControlPackets::changeMode(uint8_t *buf){ //reset _agzSheild.changeSpeed(0,0,0,0); eachModeInt.detach(); //Select Mode switch(buf[19]){ case 0: nowMode = STANDBY_MODE; break; case 1: eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0); nowMode = MANUAL_MODE; break; case 2: nowMode = AUTO_GPS_MODE; Move_Timer.reset(); break; case 3: nowMode = AUTO_GPS_MODE; Move_Timer.reset(); break; default: nowMode = STANDBY_MODE; break; } return false; }