Mami Yokokawa / setting

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

Dependents:   Aigamozu_Robot_March

setting.cpp@3:00c23c8ff4d8, 2016-02-16 (annotated)

Committer:
s1200058
Date:
Tue Feb 16 09:38:53 2016 +0000
Revision:
3:00c23c8ff4d8
Parent:
2:b03a80d9eade 
for calibration compass;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1200058 0:2299c333a9b4 1 #include "setting.h"
s1200058 0:2299c333a9b4 2
s1200058 0:2299c333a9b4 3 void setting::setGpsPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
s1200058 0:2299c333a9b4 4
s1200058 0:2299c333a9b4 5 if(changeFlag != 0){
s1200058 0:2299c333a9b4 6 pastGpsPoint.x = gpsPoint.x;
s1200058 0:2299c333a9b4 7 pastGpsPoint.y = gpsPoint.y;
s1200058 0:2299c333a9b4 8
s1200058 0:2299c333a9b4 9 gpsPoint.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0);
s1200058 0:2299c333a9b4 10 gpsPoint.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0);
s1200058 0:2299c333a9b4 11
s1200058 0:2299c333a9b4 12 setGpsDistance();
s1200058 0:2299c333a9b4 13 // flag++;
s1200058 0:2299c333a9b4 14 }
s1200058 0:2299c333a9b4 15 else{
s1200058 0:2299c333a9b4 16 // flag = 1;
s1200058 0:2299c333a9b4 17 gpsPoint.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0);
s1200058 0:2299c333a9b4 18 gpsPoint.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0);
s1200058 0:2299c333a9b4 19 changeFlag = 1;
s1200058 0:2299c333a9b4 20 }
s1200058 0:2299c333a9b4 21
s1200058 0:2299c333a9b4 22
s1200058 0:2299c333a9b4 23 }
s1200058 0:2299c333a9b4 24
s1200058 0:2299c333a9b4 25 void setting::setGpsDistance(void){
s1200058 0:2299c333a9b4 26
s1200058 1:b2263c93d1d4 27 gpsDis.x = gpsPoint.x - pastGpsPoint.x;
s1200058 1:b2263c93d1d4 28 gpsDis.y = gpsPoint.y - pastGpsPoint.y;
s1200058 0:2299c333a9b4 29
s1200058 0:2299c333a9b4 30 }
s1200058 0:2299c333a9b4 31
s1200058 0:2299c333a9b4 32 void setting::setAngle(double angle){
s1200058 0:2299c333a9b4 33
s1200058 3:00c23c8ff4d8 34
s1200058 0:2299c333a9b4 35 gpsAngle = angle;
s1200058 0:2299c333a9b4 36
s1200058 0:2299c333a9b4 37 }
s1200058 0:2299c333a9b4 38
s1200058 0:2299c333a9b4 39 void setting::setCmp(int16_t x, int16_t y, int16_t z){
s1200058 0:2299c333a9b4 40
s1200058 0:2299c333a9b4 41 /* agzCmp.x = x;
s1200058 0:2299c333a9b4 42 agzCmp.y = y;
s1200058 0:2299c333a9b4 43 agzCmp.z = z;
s1200058 0:2299c333a9b4 44
s1200058 0:2299c333a9b4 45 if(flag == 1){
s1200058 0:2299c333a9b4 46 minCmp.x = x; maxCmp.x = x;
s1200058 0:2299c333a9b4 47 minCmp.y = y; maxCmp.y = y;
s1200058 0:2299c333a9b4 48 minCmp.z = z; maxCmp.z = z;
s1200058 0:2299c333a9b4 49 }
s1200058 0:2299c333a9b4 50 else{
s1200058 0:2299c333a9b4 51 if(minCmp.x > x){
s1200058 0:2299c333a9b4 52 minCmp.x = x;
s1200058 0:2299c333a9b4 53 }
s1200058 0:2299c333a9b4 54 else if(maxCmp.x < x){
s1200058 0:2299c333a9b4 55 maxCmp.x = x;
s1200058 0:2299c333a9b4 56 }
s1200058 0:2299c333a9b4 57 if(minCmp.y > y){
s1200058 0:2299c333a9b4 58 minCmp.y = y;
s1200058 0:2299c333a9b4 59 }
s1200058 0:2299c333a9b4 60 else if(maxCmp.y < y){
s1200058 0:2299c333a9b4 61 maxCmp.y = y;
s1200058 0:2299c333a9b4 62 }
s1200058 0:2299c333a9b4 63 if(minCmp.z > z){
s1200058 0:2299c333a9b4 64 minCmp.z = z;
s1200058 0:2299c333a9b4 65 }
s1200058 0:2299c333a9b4 66 else if(maxCmp.z < z){
s1200058 0:2299c333a9b4 67 maxCmp.z = z;
s1200058 0:2299c333a9b4 68 }
s1200058 0:2299c333a9b4 69 }
s1200058 0:2299c333a9b4 70
s1200058 0:2299c333a9b4 71 */
s1200058 0:2299c333a9b4 72
s1200058 3:00c23c8ff4d8 73 /* if(flag <= 20){
s1200058 0:2299c333a9b4 74 caribCmp.x += x;
s1200058 0:2299c333a9b4 75 caribCmp.y += y;
s1200058 0:2299c333a9b4 76 caribCmp.z += z;
s1200058 3:00c23c8ff4d8 77 */
s1200058 0:2299c333a9b4 78 agzCmp.x = x;
s1200058 0:2299c333a9b4 79 agzCmp.y = y;
s1200058 0:2299c333a9b4 80 agzCmp.z = z;
s1200058 3:00c23c8ff4d8 81 /* }
s1200058 0:2299c333a9b4 82
s1200058 0:2299c333a9b4 83 else{
s1200058 0:2299c333a9b4 84
s1200058 0:2299c333a9b4 85 agzCmp.x = x - caribCmp.x;
s1200058 0:2299c333a9b4 86 agzCmp.y = y - caribCmp.y;
s1200058 0:2299c333a9b4 87 agzCmp.z = z - caribCmp.z;
s1200058 0:2299c333a9b4 88
s1200058 0:2299c333a9b4 89 }
s1200058 0:2299c333a9b4 90
s1200058 0:2299c333a9b4 91 if(flag == 20){
s1200058 0:2299c333a9b4 92 caribCmp.x /= 20;
s1200058 0:2299c333a9b4 93 caribCmp.y /= 20;
s1200058 0:2299c333a9b4 94 caribCmp.z /= 20;
s1200058 0:2299c333a9b4 95 }
s1200058 3:00c23c8ff4d8 96 */
s1200058 0:2299c333a9b4 97
s1200058 0:2299c333a9b4 98 }
s1200058 0:2299c333a9b4 99
s1200058 0:2299c333a9b4 100 void setting::setAcc(int16_t x, int16_t y, int16_t z){
s1200058 0:2299c333a9b4 101
s1200058 0:2299c333a9b4 102 /* agzAcc.x = x;
s1200058 0:2299c333a9b4 103 agzAcc.y = y;
s1200058 0:2299c333a9b4 104 agzAcc.z = z;
s1200058 0:2299c333a9b4 105
s1200058 0:2299c333a9b4 106 if(flag == 1){
s1200058 0:2299c333a9b4 107 minAcc.x = x; maxAcc.x = x;
s1200058 0:2299c333a9b4 108 minAcc.y = y; maxAcc.y = y;
s1200058 0:2299c333a9b4 109 minAcc.z = z; maxAcc.z = z;
s1200058 0:2299c333a9b4 110 }
s1200058 0:2299c333a9b4 111 else{
s1200058 0:2299c333a9b4 112 if(minAcc.x > x){
s1200058 0:2299c333a9b4 113 minAcc.x = x;
s1200058 0:2299c333a9b4 114 }
s1200058 0:2299c333a9b4 115 else if(maxAcc.x < x){
s1200058 0:2299c333a9b4 116 maxAcc.x = x;
s1200058 0:2299c333a9b4 117 }
s1200058 0:2299c333a9b4 118 if(minCmp.y > y){
s1200058 0:2299c333a9b4 119 minAcc.y = y;
s1200058 0:2299c333a9b4 120 }
s1200058 0:2299c333a9b4 121 else if(maxAcc.y < y){
s1200058 0:2299c333a9b4 122 maxAcc.y = y;
s1200058 0:2299c333a9b4 123 }
s1200058 0:2299c333a9b4 124 if(minAcc.z > z){
s1200058 0:2299c333a9b4 125 minAcc.z = z;
s1200058 0:2299c333a9b4 126 }
s1200058 0:2299c333a9b4 127 else if(maxAcc.z < z){
s1200058 0:2299c333a9b4 128 maxAcc.z = z;
s1200058 0:2299c333a9b4 129 }
s1200058 0:2299c333a9b4 130 }
s1200058 0:2299c333a9b4 131 */
s1200058 0:2299c333a9b4 132 agzAcc.x = x;
s1200058 0:2299c333a9b4 133 agzAcc.y = y;
s1200058 0:2299c333a9b4 134 agzAcc.z = z;
s1200058 0:2299c333a9b4 135
s1200058 0:2299c333a9b4 136
s1200058 0:2299c333a9b4 137 }
s1200058 0:2299c333a9b4 138
s1200058 0:2299c333a9b4 139 void setting::setGyr(int16_t x, int16_t y, int16_t z){
s1200058 0:2299c333a9b4 140
s1200058 0:2299c333a9b4 141 /* agzGyr.x = x;
s1200058 0:2299c333a9b4 142 agzGyr.y = y;
s1200058 0:2299c333a9b4 143 agzGyr.z = z;
s1200058 0:2299c333a9b4 144
s1200058 0:2299c333a9b4 145 if(flag == 1){
s1200058 0:2299c333a9b4 146 minGyr.x = x; maxGyr.x = x;
s1200058 0:2299c333a9b4 147 minGyr.y = y; maxGyr.y = y;
s1200058 0:2299c333a9b4 148 minGyr.z = z; maxGyr.z = z;
s1200058 0:2299c333a9b4 149 }
s1200058 0:2299c333a9b4 150 else{
s1200058 0:2299c333a9b4 151 if(minGyr.x > x){
s1200058 0:2299c333a9b4 152 minGyr.x = x;
s1200058 0:2299c333a9b4 153 }
s1200058 0:2299c333a9b4 154 else if(maxGyr.x < x){
s1200058 0:2299c333a9b4 155 maxGyr.x = x;
s1200058 0:2299c333a9b4 156 }
s1200058 0:2299c333a9b4 157 if(minGyr.y > y){
s1200058 0:2299c333a9b4 158 minGyr.y = y;
s1200058 0:2299c333a9b4 159 }
s1200058 0:2299c333a9b4 160 else if(maxGyr.y < y){
s1200058 0:2299c333a9b4 161 maxGyr.y = y;
s1200058 0:2299c333a9b4 162 }
s1200058 0:2299c333a9b4 163 if(minGyr.z > z){
s1200058 0:2299c333a9b4 164 minGyr.z = z;
s1200058 0:2299c333a9b4 165 }
s1200058 0:2299c333a9b4 166 else if(maxGyr.z < z){
s1200058 0:2299c333a9b4 167 maxGyr.z = z;
s1200058 0:2299c333a9b4 168 }
s1200058 0:2299c333a9b4 169 }
s1200058 0:2299c333a9b4 170 */
s1200058 3:00c23c8ff4d8 171 /* if(flag <= 20){
s1200058 0:2299c333a9b4 172 caribGyr.x += x;
s1200058 0:2299c333a9b4 173 caribGyr.y += y;
s1200058 0:2299c333a9b4 174 caribGyr.z += z;
s1200058 0:2299c333a9b4 175
s1200058 0:2299c333a9b4 176 agzGyr.x = x;
s1200058 0:2299c333a9b4 177 agzGyr.y = y;
s1200058 0:2299c333a9b4 178 agzGyr.z = z;
s1200058 0:2299c333a9b4 179 }
s1200058 0:2299c333a9b4 180
s1200058 0:2299c333a9b4 181 else{
s1200058 0:2299c333a9b4 182
s1200058 0:2299c333a9b4 183 pastGyr.x = agzGyr.x;
s1200058 0:2299c333a9b4 184 pastGyr.y = agzGyr.y;
s1200058 0:2299c333a9b4 185 pastGyr.z = agzGyr.z;
s1200058 0:2299c333a9b4 186
s1200058 0:2299c333a9b4 187 agzGyr.x = (x - caribGyr.x);
s1200058 0:2299c333a9b4 188 agzGyr.y = (y - caribGyr.y);
s1200058 0:2299c333a9b4 189 agzGyr.z = (z - caribGyr.z);
s1200058 0:2299c333a9b4 190
s1200058 0:2299c333a9b4 191 }
s1200058 0:2299c333a9b4 192
s1200058 0:2299c333a9b4 193 if(flag == 20){
s1200058 0:2299c333a9b4 194 caribGyr.x /= 20;
s1200058 0:2299c333a9b4 195 caribGyr.y /= 20;
s1200058 0:2299c333a9b4 196 caribGyr.z /= 20;
s1200058 0:2299c333a9b4 197 }
s1200058 3:00c23c8ff4d8 198 */
s1200058 3:00c23c8ff4d8 199 agzGyr.x = x;
s1200058 3:00c23c8ff4d8 200 agzGyr.y = y;
s1200058 3:00c23c8ff4d8 201 agzGyr.z = z;
s1200058 3:00c23c8ff4d8 202
s1200058 0:2299c333a9b4 203 }