Diff: AigamozuControlPackets.cpp

Revision:

0:806e9a434e81

Child:

1:6d7c854c927d

diff -r 000000000000 -r 806e9a434e81 AigamozuControlPackets.cpp
--- /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;
+}
+
+
+