加速度・ジャイロセンサのセッティング

Dependents:   Aigamozu_Robot_March

setting.cpp

Committer:
s1200058
Date:
2016-02-16
Revision:
3:00c23c8ff4d8
Parent:
2:b03a80d9eade

File content as of revision 3:00c23c8ff4d8:

#include "setting.h"

void setting::setGpsPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
    
    if(changeFlag != 0){
        pastGpsPoint.x = gpsPoint.x;
        pastGpsPoint.y = gpsPoint.y;
        
        gpsPoint.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0);
        gpsPoint.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0);
        
        setGpsDistance();
 //       flag++;
    }
    else{
//        flag = 1;
        gpsPoint.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0);
        gpsPoint.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0);
        changeFlag = 1;
    }
   
    
}

void setting::setGpsDistance(void){
    
    gpsDis.x = gpsPoint.x - pastGpsPoint.x;
    gpsDis.y = gpsPoint.y - pastGpsPoint.y;
    
}

void setting::setAngle(double angle){
    

        gpsAngle = angle;
    
}

void setting::setCmp(int16_t x, int16_t y, int16_t z){
    
/*        agzCmp.x = x;
        agzCmp.y = y;
        agzCmp.z = z;

        if(flag == 1){
            minCmp.x = x; maxCmp.x = x;
            minCmp.y = y; maxCmp.y = y;
            minCmp.z = z; maxCmp.z = z;
        }
        else{
            if(minCmp.x > x){
                minCmp.x = x;
            }
            else if(maxCmp.x < x){
                maxCmp.x = x;
            }
            if(minCmp.y > y){
                minCmp.y = y;
            }
            else if(maxCmp.y < y){
                maxCmp.y = y;
            }
            if(minCmp.z > z){
                minCmp.z = z;
            }
            else if(maxCmp.z < z){
                maxCmp.z = z;
            }
        }

*/
    
/*   if(flag <= 20){
        caribCmp.x += x;
        caribCmp.y += y;
        caribCmp.z += z;
 */       
        agzCmp.x = x;
        agzCmp.y = y;
        agzCmp.z = z;
/*    }
    
    else{
        
        agzCmp.x = x - caribCmp.x;
        agzCmp.y = y - caribCmp.y;
        agzCmp.z = z - caribCmp.z;
  
    }
    
    if(flag == 20){
        caribCmp.x /= 20;
        caribCmp.y /= 20;
        caribCmp.z /= 20;
   } 
 */   
    
}

void setting::setAcc(int16_t x, int16_t y, int16_t z){
    
/*    agzAcc.x = x;
    agzAcc.y = y;
    agzAcc.z = z;

    if(flag == 1){
        minAcc.x = x; maxAcc.x = x;
        minAcc.y = y; maxAcc.y = y;
        minAcc.z = z; maxAcc.z = z;
    }
    else{
        if(minAcc.x > x){
            minAcc.x = x;
        }
        else if(maxAcc.x < x){
            maxAcc.x = x;
        }
        if(minCmp.y > y){
            minAcc.y = y;
        }
        else if(maxAcc.y < y){
            maxAcc.y = y;
        }
        if(minAcc.z > z){
            minAcc.z = z;
        }
        else if(maxAcc.z < z){
            maxAcc.z = z;
        }
    }
*/
        agzAcc.x = x;
        agzAcc.y = y;
        agzAcc.z = z;

    
}

void setting::setGyr(int16_t x, int16_t y, int16_t z){
    
/*    agzGyr.x = x;
    agzGyr.y = y;
    agzGyr.z = z;

    if(flag == 1){
        minGyr.x = x; maxGyr.x = x;
        minGyr.y = y; maxGyr.y = y;
        minGyr.z = z; maxGyr.z = z;
    }
    else{
        if(minGyr.x > x){
            minGyr.x = x;
        }
        else if(maxGyr.x < x){
            maxGyr.x = x;
        }
        if(minGyr.y > y){
            minGyr.y = y;
        }
        else if(maxGyr.y < y){
            maxGyr.y = y;
        }
        if(minGyr.z > z){
            minGyr.z = z;
        }
        else if(maxGyr.z < z){
            maxGyr.z = z;
        }
    }
*/
/*    if(flag <= 20){
        caribGyr.x += x;
        caribGyr.y += y;
        caribGyr.z += z;
        
        agzGyr.x = x;
        agzGyr.y = y;
        agzGyr.z = z;
    }
    
    else{
        
       pastGyr.x = agzGyr.x;
        pastGyr.y = agzGyr.y;
        pastGyr.z = agzGyr.z;
        
        agzGyr.x = (x - caribGyr.x);
        agzGyr.y = (y - caribGyr.y);
        agzGyr.z = (z - caribGyr.z);
        
    }
    
    if(flag == 20){
       caribGyr.x /= 20;
        caribGyr.y /= 20;
        caribGyr.z /= 20;
   } 
*/  
        agzGyr.x = x;
        agzGyr.y = y;
        agzGyr.z = z;
        
}