Haruna Nakazawa
20150512 Nakazawa
Diff: AigamozuControlPackets.cpp
- Revision:
- 0:806e9a434e81
- Child:
- 1:6d7c854c927d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AigamozuControlPackets.cpp Tue May 12 10:40:07 2015 +0000
@@ -0,0 +1,334 @@
+#include "AigamozuControlPackets.h"
+#include "VNH5019.h"
+
+//////////////////////////////
+// Init //
+//////////////////////////////
+AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild){
+ packetData = new uint8_t[50];
+ packetLength = 0;
+ //--init base point--//
+ //basePoint[0].x = 13956.2655;
+ //basePoint[0].y = 3731.5060;
+ basePoint[0].x = 100.2655;
+ basePoint[0].y = 30.5060;
+
+ //basePoint[1].x = 13956.2898;
+ //basePoint[1].y = 3731.5055;
+ basePoint[1].x = 200.2898;
+ basePoint[1].y = 30.5055;
+
+ //basePoint[2].x = 13956.2915;
+ //basePoint[2].y = 3731.5245;
+ basePoint[2].x = 200.2915;
+ basePoint[2].y = 40.5245;
+
+ //basePoint[3].x = 13956.2680;
+ //basePoint[3].y = 3731.5248;
+ basePoint[3].x = 100.2680;
+ basePoint[3].y = 40.5248;
+
+ agzPoint.x=0;agzPoint.y=0;
+ agzPointKalman.x=0;agzPointKalman.y=0;
+ agzCov.x=0;agzCov.y=0;
+ }
+
+//////////////////////////////
+// get関数
+//////////////////////////////
+double AigamozuControlPackets::get_agzPoint_lati(){
+ return agzPoint.x;
+ }
+double AigamozuControlPackets::get_agzPoint_longi(){
+ return agzPoint.y;
+ }
+double AigamozuControlPackets::get_agzPointKalman_lati(){
+ return agzPointKalman.x;
+ }
+double AigamozuControlPackets::get_agzPointKalman_longi(){
+ return agzPointKalman.y;
+ }
+double AigamozuControlPackets::get_agzCov_lati(){
+ return agzCov.x;
+}
+double AigamozuControlPackets::get_agzCov_longi(){
+ return agzCov.y;
+}
+
+double AigamozuControlPackets::get_basePointKalman_lati(int i){
+ return basePointKalman[i].x;
+ }
+double AigamozuControlPackets::get_basePointKalman_longi(int i){
+ return basePointKalman[i].y;
+ }
+//////////////////////////////
+// set関数
+//////////////////////////////
+void AigamozuControlPackets::set_agzCov(double cov_lati,double cov_longi){
+ agzCov.x = cov_lati;
+ agzCov.y = cov_longi;
+}
+
+
+void AigamozuControlPackets::set_agzAutoGPS(){
+ agzPoint.x += 0.001;
+ //agzPoint.y += 0.001;
+}
+
+void AigamozuControlPackets::set_agzKalmanGPS(){
+ agzPointKalman.x += 0.001;
+ //agzPointKalman.y += 0.001;
+
+}
+
+//////////////////////////////
+// Controller/Base -> Robot //
+//////////////////////////////
+
+ void AigamozuControlPackets::createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR)
+ {
+ uint8_t tmp[] = {'A','G','S','M','F',fromID,'T',toID,'L',directionL,pwmL,'R',directionR,pwmR,'A','G','E'};
+ for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
+ packetLength = RECEIVE_STATUS_COMMNAD_LENGTH;
+ }
+
+void AigamozuControlPackets::createRequestCommand(uint8_t fromID,uint8_t toID)
+{
+ uint8_t tmp[] = {'A','G','S','S','F',fromID,'T',toID,'A','G','E'};
+ for(int i = 0; i < REQUEST_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
+ packetLength = REQUEST_COMMNAD_LENGTH;
+ }
+
+void AigamozuControlPackets::createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,MODE mode){
+ uint8_t tmp[] = {'A','G','S','C','F',fromID,'T',toID,mode,'A','G','E'};
+ for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; ++i) packetData[i] = tmp[i];
+ packetLength = CHANGE_MODE_COMMAND_LENGTH;
+ }
+
+
+//////////////////////////////
+// Robot -> Controller/Base //
+//////////////////////////////
+
+void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID,uint8_t toID,double latitude,double longitude,double latitudeKalman,double longitudeKalman,double covarLati,double covarLongi)
+{
+ UNION_double_char latitude_data,longitude_data, latitudeKalman_data, longitudeKalman_data;
+ UNION_double_char covarLati_data,covarLongi_data;
+
+ latitude_data.double_value=latitude;
+ longitude_data.double_value=longitude;
+ longitude_data.double_value=latitudeKalman;
+ longitudeKalman_data.double_value=longitudeKalman;
+ covarLati_data.double_value=covarLati;
+ covarLongi_data.double_value=covarLongi;
+
+ uint8_t tmp[] = {'A','G','S','R','F',fromID,'T',toID,'S',nowMode,71,80,83,
+ latitude_data.char_value[0],latitude_data.char_value[1],latitude_data.char_value[2],latitude_data.char_value[3],latitude_data.char_value[4],latitude_data.char_value[5],latitude_data.char_value[6],latitude_data.char_value[7],
+ longitude_data.char_value[0],longitude_data.char_value[1],longitude_data.char_value[2],longitude_data.char_value[3],longitude_data.char_value[4],longitude_data.char_value[5],longitude_data.char_value[6],longitude_data.char_value[7],
+ latitudeKalman_data.char_value[0],latitudeKalman_data.char_value[1],latitudeKalman_data.char_value[2],latitudeKalman_data.char_value[3],latitudeKalman_data.char_value[4],latitudeKalman_data.char_value[5],latitudeKalman_data.char_value[6],latitudeKalman_data.char_value[7],
+ longitudeKalman_data.char_value[0],longitudeKalman_data.char_value[1],longitudeKalman_data.char_value[2],longitudeKalman_data.char_value[3],longitudeKalman_data.char_value[4],longitudeKalman_data.char_value[5],longitudeKalman_data.char_value[6],longitudeKalman_data.char_value[7],
+ covarLati_data.char_value[0],covarLati_data.char_value[1],covarLati_data.char_value[2],covarLati_data.char_value[3],covarLati_data.char_value[4],covarLati_data.char_value[5],covarLati_data.char_value[6],covarLati_data.char_value[7],
+ covarLongi_data.char_value[0],covarLongi_data.char_value[1],covarLongi_data.char_value[2],covarLongi_data.char_value[3],covarLongi_data.char_value[4],covarLongi_data.char_value[5],covarLongi_data.char_value[6],covarLongi_data.char_value[7],
+ 65,71,69};
+ for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
+ packetLength = RECEIVE_STATUS_COMMNAD_LENGTH;
+}
+
+
+
+//////////////////////////////
+// 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]);
+ }
+ }
+
+//////////////////////////////
+// each mode interrupt //
+//////////////////////////////
+
+void AigamozuControlPackets::manualMode(){
+ manualCount++;
+ if(manualCount > 10){
+ _agzSheild.changeSpeed(0,0,0,0);
+ manualCount = 0;
+ }
+ }
+
+
+void AigamozuControlPackets::randomAuto(){
+
+ randomCount++;
+
+ if(randomCount < 10) _agzSheild.changeSpeed(1,128,1,128);
+ else if(randomCount < 11) _agzSheild.changeSpeed(2,128,2,128);
+ else randomCount = 0;
+
+ }
+
+bool AigamozuControlPackets::gpsAuto(){
+
+ /*
+ _agzSheild.changeSpeed(2,128,2,128):for moving robot
+ */
+
+ Timer Automove_Timer;
+
+ bool out_flag = true;
+ static bool out_count_flag = false;
+
+ if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)){
+ out_flag = false;
+ out_count_flag = false;
+ }else if(AigamozuControlPackets::checkGpsHit(basePointKalman[2],basePointKalman[3],basePointKalman[0],agzPointKalman)){
+ out_flag = false;
+ out_count_flag = false;
+ }else{//if robot is out
+ out_flag = true;
+ }
+
+ //if robot is out:
+ if(out_flag == true || out_count_flag == false){
+ Automove_Timer.reset();
+ out_count_flag = true;
+ }
+ if(out_flag){
+ if(Automove_Timer.read_ms() < 5000){
+ _agzSheild.changeSpeed(2,64,2,64);//back
+ }else if(Automove_Timer.read_ms() < 6000){
+ _agzSheild.changeSpeed(1,64,2,64);//straight
+ }else if(Automove_Timer.read_ms() >= 6000){
+ out_count_flag=false;
+ out_flag=false;
+ }
+ }else{
+ //if robot is inner
+ _agzSheild.changeSpeed(1,64,1,64);//straight
+ }
+
+ return out_flag;
+
+}
+
+
+
+
+
+//Update Robot Point
+void AigamozuControlPackets::reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
+ agzPoint.x = (double)latitudeH + (double)(latitudeL / 1000000.0/60.0);
+ agzPoint.y = (double)longitudeH +(double)(longitudeL / 1000000.0/60.0);
+
+ }
+
+//Updata Base Point
+void AigamozuControlPackets::reNewBasePoint(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL){
+ basePoint[id].x = (double)latitudeH + (double)(latitudeL / 1000000.0);
+ basePoint[id].y = (double)longitudeH +(double)(longitudeL / 1000000.0);
+}
+
+//Update Robot Point
+void AigamozuControlPackets::reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
+ agzPointKalman.x = (double)latitudeH + (double)(latitudeL / 1000000.0/60.0);
+ agzPointKalman.y = (double)longitudeH +(double)(longitudeL / 1000000.0/60.0);
+
+ }
+
+//Updata Base Point
+void AigamozuControlPackets::reNewBasePointKalman(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL){
+ basePointKalman[id].x = (double)latitudeH + (double)(latitudeL / 1000000.0);
+ basePointKalman[id].y = (double)longitudeH +(double)(longitudeL / 1000000.0);
+}
+
+
+//Check Hit Point Area
+bool AigamozuControlPackets::checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P){
+ vector2D AB = AigamozuControlPackets::sub_vector(B, A);
+ vector2D BP = AigamozuControlPackets::sub_vector(P, B);
+
+ vector2D BC = AigamozuControlPackets::sub_vector(C, B);
+ vector2D CP = AigamozuControlPackets::sub_vector(P, C);
+
+ vector2D CA = AigamozuControlPackets::sub_vector(A, C);
+ vector2D AP = AigamozuControlPackets::sub_vector(P, A);
+
+ double c1 = AB.x * BP.y - AB.y * BP.x;
+ double c2 = BC.x * CP.y - BC.y * CP.x;
+ double c3 = CA.x * AP.y - CA.y * AP.x;
+
+ if( ( c1 > 0 && c2 > 0 && c3 > 0 ) || ( c1 < 0 && c2 < 0 && c3 < 0 ) ) {
+ return true;
+ }
+
+ return false;
+
+ }
+
+vector2D AigamozuControlPackets::sub_vector( const vector2D& a, const vector2D& b )
+{
+ vector2D ret;
+ ret.x = a.x - b.x;
+ ret.y = a.y - b.y;
+ return ret;
+}
+
+
+
+
+//////////////////////////////
+// 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_MODE;
+ //eachModeInt.attach(this,&AigamozuControlPackets::randomAuto,1.0);
+// eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,2.0);
+ nowMode = AUTO_GPS_MODE;
+ break;
+
+ case 3:
+ //eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,0.2);
+ nowMode = AUTO_GPS_MODE;
+ break;
+
+ default:
+ nowMode = STANDBY_MODE;
+ break;
+
+ }
+ return false;
+}
+
+
+