Mami Yokokawa / setting

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

Dependents:   Aigamozu_Robot_March

setting.cpp@0:2299c333a9b4, 2016-01-20 (annotated)

Committer:
s1200058
Date:
Wed Jan 20 15:48:38 2016 +0000
Revision:
0:2299c333a9b4
Child:
1:b2263c93d1d4
ver1;

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 0:2299c333a9b4 27 gpsDis.x = (gpsPoint.x - pastGpsPoint.x) * 111120;
s1200058 0:2299c333a9b4 28 gpsDis.y = (gpsPoint.y - pastGpsPoint.y) * dis * 1000;
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 0:2299c333a9b4 34 if(changeFlag != 0){
s1200058 0:2299c333a9b4 35 pastGpsAngle = gpsAngle;
s1200058 0:2299c333a9b4 36 gpsAngle = angle;
s1200058 0:2299c333a9b4 37 difAngle = gpsAngle - pastGpsAngle;
s1200058 0:2299c333a9b4 38 }
s1200058 0:2299c333a9b4 39 else{
s1200058 0:2299c333a9b4 40 gpsAngle = angle;
s1200058 0:2299c333a9b4 41 }
s1200058 0:2299c333a9b4 42
s1200058 0:2299c333a9b4 43 }
s1200058 0:2299c333a9b4 44
s1200058 0:2299c333a9b4 45 void setting::setCmp(int16_t x, int16_t y, int16_t z){
s1200058 0:2299c333a9b4 46
s1200058 0:2299c333a9b4 47 /* agzCmp.x = x;
s1200058 0:2299c333a9b4 48 agzCmp.y = y;
s1200058 0:2299c333a9b4 49 agzCmp.z = z;
s1200058 0:2299c333a9b4 50
s1200058 0:2299c333a9b4 51 if(flag == 1){
s1200058 0:2299c333a9b4 52 minCmp.x = x; maxCmp.x = x;
s1200058 0:2299c333a9b4 53 minCmp.y = y; maxCmp.y = y;
s1200058 0:2299c333a9b4 54 minCmp.z = z; maxCmp.z = z;
s1200058 0:2299c333a9b4 55 }
s1200058 0:2299c333a9b4 56 else{
s1200058 0:2299c333a9b4 57 if(minCmp.x > x){
s1200058 0:2299c333a9b4 58 minCmp.x = x;
s1200058 0:2299c333a9b4 59 }
s1200058 0:2299c333a9b4 60 else if(maxCmp.x < x){
s1200058 0:2299c333a9b4 61 maxCmp.x = x;
s1200058 0:2299c333a9b4 62 }
s1200058 0:2299c333a9b4 63 if(minCmp.y > y){
s1200058 0:2299c333a9b4 64 minCmp.y = y;
s1200058 0:2299c333a9b4 65 }
s1200058 0:2299c333a9b4 66 else if(maxCmp.y < y){
s1200058 0:2299c333a9b4 67 maxCmp.y = y;
s1200058 0:2299c333a9b4 68 }
s1200058 0:2299c333a9b4 69 if(minCmp.z > z){
s1200058 0:2299c333a9b4 70 minCmp.z = z;
s1200058 0:2299c333a9b4 71 }
s1200058 0:2299c333a9b4 72 else if(maxCmp.z < z){
s1200058 0:2299c333a9b4 73 maxCmp.z = z;
s1200058 0:2299c333a9b4 74 }
s1200058 0:2299c333a9b4 75 }
s1200058 0:2299c333a9b4 76
s1200058 0:2299c333a9b4 77 */
s1200058 0:2299c333a9b4 78
s1200058 0:2299c333a9b4 79 if(flag <= 20){
s1200058 0:2299c333a9b4 80 caribCmp.x += x;
s1200058 0:2299c333a9b4 81 caribCmp.y += y;
s1200058 0:2299c333a9b4 82 caribCmp.z += z;
s1200058 0:2299c333a9b4 83
s1200058 0:2299c333a9b4 84 agzCmp.x = x;
s1200058 0:2299c333a9b4 85 agzCmp.y = y;
s1200058 0:2299c333a9b4 86 agzCmp.z = z;
s1200058 0:2299c333a9b4 87 }
s1200058 0:2299c333a9b4 88
s1200058 0:2299c333a9b4 89 else{
s1200058 0:2299c333a9b4 90
s1200058 0:2299c333a9b4 91 agzCmp.x = x - caribCmp.x;
s1200058 0:2299c333a9b4 92 agzCmp.y = y - caribCmp.y;
s1200058 0:2299c333a9b4 93 agzCmp.z = z - caribCmp.z;
s1200058 0:2299c333a9b4 94
s1200058 0:2299c333a9b4 95 }
s1200058 0:2299c333a9b4 96
s1200058 0:2299c333a9b4 97 if(flag == 20){
s1200058 0:2299c333a9b4 98 caribCmp.x /= 20;
s1200058 0:2299c333a9b4 99 caribCmp.y /= 20;
s1200058 0:2299c333a9b4 100 caribCmp.z /= 20;
s1200058 0:2299c333a9b4 101 }
s1200058 0:2299c333a9b4 102
s1200058 0:2299c333a9b4 103
s1200058 0:2299c333a9b4 104 }
s1200058 0:2299c333a9b4 105
s1200058 0:2299c333a9b4 106 void setting::setAcc(int16_t x, int16_t y, int16_t z){
s1200058 0:2299c333a9b4 107
s1200058 0:2299c333a9b4 108 /* agzAcc.x = x;
s1200058 0:2299c333a9b4 109 agzAcc.y = y;
s1200058 0:2299c333a9b4 110 agzAcc.z = z;
s1200058 0:2299c333a9b4 111
s1200058 0:2299c333a9b4 112 if(flag == 1){
s1200058 0:2299c333a9b4 113 minAcc.x = x; maxAcc.x = x;
s1200058 0:2299c333a9b4 114 minAcc.y = y; maxAcc.y = y;
s1200058 0:2299c333a9b4 115 minAcc.z = z; maxAcc.z = z;
s1200058 0:2299c333a9b4 116 }
s1200058 0:2299c333a9b4 117 else{
s1200058 0:2299c333a9b4 118 if(minAcc.x > x){
s1200058 0:2299c333a9b4 119 minAcc.x = x;
s1200058 0:2299c333a9b4 120 }
s1200058 0:2299c333a9b4 121 else if(maxAcc.x < x){
s1200058 0:2299c333a9b4 122 maxAcc.x = x;
s1200058 0:2299c333a9b4 123 }
s1200058 0:2299c333a9b4 124 if(minCmp.y > y){
s1200058 0:2299c333a9b4 125 minAcc.y = y;
s1200058 0:2299c333a9b4 126 }
s1200058 0:2299c333a9b4 127 else if(maxAcc.y < y){
s1200058 0:2299c333a9b4 128 maxAcc.y = y;
s1200058 0:2299c333a9b4 129 }
s1200058 0:2299c333a9b4 130 if(minAcc.z > z){
s1200058 0:2299c333a9b4 131 minAcc.z = z;
s1200058 0:2299c333a9b4 132 }
s1200058 0:2299c333a9b4 133 else if(maxAcc.z < z){
s1200058 0:2299c333a9b4 134 maxAcc.z = z;
s1200058 0:2299c333a9b4 135 }
s1200058 0:2299c333a9b4 136 }
s1200058 0:2299c333a9b4 137 */
s1200058 0:2299c333a9b4 138 if(flag <= 20){
s1200058 0:2299c333a9b4 139 caribAcc.x += x;
s1200058 0:2299c333a9b4 140 caribAcc.y += y;
s1200058 0:2299c333a9b4 141 caribAcc.z += z;
s1200058 0:2299c333a9b4 142
s1200058 0:2299c333a9b4 143 agzAcc.x = x;
s1200058 0:2299c333a9b4 144 agzAcc.y = y;
s1200058 0:2299c333a9b4 145 agzAcc.z = z;
s1200058 0:2299c333a9b4 146 }
s1200058 0:2299c333a9b4 147
s1200058 0:2299c333a9b4 148 else{
s1200058 0:2299c333a9b4 149
s1200058 0:2299c333a9b4 150 agzAcc.x = x - caribAcc.x;
s1200058 0:2299c333a9b4 151 agzAcc.y = y - caribAcc.y;
s1200058 0:2299c333a9b4 152 agzAcc.z = z - caribAcc.z;
s1200058 0:2299c333a9b4 153
s1200058 0:2299c333a9b4 154 }
s1200058 0:2299c333a9b4 155
s1200058 0:2299c333a9b4 156 if(flag == 20){
s1200058 0:2299c333a9b4 157 caribAcc.x /= 20;
s1200058 0:2299c333a9b4 158 caribAcc.y /= 20;
s1200058 0:2299c333a9b4 159 caribAcc.z /= 20;
s1200058 0:2299c333a9b4 160
s1200058 0:2299c333a9b4 161 }
s1200058 0:2299c333a9b4 162
s1200058 0:2299c333a9b4 163 }
s1200058 0:2299c333a9b4 164
s1200058 0:2299c333a9b4 165 void setting::setGyr(int16_t x, int16_t y, int16_t z){
s1200058 0:2299c333a9b4 166
s1200058 0:2299c333a9b4 167 /* agzGyr.x = x;
s1200058 0:2299c333a9b4 168 agzGyr.y = y;
s1200058 0:2299c333a9b4 169 agzGyr.z = z;
s1200058 0:2299c333a9b4 170
s1200058 0:2299c333a9b4 171 if(flag == 1){
s1200058 0:2299c333a9b4 172 minGyr.x = x; maxGyr.x = x;
s1200058 0:2299c333a9b4 173 minGyr.y = y; maxGyr.y = y;
s1200058 0:2299c333a9b4 174 minGyr.z = z; maxGyr.z = z;
s1200058 0:2299c333a9b4 175 }
s1200058 0:2299c333a9b4 176 else{
s1200058 0:2299c333a9b4 177 if(minGyr.x > x){
s1200058 0:2299c333a9b4 178 minGyr.x = x;
s1200058 0:2299c333a9b4 179 }
s1200058 0:2299c333a9b4 180 else if(maxGyr.x < x){
s1200058 0:2299c333a9b4 181 maxGyr.x = x;
s1200058 0:2299c333a9b4 182 }
s1200058 0:2299c333a9b4 183 if(minGyr.y > y){
s1200058 0:2299c333a9b4 184 minGyr.y = y;
s1200058 0:2299c333a9b4 185 }
s1200058 0:2299c333a9b4 186 else if(maxGyr.y < y){
s1200058 0:2299c333a9b4 187 maxGyr.y = y;
s1200058 0:2299c333a9b4 188 }
s1200058 0:2299c333a9b4 189 if(minGyr.z > z){
s1200058 0:2299c333a9b4 190 minGyr.z = z;
s1200058 0:2299c333a9b4 191 }
s1200058 0:2299c333a9b4 192 else if(maxGyr.z < z){
s1200058 0:2299c333a9b4 193 maxGyr.z = z;
s1200058 0:2299c333a9b4 194 }
s1200058 0:2299c333a9b4 195 }
s1200058 0:2299c333a9b4 196 */
s1200058 0:2299c333a9b4 197 if(flag <= 20){
s1200058 0:2299c333a9b4 198 caribGyr.x += x;
s1200058 0:2299c333a9b4 199 caribGyr.y += y;
s1200058 0:2299c333a9b4 200 caribGyr.z += z;
s1200058 0:2299c333a9b4 201
s1200058 0:2299c333a9b4 202 agzGyr.x = x;
s1200058 0:2299c333a9b4 203 agzGyr.y = y;
s1200058 0:2299c333a9b4 204 agzGyr.z = z;
s1200058 0:2299c333a9b4 205 }
s1200058 0:2299c333a9b4 206
s1200058 0:2299c333a9b4 207 else{
s1200058 0:2299c333a9b4 208
s1200058 0:2299c333a9b4 209 pastGyr.x = agzGyr.x;
s1200058 0:2299c333a9b4 210 pastGyr.y = agzGyr.y;
s1200058 0:2299c333a9b4 211 pastGyr.z = agzGyr.z;
s1200058 0:2299c333a9b4 212
s1200058 0:2299c333a9b4 213 agzGyr.x = (x - caribGyr.x);
s1200058 0:2299c333a9b4 214 agzGyr.y = (y - caribGyr.y);
s1200058 0:2299c333a9b4 215 agzGyr.z = (z - caribGyr.z);
s1200058 0:2299c333a9b4 216
s1200058 0:2299c333a9b4 217 }
s1200058 0:2299c333a9b4 218
s1200058 0:2299c333a9b4 219 if(flag == 20){
s1200058 0:2299c333a9b4 220 caribGyr.x /= 20;
s1200058 0:2299c333a9b4 221 caribGyr.y /= 20;
s1200058 0:2299c333a9b4 222 caribGyr.z /= 20;
s1200058 0:2299c333a9b4 223 }
s1200058 0:2299c333a9b4 224
s1200058 0:2299c333a9b4 225 }