加速度・ジャイロセンサのセッティング
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; }