Ermis Gasetopulos
Mapping programm, using an minimu9 V3 sensormodule and an USB-library to save the sensorvalues on.
Dependencies: FatFileSystem m3pi_TUB mbed
Fork of
USB-A
by 
Chris Styles
main.cpp@1:c92781bb4d5e, 2016-01-30 (annotated)
- Committer:
- ErmGas
- Date:
- Sat Jan 30 18:56:10 2016 +0000
- Revision:
- 1:c92781bb4d5e
- Parent:
- 0:4e756c4c88a7
Programm to use a minimu9 V3 sensormodule and save its values on an USB-Stick. Additionally an EKF (extended Kalman-Filter) is implemented, but commented because the m3pi works not fast enough to do this on runtime.
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| chris | 0:4e756c4c88a7 | 1 | #include "mbed.h" |
| ErmGas | 1:c92781bb4d5e | 2 | #include "m3pi_ng.h" |
| chris | 0:4e756c4c88a7 | 3 | #include "MSCFileSystem.h" |
| ErmGas | 1:c92781bb4d5e | 4 | #include <string.h> |
| ErmGas | 1:c92781bb4d5e | 5 | #include <fstream> |
| ErmGas | 1:c92781bb4d5e | 6 | //#include <vector> |
| ErmGas | 1:c92781bb4d5e | 7 | #include <sstream> |
| ErmGas | 1:c92781bb4d5e | 8 | //#include <algorithm> |
| ErmGas | 1:c92781bb4d5e | 9 | //#include <functional> |
| ErmGas | 1:c92781bb4d5e | 10 | #include <math.h> |
| chris | 0:4e756c4c88a7 | 11 | |
| ErmGas | 1:c92781bb4d5e | 12 | |
| ErmGas | 1:c92781bb4d5e | 13 | m3pi m3pi; |
| ErmGas | 1:c92781bb4d5e | 14 | MSCFileSystem msc("usb"); |
| ErmGas | 1:c92781bb4d5e | 15 | I2C minimu9V3(p28,p27); |
| ErmGas | 1:c92781bb4d5e | 16 | |
| ErmGas | 1:c92781bb4d5e | 17 | #define pi 3.14159265358979323846 |
| ErmGas | 1:c92781bb4d5e | 18 | /* |
| ErmGas | 1:c92781bb4d5e | 19 | ---------------------------------------------------------------------------------------------------- |
| ErmGas | 1:c92781bb4d5e | 20 | Completely commented out, needed for EKF, but is not working, because of the m3pi's small power |
| ErmGas | 1:c92781bb4d5e | 21 | ---------------------------------------------------------------------------------------------------- |
| ErmGas | 1:c92781bb4d5e | 22 | // Lookuptable with map from txt-file, will be facking slow at runtime |
| ErmGas | 1:c92781bb4d5e | 23 | float Lookuptable(float x_pos, float y_pos, float alpha, int size) |
| ErmGas | 1:c92781bb4d5e | 24 | { ifstream map("/usb/wheelmap.txt"); |
| ErmGas | 1:c92781bb4d5e | 25 | m3pi.locate(0,1); |
| ErmGas | 1:c92781bb4d5e | 26 | m3pi.printf("TestLoTa"); |
| ErmGas | 1:c92781bb4d5e | 27 | float dalpha = pi; |
| ErmGas | 1:c92781bb4d5e | 28 | float POL = -1; |
| ErmGas | 1:c92781bb4d5e | 29 | int counter = 0; |
| ErmGas | 1:c92781bb4d5e | 30 | float alpha_corr = atan2(sin(alpha), cos(alpha)); |
| ErmGas | 1:c92781bb4d5e | 31 | float x_abs, y_abs, dist, dy, dx, alpha_map; |
| ErmGas | 1:c92781bb4d5e | 32 | string ssx_abs, ssy_abs; |
| ErmGas | 1:c92781bb4d5e | 33 | map>>ssx_abs; |
| ErmGas | 1:c92781bb4d5e | 34 | map>>ssy_abs; |
| ErmGas | 1:c92781bb4d5e | 35 | x_abs = std::atof(ssx_abs.c_str()); |
| ErmGas | 1:c92781bb4d5e | 36 | y_abs = std::atof(ssy_abs.c_str()); |
| ErmGas | 1:c92781bb4d5e | 37 | float dist_o = sqrt((x_abs-x_pos)*(x_abs-x_pos) + (y_abs-y_pos)*(y_abs-y_pos)); |
| ErmGas | 1:c92781bb4d5e | 38 | ssx_abs.clear(); |
| ErmGas | 1:c92781bb4d5e | 39 | ssy_abs.clear(); |
| ErmGas | 1:c92781bb4d5e | 40 | while (abs(dalpha)>pi/5 && counter< size){ |
| ErmGas | 1:c92781bb4d5e | 41 | string ssx_abs, ssy_abs; |
| ErmGas | 1:c92781bb4d5e | 42 | map>>ssx_abs; |
| ErmGas | 1:c92781bb4d5e | 43 | map>>ssy_abs; |
| ErmGas | 1:c92781bb4d5e | 44 | x_abs = std::atof(ssx_abs.c_str()); |
| ErmGas | 1:c92781bb4d5e | 45 | y_abs = std::atof(ssy_abs.c_str()); |
| ErmGas | 1:c92781bb4d5e | 46 | dist = sqrt((x_abs-x_pos)*(x_abs-x_pos) + (y_abs-y_pos)*(y_abs-y_pos)); |
| ErmGas | 1:c92781bb4d5e | 47 | // if measurementradius is crossed, compute angle |
| ErmGas | 1:c92781bb4d5e | 48 | if ( (dist >4.05 && dist_o <4.05) || (dist <4.05 && dist_o >4.05)){ |
| ErmGas | 1:c92781bb4d5e | 49 | dx = x_abs-x_pos; |
| ErmGas | 1:c92781bb4d5e | 50 | dy = y_abs-y_pos; |
| ErmGas | 1:c92781bb4d5e | 51 | alpha_map = atan2(dy, dx); |
| ErmGas | 1:c92781bb4d5e | 52 | dalpha = alpha_corr - alpha_map; |
| ErmGas | 1:c92781bb4d5e | 53 | // correct it, if the angle is obtuse |
| ErmGas | 1:c92781bb4d5e | 54 | if (dalpha<-pi){ |
| ErmGas | 1:c92781bb4d5e | 55 | dalpha=2*pi + dalpha; |
| ErmGas | 1:c92781bb4d5e | 56 | } |
| ErmGas | 1:c92781bb4d5e | 57 | else if (dalpha>pi){ |
| ErmGas | 1:c92781bb4d5e | 58 | dalpha = 2*pi - dalpha; |
| ErmGas | 1:c92781bb4d5e | 59 | } |
| ErmGas | 1:c92781bb4d5e | 60 | } |
| ErmGas | 1:c92781bb4d5e | 61 | // if the end of the txt is reached, we need to break out |
| ErmGas | 1:c92781bb4d5e | 62 | counter++; |
| ErmGas | 1:c92781bb4d5e | 63 | m3pi.locate(0,0); |
| ErmGas | 1:c92781bb4d5e | 64 | m3pi.printf("%i",(float)dist); |
| ErmGas | 1:c92781bb4d5e | 65 | ssx_abs.clear(); |
| ErmGas | 1:c92781bb4d5e | 66 | ssy_abs.clear(); |
| ErmGas | 1:c92781bb4d5e | 67 | } |
| ErmGas | 1:c92781bb4d5e | 68 | if (abs(dalpha)<pi/5){POL = std::pow(abs(dalpha*5/pi),2.5) * dalpha/abs(dalpha);} |
| ErmGas | 1:c92781bb4d5e | 69 | map.close(); |
| ErmGas | 1:c92781bb4d5e | 70 | return POL; |
| ErmGas | 1:c92781bb4d5e | 71 | |
| ErmGas | 1:c92781bb4d5e | 72 | |
| ErmGas | 1:c92781bb4d5e | 73 | } |
| ErmGas | 1:c92781bb4d5e | 74 | // 4x4 Matrix inversion |
| ErmGas | 1:c92781bb4d5e | 75 | bool gluInvertMatrix(float M[4][4], float INVOut[4][4]) |
| ErmGas | 1:c92781bb4d5e | 76 | { |
| ErmGas | 1:c92781bb4d5e | 77 | double inv[16], m[16], invOut[16], det; |
| ErmGas | 1:c92781bb4d5e | 78 | // conversion, because too lazy to change the whole code |
| ErmGas | 1:c92781bb4d5e | 79 | for (int i = 0; i<4; i++){ |
| ErmGas | 1:c92781bb4d5e | 80 | for (int j = 0; j<4; j++){ |
| ErmGas | 1:c92781bb4d5e | 81 | m[4*i + j] = M[i][j]; |
| ErmGas | 1:c92781bb4d5e | 82 | } |
| ErmGas | 1:c92781bb4d5e | 83 | } |
| ErmGas | 1:c92781bb4d5e | 84 | int i; |
| ErmGas | 1:c92781bb4d5e | 85 | |
| ErmGas | 1:c92781bb4d5e | 86 | inv[0] = m[5] * m[10] * m[15] - |
| ErmGas | 1:c92781bb4d5e | 87 | m[5] * m[11] * m[14] - |
| ErmGas | 1:c92781bb4d5e | 88 | m[9] * m[6] * m[15] + |
| ErmGas | 1:c92781bb4d5e | 89 | m[9] * m[7] * m[14] + |
| ErmGas | 1:c92781bb4d5e | 90 | m[13] * m[6] * m[11] - |
| ErmGas | 1:c92781bb4d5e | 91 | m[13] * m[7] * m[10]; |
| ErmGas | 1:c92781bb4d5e | 92 | |
| ErmGas | 1:c92781bb4d5e | 93 | inv[4] = -m[4] * m[10] * m[15] + |
| ErmGas | 1:c92781bb4d5e | 94 | m[4] * m[11] * m[14] + |
| ErmGas | 1:c92781bb4d5e | 95 | m[8] * m[6] * m[15] - |
| ErmGas | 1:c92781bb4d5e | 96 | m[8] * m[7] * m[14] - |
| ErmGas | 1:c92781bb4d5e | 97 | m[12] * m[6] * m[11] + |
| ErmGas | 1:c92781bb4d5e | 98 | m[12] * m[7] * m[10]; |
| ErmGas | 1:c92781bb4d5e | 99 | |
| ErmGas | 1:c92781bb4d5e | 100 | inv[8] = m[4] * m[9] * m[15] - |
| ErmGas | 1:c92781bb4d5e | 101 | m[4] * m[11] * m[13] - |
| ErmGas | 1:c92781bb4d5e | 102 | m[8] * m[5] * m[15] + |
| ErmGas | 1:c92781bb4d5e | 103 | m[8] * m[7] * m[13] + |
| ErmGas | 1:c92781bb4d5e | 104 | m[12] * m[5] * m[11] - |
| ErmGas | 1:c92781bb4d5e | 105 | m[12] * m[7] * m[9]; |
| ErmGas | 1:c92781bb4d5e | 106 | |
| ErmGas | 1:c92781bb4d5e | 107 | inv[12] = -m[4] * m[9] * m[14] + |
| ErmGas | 1:c92781bb4d5e | 108 | m[4] * m[10] * m[13] + |
| ErmGas | 1:c92781bb4d5e | 109 | m[8] * m[5] * m[14] - |
| ErmGas | 1:c92781bb4d5e | 110 | m[8] * m[6] * m[13] - |
| ErmGas | 1:c92781bb4d5e | 111 | m[12] * m[5] * m[10] + |
| ErmGas | 1:c92781bb4d5e | 112 | m[12] * m[6] * m[9]; |
| ErmGas | 1:c92781bb4d5e | 113 | |
| ErmGas | 1:c92781bb4d5e | 114 | inv[1] = -m[1] * m[10] * m[15] + |
| ErmGas | 1:c92781bb4d5e | 115 | m[1] * m[11] * m[14] + |
| ErmGas | 1:c92781bb4d5e | 116 | m[9] * m[2] * m[15] - |
| ErmGas | 1:c92781bb4d5e | 117 | m[9] * m[3] * m[14] - |
| ErmGas | 1:c92781bb4d5e | 118 | m[13] * m[2] * m[11] + |
| ErmGas | 1:c92781bb4d5e | 119 | m[13] * m[3] * m[10]; |
| ErmGas | 1:c92781bb4d5e | 120 | |
| ErmGas | 1:c92781bb4d5e | 121 | inv[5] = m[0] * m[10] * m[15] - |
| ErmGas | 1:c92781bb4d5e | 122 | m[0] * m[11] * m[14] - |
| ErmGas | 1:c92781bb4d5e | 123 | m[8] * m[2] * m[15] + |
| ErmGas | 1:c92781bb4d5e | 124 | m[8] * m[3] * m[14] + |
| ErmGas | 1:c92781bb4d5e | 125 | m[12] * m[2] * m[11] - |
| ErmGas | 1:c92781bb4d5e | 126 | m[12] * m[3] * m[10]; |
| ErmGas | 1:c92781bb4d5e | 127 | |
| ErmGas | 1:c92781bb4d5e | 128 | inv[9] = -m[0] * m[9] * m[15] + |
| ErmGas | 1:c92781bb4d5e | 129 | m[0] * m[11] * m[13] + |
| ErmGas | 1:c92781bb4d5e | 130 | m[8] * m[1] * m[15] - |
| ErmGas | 1:c92781bb4d5e | 131 | m[8] * m[3] * m[13] - |
| ErmGas | 1:c92781bb4d5e | 132 | m[12] * m[1] * m[11] + |
| ErmGas | 1:c92781bb4d5e | 133 | m[12] * m[3] * m[9]; |
| ErmGas | 1:c92781bb4d5e | 134 | |
| ErmGas | 1:c92781bb4d5e | 135 | inv[13] = m[0] * m[9] * m[14] - |
| ErmGas | 1:c92781bb4d5e | 136 | m[0] * m[10] * m[13] - |
| ErmGas | 1:c92781bb4d5e | 137 | m[8] * m[1] * m[14] + |
| ErmGas | 1:c92781bb4d5e | 138 | m[8] * m[2] * m[13] + |
| ErmGas | 1:c92781bb4d5e | 139 | m[12] * m[1] * m[10] - |
| ErmGas | 1:c92781bb4d5e | 140 | m[12] * m[2] * m[9]; |
| ErmGas | 1:c92781bb4d5e | 141 | |
| ErmGas | 1:c92781bb4d5e | 142 | inv[2] = m[1] * m[6] * m[15] - |
| ErmGas | 1:c92781bb4d5e | 143 | m[1] * m[7] * m[14] - |
| ErmGas | 1:c92781bb4d5e | 144 | m[5] * m[2] * m[15] + |
| ErmGas | 1:c92781bb4d5e | 145 | m[5] * m[3] * m[14] + |
| ErmGas | 1:c92781bb4d5e | 146 | m[13] * m[2] * m[7] - |
| ErmGas | 1:c92781bb4d5e | 147 | m[13] * m[3] * m[6]; |
| ErmGas | 1:c92781bb4d5e | 148 | |
| ErmGas | 1:c92781bb4d5e | 149 | inv[6] = -m[0] * m[6] * m[15] + |
| ErmGas | 1:c92781bb4d5e | 150 | m[0] * m[7] * m[14] + |
| ErmGas | 1:c92781bb4d5e | 151 | m[4] * m[2] * m[15] - |
| ErmGas | 1:c92781bb4d5e | 152 | m[4] * m[3] * m[14] - |
| ErmGas | 1:c92781bb4d5e | 153 | m[12] * m[2] * m[7] + |
| ErmGas | 1:c92781bb4d5e | 154 | m[12] * m[3] * m[6]; |
| ErmGas | 1:c92781bb4d5e | 155 | |
| ErmGas | 1:c92781bb4d5e | 156 | inv[10] = m[0] * m[5] * m[15] - |
| ErmGas | 1:c92781bb4d5e | 157 | m[0] * m[7] * m[13] - |
| ErmGas | 1:c92781bb4d5e | 158 | m[4] * m[1] * m[15] + |
| ErmGas | 1:c92781bb4d5e | 159 | m[4] * m[3] * m[13] + |
| ErmGas | 1:c92781bb4d5e | 160 | m[12] * m[1] * m[7] - |
| ErmGas | 1:c92781bb4d5e | 161 | m[12] * m[3] * m[5]; |
| ErmGas | 1:c92781bb4d5e | 162 | |
| ErmGas | 1:c92781bb4d5e | 163 | inv[14] = -m[0] * m[5] * m[14] + |
| ErmGas | 1:c92781bb4d5e | 164 | m[0] * m[6] * m[13] + |
| ErmGas | 1:c92781bb4d5e | 165 | m[4] * m[1] * m[14] - |
| ErmGas | 1:c92781bb4d5e | 166 | m[4] * m[2] * m[13] - |
| ErmGas | 1:c92781bb4d5e | 167 | m[12] * m[1] * m[6] + |
| ErmGas | 1:c92781bb4d5e | 168 | m[12] * m[2] * m[5]; |
| ErmGas | 1:c92781bb4d5e | 169 | |
| ErmGas | 1:c92781bb4d5e | 170 | inv[3] = -m[1] * m[6] * m[11] + |
| ErmGas | 1:c92781bb4d5e | 171 | m[1] * m[7] * m[10] + |
| ErmGas | 1:c92781bb4d5e | 172 | m[5] * m[2] * m[11] - |
| ErmGas | 1:c92781bb4d5e | 173 | m[5] * m[3] * m[10] - |
| ErmGas | 1:c92781bb4d5e | 174 | m[9] * m[2] * m[7] + |
| ErmGas | 1:c92781bb4d5e | 175 | m[9] * m[3] * m[6]; |
| ErmGas | 1:c92781bb4d5e | 176 | |
| ErmGas | 1:c92781bb4d5e | 177 | inv[7] = m[0] * m[6] * m[11] - |
| ErmGas | 1:c92781bb4d5e | 178 | m[0] * m[7] * m[10] - |
| ErmGas | 1:c92781bb4d5e | 179 | m[4] * m[2] * m[11] + |
| ErmGas | 1:c92781bb4d5e | 180 | m[4] * m[3] * m[10] + |
| ErmGas | 1:c92781bb4d5e | 181 | m[8] * m[2] * m[7] - |
| ErmGas | 1:c92781bb4d5e | 182 | m[8] * m[3] * m[6]; |
| ErmGas | 1:c92781bb4d5e | 183 | |
| ErmGas | 1:c92781bb4d5e | 184 | inv[11] = -m[0] * m[5] * m[11] + |
| ErmGas | 1:c92781bb4d5e | 185 | m[0] * m[7] * m[9] + |
| ErmGas | 1:c92781bb4d5e | 186 | m[4] * m[1] * m[11] - |
| ErmGas | 1:c92781bb4d5e | 187 | m[4] * m[3] * m[9] - |
| ErmGas | 1:c92781bb4d5e | 188 | m[8] * m[1] * m[7] + |
| ErmGas | 1:c92781bb4d5e | 189 | m[8] * m[3] * m[5]; |
| ErmGas | 1:c92781bb4d5e | 190 | |
| ErmGas | 1:c92781bb4d5e | 191 | inv[15] = m[0] * m[5] * m[10] - |
| ErmGas | 1:c92781bb4d5e | 192 | m[0] * m[6] * m[9] - |
| ErmGas | 1:c92781bb4d5e | 193 | m[4] * m[1] * m[10] + |
| ErmGas | 1:c92781bb4d5e | 194 | m[4] * m[2] * m[9] + |
| ErmGas | 1:c92781bb4d5e | 195 | m[8] * m[1] * m[6] - |
| ErmGas | 1:c92781bb4d5e | 196 | m[8] * m[2] * m[5]; |
| ErmGas | 1:c92781bb4d5e | 197 | |
| ErmGas | 1:c92781bb4d5e | 198 | det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12]; |
| ErmGas | 1:c92781bb4d5e | 199 | |
| ErmGas | 1:c92781bb4d5e | 200 | if (det == 0) |
| ErmGas | 1:c92781bb4d5e | 201 | return false; |
| ErmGas | 1:c92781bb4d5e | 202 | |
| ErmGas | 1:c92781bb4d5e | 203 | det = 1.0 / det; |
| ErmGas | 1:c92781bb4d5e | 204 | |
| ErmGas | 1:c92781bb4d5e | 205 | for (i = 0; i < 16; i++) |
| ErmGas | 1:c92781bb4d5e | 206 | invOut[i] = inv[i] * det; |
| ErmGas | 1:c92781bb4d5e | 207 | |
| ErmGas | 1:c92781bb4d5e | 208 | |
| ErmGas | 1:c92781bb4d5e | 209 | for (int i = 0; i<4; i++){ |
| ErmGas | 1:c92781bb4d5e | 210 | for (int j = 0; j<4; j++){ |
| ErmGas | 1:c92781bb4d5e | 211 | INVOut[i][j]=invOut[4*i + j]; |
| ErmGas | 1:c92781bb4d5e | 212 | } |
| ErmGas | 1:c92781bb4d5e | 213 | } |
| ErmGas | 1:c92781bb4d5e | 214 | return true; |
| ErmGas | 1:c92781bb4d5e | 215 | } |
| ErmGas | 1:c92781bb4d5e | 216 | |
| ErmGas | 1:c92781bb4d5e | 217 | ---------------------------------------------------------------------------------------------------- |
| ErmGas | 1:c92781bb4d5e | 218 | End of out commented subfunctions |
| ErmGas | 1:c92781bb4d5e | 219 | ---------------------------------------------------------------------------------------------------- |
| ErmGas | 1:c92781bb4d5e | 220 | */ |
| ErmGas | 1:c92781bb4d5e | 221 | int main() { |
| ErmGas | 1:c92781bb4d5e | 222 | |
| ErmGas | 1:c92781bb4d5e | 223 | /* PHASE 1 */ |
| ErmGas | 1:c92781bb4d5e | 224 | |
| ErmGas | 1:c92781bb4d5e | 225 | // beepsound |
| ErmGas | 1:c92781bb4d5e | 226 | char dixie[]={'V','1','5','O','5','G','1','6','C','1','6'}; |
| ErmGas | 1:c92781bb4d5e | 227 | //the number of characters in the array |
| ErmGas | 1:c92781bb4d5e | 228 | int numb=10; |
| ErmGas | 1:c92781bb4d5e | 229 | float mu = 0.4; // mu between m3pi and ground |
| ErmGas | 1:c92781bb4d5e | 230 | float g = 9.81; // you know it! |
| ErmGas | 1:c92781bb4d5e | 231 | // system time |
| ErmGas | 1:c92781bb4d5e | 232 | float time; |
| ErmGas | 1:c92781bb4d5e | 233 | // sensorarray |
| ErmGas | 1:c92781bb4d5e | 234 | int sensors [5] = {0, 0, 0, 0, 0}; |
| ErmGas | 1:c92781bb4d5e | 235 | // number of rounds driven with PID-Controller to map the track |
| ErmGas | 1:c92781bb4d5e | 236 | int testrounds = 1; |
| ErmGas | 1:c92781bb4d5e | 237 | // max velocity in phase 1 |
| ErmGas | 1:c92781bb4d5e | 238 | float speed1 = 0.1; |
| ErmGas | 1:c92781bb4d5e | 239 | // how strong will the outer motor be slowed to get back on track / the inner motor be fastend |
| ErmGas | 1:c92781bb4d5e | 240 | float correction = speed1; |
| ErmGas | 1:c92781bb4d5e | 241 | // how far away from the black line is tolerable |
| ErmGas | 1:c92781bb4d5e | 242 | float threshold = 0.2; |
| ErmGas | 1:c92781bb4d5e | 243 | |
| ErmGas | 1:c92781bb4d5e | 244 | m3pi.locate(0,1); // x,y-Position on LCD |
| ErmGas | 1:c92781bb4d5e | 245 | m3pi.printf("Line Flw"); // display output |
| ErmGas | 1:c92781bb4d5e | 246 | |
| ErmGas | 1:c92781bb4d5e | 247 | // wait. REMIND: While waiting, the drive-order will be fullfilled |
| ErmGas | 1:c92781bb4d5e | 248 | wait(2.0); |
| ErmGas | 1:c92781bb4d5e | 249 | |
| ErmGas | 1:c92781bb4d5e | 250 | // robot turns left and right, looking for a line |
| ErmGas | 1:c92781bb4d5e | 251 | m3pi.sensor_auto_calibrate(); |
| ErmGas | 1:c92781bb4d5e | 252 | |
| ErmGas | 1:c92781bb4d5e | 253 | // this first part should go on as long as the startline isn't crossed |
| ErmGas | 1:c92781bb4d5e | 254 | int rounds = 0; |
| ErmGas | 1:c92781bb4d5e | 255 | while (rounds == 0) { |
| ErmGas | 1:c92781bb4d5e | 256 | |
| ErmGas | 1:c92781bb4d5e | 257 | // -1.0 is far left, 1.0 is far right, 0.0 in the middle |
| ErmGas | 1:c92781bb4d5e | 258 | float position_of_line = m3pi.line_position(); |
| ErmGas | 1:c92781bb4d5e | 259 | |
| ErmGas | 1:c92781bb4d5e | 260 | // Line is more than the threshold to the right, slow the left motor |
| ErmGas | 1:c92781bb4d5e | 261 | if (position_of_line > threshold) { |
| ErmGas | 1:c92781bb4d5e | 262 | m3pi.left_motor(speed1+correction); |
| ErmGas | 1:c92781bb4d5e | 263 | m3pi.right_motor(speed1-correction); |
| ErmGas | 1:c92781bb4d5e | 264 | } |
| ErmGas | 1:c92781bb4d5e | 265 | |
| ErmGas | 1:c92781bb4d5e | 266 | // Line is more than 50% to the left, slow the right motor |
| ErmGas | 1:c92781bb4d5e | 267 | else if (position_of_line < -threshold) { |
| ErmGas | 1:c92781bb4d5e | 268 | m3pi.right_motor(speed1+correction); |
| ErmGas | 1:c92781bb4d5e | 269 | m3pi.left_motor(speed1-correction); |
| ErmGas | 1:c92781bb4d5e | 270 | } |
| ErmGas | 1:c92781bb4d5e | 271 | |
| ErmGas | 1:c92781bb4d5e | 272 | // Line is in the middle |
| ErmGas | 1:c92781bb4d5e | 273 | else { |
| ErmGas | 1:c92781bb4d5e | 274 | m3pi.forward(speed1); |
| ErmGas | 1:c92781bb4d5e | 275 | } |
| ErmGas | 1:c92781bb4d5e | 276 | |
| ErmGas | 1:c92781bb4d5e | 277 | |
| ErmGas | 1:c92781bb4d5e | 278 | |
| ErmGas | 1:c92781bb4d5e | 279 | |
| ErmGas | 1:c92781bb4d5e | 280 | // read the sensors, values in [0,1000] with 1000=completely dark |
| ErmGas | 1:c92781bb4d5e | 281 | m3pi.readsensor(sensors); |
| ErmGas | 1:c92781bb4d5e | 282 | |
| ErmGas | 1:c92781bb4d5e | 283 | |
| ErmGas | 1:c92781bb4d5e | 284 | // startline is found if all sensor show black |
| ErmGas | 1:c92781bb4d5e | 285 | if (sensors[0]+sensors[1]+sensors[2]+sensors[3]+sensors[4]>4500){ |
| ErmGas | 1:c92781bb4d5e | 286 | rounds=1; |
| ErmGas | 1:c92781bb4d5e | 287 | m3pi.playtune(dixie,numb); |
| ErmGas | 1:c92781bb4d5e | 288 | time = clock()/CLOCKS_PER_SEC; |
| ErmGas | 1:c92781bb4d5e | 289 | |
| ErmGas | 1:c92781bb4d5e | 290 | |
| ErmGas | 1:c92781bb4d5e | 291 | } |
| ErmGas | 1:c92781bb4d5e | 292 | } |
| ErmGas | 1:c92781bb4d5e | 293 | |
| ErmGas | 1:c92781bb4d5e | 294 | // startline is crossed now for the first time |
| ErmGas | 1:c92781bb4d5e | 295 | // now its time to follow the track with PID-Controll while mapping it |
| ErmGas | 1:c92781bb4d5e | 296 | ofstream testmap; |
| ErmGas | 1:c92781bb4d5e | 297 | testmap.open ("/usb/map.txt"); |
| ErmGas | 1:c92781bb4d5e | 298 | |
| ErmGas | 1:c92781bb4d5e | 299 | /* LSM303D initialization*/ |
| ErmGas | 1:c92781bb4d5e | 300 | int sensor_addr_LSM = 0x3A; // 0x3C; |
| ErmGas | 1:c92781bb4d5e | 301 | // request for x_l_m, results in sending: xlm,xhm,ylm,yhm,zlm,zhm |
| ErmGas | 1:c92781bb4d5e | 302 | char START_MAG = 0x08; |
| ErmGas | 1:c92781bb4d5e | 303 | // request for x_l_a, same as line 13 |
| ErmGas | 1:c92781bb4d5e | 304 | char START_ACC = 0x28; |
| ErmGas | 1:c92781bb4d5e | 305 | // request for x_l_g, same as line 13 |
| ErmGas | 1:c92781bb4d5e | 306 | char START_GYRO = 0x28; |
| ErmGas | 1:c92781bb4d5e | 307 | // empty array for readingvalues |
| ErmGas | 1:c92781bb4d5e | 308 | char content[6] = {0x00,0x00,0x00,0x00,0x00,0x00}; |
| ErmGas | 1:c92781bb4d5e | 309 | // array for setting properties |
| ErmGas | 1:c92781bb4d5e | 310 | char ctrl[2]; |
| ErmGas | 1:c92781bb4d5e | 311 | // set properties |
| ErmGas | 1:c92781bb4d5e | 312 | ctrl[0] = 0x21; // propertyadress |
| ErmGas | 1:c92781bb4d5e | 313 | ctrl[1] = 0x00; // AFS = 0 |
| ErmGas | 1:c92781bb4d5e | 314 | minimu9V3.write(sensor_addr_LSM, ctrl, 2); |
| ErmGas | 1:c92781bb4d5e | 315 | ctrl[0] = 0x20; // propertyadress |
| ErmGas | 1:c92781bb4d5e | 316 | ctrl[1] = 0x57; // = 0b01010111, AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled) |
| ErmGas | 1:c92781bb4d5e | 317 | minimu9V3.write(sensor_addr_LSM, ctrl, 2); |
| ErmGas | 1:c92781bb4d5e | 318 | ctrl[0] = 0x24; // propertyadress |
| ErmGas | 1:c92781bb4d5e | 319 | ctrl[1] = 0x64; // = 0b01100100, M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR) |
| ErmGas | 1:c92781bb4d5e | 320 | minimu9V3.write(sensor_addr_LSM, ctrl, 2); |
| ErmGas | 1:c92781bb4d5e | 321 | ctrl[0] = 0x25; // propertyadress |
| ErmGas | 1:c92781bb4d5e | 322 | ctrl[1] = 0x20; // 0b00100000, MFS = 01 (+/- 4 gauss full scale) |
| ErmGas | 1:c92781bb4d5e | 323 | minimu9V3.write(sensor_addr_LSM, ctrl, 2); |
| ErmGas | 1:c92781bb4d5e | 324 | ctrl[0] = 0x26; // propertyadress |
| ErmGas | 1:c92781bb4d5e | 325 | ctrl[1] = 0x00; // = 0b00100000, MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode) |
| ErmGas | 1:c92781bb4d5e | 326 | minimu9V3.write(sensor_addr_LSM, ctrl, 2); |
| ErmGas | 1:c92781bb4d5e | 327 | /* L3GD20H initialization*/ |
| ErmGas | 1:c92781bb4d5e | 328 | int sensor_addr_L3G = 0xD6; |
| ErmGas | 1:c92781bb4d5e | 329 | // set properties |
| ErmGas | 1:c92781bb4d5e | 330 | ctrl[0] = 0x39; // propertyadress |
| ErmGas | 1:c92781bb4d5e | 331 | ctrl[1] = 0x00; // Low_ODR = 0 (low speed ODR disabled) |
| ErmGas | 1:c92781bb4d5e | 332 | minimu9V3.write(sensor_addr_L3G,ctrl,2); |
| ErmGas | 1:c92781bb4d5e | 333 | ctrl[0] = 0x23; // propertyadress |
| ErmGas | 1:c92781bb4d5e | 334 | ctrl[1] = 0x00; // FS = 00 (+/- 250 dps full scale) |
| ErmGas | 1:c92781bb4d5e | 335 | minimu9V3.write(sensor_addr_L3G,ctrl,2); |
| ErmGas | 1:c92781bb4d5e | 336 | ctrl[0] = 0x20; // propertyadress |
| ErmGas | 1:c92781bb4d5e | 337 | ctrl[1] = 0x6F; // = 0b01101111, DR = 01 (200 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled) |
| ErmGas | 1:c92781bb4d5e | 338 | minimu9V3.write(sensor_addr_L3G,ctrl,2); |
| ErmGas | 1:c92781bb4d5e | 339 | |
| ErmGas | 1:c92781bb4d5e | 340 | // MAY be unneeded |
| ErmGas | 1:c92781bb4d5e | 341 | START_MAG |= 0x80; |
| ErmGas | 1:c92781bb4d5e | 342 | START_ACC |= 0x80; |
| ErmGas | 1:c92781bb4d5e | 343 | START_GYRO |= 0x80; |
| ErmGas | 1:c92781bb4d5e | 344 | |
| ErmGas | 1:c92781bb4d5e | 345 | uint8_t axl, axh, ayl, ayh, azl, azh; |
| ErmGas | 1:c92781bb4d5e | 346 | uint8_t mxl, mxh, myl, myh, mzl, mzh; |
| ErmGas | 1:c92781bb4d5e | 347 | uint8_t gxl, gxh, gyl, gyh, gzl, gzh; |
| ErmGas | 1:c92781bb4d5e | 348 | int16_t ax, ay, az; |
| ErmGas | 1:c92781bb4d5e | 349 | int16_t mx, my, mz; |
| ErmGas | 1:c92781bb4d5e | 350 | int16_t gx, gy, gz; |
| ErmGas | 1:c92781bb4d5e | 351 | |
| ErmGas | 1:c92781bb4d5e | 352 | m3pi.locate(0,1); |
| ErmGas | 1:c92781bb4d5e | 353 | m3pi.printf("Line PID"); |
| chris | 0:4e756c4c88a7 | 354 | |
| ErmGas | 1:c92781bb4d5e | 355 | // params for the PID-Controller |
| ErmGas | 1:c92781bb4d5e | 356 | float P_TERM=1; |
| ErmGas | 1:c92781bb4d5e | 357 | float I_TERM=0; |
| ErmGas | 1:c92781bb4d5e | 358 | float D_TERM=20; |
| ErmGas | 1:c92781bb4d5e | 359 | |
| ErmGas | 1:c92781bb4d5e | 360 | float speed2 = 5*speed1; // max velocity in phase 2 |
| ErmGas | 1:c92781bb4d5e | 361 | float right; |
| ErmGas | 1:c92781bb4d5e | 362 | float left; |
| ErmGas | 1:c92781bb4d5e | 363 | float current_pos_of_line = 0.0; |
| ErmGas | 1:c92781bb4d5e | 364 | float previous_pos_of_line = 0.0; |
| ErmGas | 1:c92781bb4d5e | 365 | float derivative,proportional,integral = 0; |
| ErmGas | 1:c92781bb4d5e | 366 | float power; |
| ErmGas | 1:c92781bb4d5e | 367 | float MAX = 2*speed2; |
| ErmGas | 1:c92781bb4d5e | 368 | if(MAX>1) MAX = 1; |
| ErmGas | 1:c92781bb4d5e | 369 | float MIN = 0; |
| ErmGas | 1:c92781bb4d5e | 370 | // mapping variables |
| ErmGas | 1:c92781bb4d5e | 371 | float x_abs = 0; |
| ErmGas | 1:c92781bb4d5e | 372 | float y_abs = 0; |
| ErmGas | 1:c92781bb4d5e | 373 | float alpha_abs = 0; |
| ErmGas | 1:c92781bb4d5e | 374 | //float x_rel, dx_abs; |
| ErmGas | 1:c92781bb4d5e | 375 | //float y_rel, dy_abs; |
| ErmGas | 1:c92781bb4d5e | 376 | float min_mx=0; |
| ErmGas | 1:c92781bb4d5e | 377 | float min_my=0; |
| ErmGas | 1:c92781bb4d5e | 378 | float max_mx=0; |
| ErmGas | 1:c92781bb4d5e | 379 | float max_my=0; |
| ErmGas | 1:c92781bb4d5e | 380 | float faktor = 893562.2181291698; |
| ErmGas | 1:c92781bb4d5e | 381 | |
| ErmGas | 1:c92781bb4d5e | 382 | float dt = 0.0046; |
| ErmGas | 1:c92781bb4d5e | 383 | |
| ErmGas | 1:c92781bb4d5e | 384 | int size = 0; |
| ErmGas | 1:c92781bb4d5e | 385 | while (rounds < testrounds+1){ |
| ErmGas | 1:c92781bb4d5e | 386 | |
| ErmGas | 1:c92781bb4d5e | 387 | // Get the position of the line. |
| ErmGas | 1:c92781bb4d5e | 388 | current_pos_of_line = m3pi.line_position(); |
| ErmGas | 1:c92781bb4d5e | 389 | proportional = current_pos_of_line; |
| ErmGas | 1:c92781bb4d5e | 390 | |
| ErmGas | 1:c92781bb4d5e | 391 | // Compute the derivative |
| ErmGas | 1:c92781bb4d5e | 392 | derivative = current_pos_of_line - previous_pos_of_line; |
| ErmGas | 1:c92781bb4d5e | 393 | |
| ErmGas | 1:c92781bb4d5e | 394 | // Compute the integral |
| ErmGas | 1:c92781bb4d5e | 395 | integral += proportional; |
| ErmGas | 1:c92781bb4d5e | 396 | |
| ErmGas | 1:c92781bb4d5e | 397 | // Remember the last position. |
| ErmGas | 1:c92781bb4d5e | 398 | previous_pos_of_line = current_pos_of_line; |
| ErmGas | 1:c92781bb4d5e | 399 | |
| ErmGas | 1:c92781bb4d5e | 400 | // Compute the power |
| ErmGas | 1:c92781bb4d5e | 401 | power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; |
| ErmGas | 1:c92781bb4d5e | 402 | |
| ErmGas | 1:c92781bb4d5e | 403 | // Compute new speeds |
| ErmGas | 1:c92781bb4d5e | 404 | right = speed2-power; |
| ErmGas | 1:c92781bb4d5e | 405 | left = speed2+power; |
| ErmGas | 1:c92781bb4d5e | 406 | |
| ErmGas | 1:c92781bb4d5e | 407 | // limit checks |
| ErmGas | 1:c92781bb4d5e | 408 | if (right < MIN) |
| ErmGas | 1:c92781bb4d5e | 409 | right = MIN; |
| ErmGas | 1:c92781bb4d5e | 410 | else if (right > MAX) |
| ErmGas | 1:c92781bb4d5e | 411 | right = MAX; |
| ErmGas | 1:c92781bb4d5e | 412 | |
| ErmGas | 1:c92781bb4d5e | 413 | if (left < MIN) |
| ErmGas | 1:c92781bb4d5e | 414 | left = MIN; |
| ErmGas | 1:c92781bb4d5e | 415 | else if (left > MAX) |
| ErmGas | 1:c92781bb4d5e | 416 | left = MAX; |
| ErmGas | 1:c92781bb4d5e | 417 | |
| ErmGas | 1:c92781bb4d5e | 418 | // set speed |
| ErmGas | 1:c92781bb4d5e | 419 | m3pi.left_motor(left); |
| ErmGas | 1:c92781bb4d5e | 420 | m3pi.right_motor(right); |
| ErmGas | 1:c92781bb4d5e | 421 | |
| ErmGas | 1:c92781bb4d5e | 422 | // sensors |
| ErmGas | 1:c92781bb4d5e | 423 | minimu9V3.write(sensor_addr_L3G, &START_GYRO,1,true); |
| ErmGas | 1:c92781bb4d5e | 424 | minimu9V3.read(sensor_addr_L3G, content, 6); |
| ErmGas | 1:c92781bb4d5e | 425 | |
| ErmGas | 1:c92781bb4d5e | 426 | gxl = content[0]; |
| ErmGas | 1:c92781bb4d5e | 427 | gxh = content[1]; |
| ErmGas | 1:c92781bb4d5e | 428 | gyl = content[2]; |
| ErmGas | 1:c92781bb4d5e | 429 | gyh = content[3]; |
| ErmGas | 1:c92781bb4d5e | 430 | gzl = content[4]; |
| ErmGas | 1:c92781bb4d5e | 431 | gzh = content[5]; |
| ErmGas | 1:c92781bb4d5e | 432 | |
| ErmGas | 1:c92781bb4d5e | 433 | |
| ErmGas | 1:c92781bb4d5e | 434 | gx=(int16_t)(gxh<<8|gxl); |
| ErmGas | 1:c92781bb4d5e | 435 | gy=(int16_t)(gyh<<8|gyl); |
| ErmGas | 1:c92781bb4d5e | 436 | gz=(int16_t)(gzh<<8|gzl); |
| ErmGas | 1:c92781bb4d5e | 437 | |
| ErmGas | 1:c92781bb4d5e | 438 | minimu9V3.write(sensor_addr_LSM, &START_MAG, 1, true);//, true |
| ErmGas | 1:c92781bb4d5e | 439 | minimu9V3.read(sensor_addr_LSM, content,6); |
| ErmGas | 1:c92781bb4d5e | 440 | |
| ErmGas | 1:c92781bb4d5e | 441 | mxl = content[0]; |
| ErmGas | 1:c92781bb4d5e | 442 | mxh = content[1]; |
| ErmGas | 1:c92781bb4d5e | 443 | myl = content[2]; |
| ErmGas | 1:c92781bb4d5e | 444 | myh = content[3]; |
| ErmGas | 1:c92781bb4d5e | 445 | mzl = content[4]; |
| ErmGas | 1:c92781bb4d5e | 446 | mzh = content[5]; |
| ErmGas | 1:c92781bb4d5e | 447 | |
| ErmGas | 1:c92781bb4d5e | 448 | |
| ErmGas | 1:c92781bb4d5e | 449 | mx=(int16_t)(mxh<<8|mxl); |
| ErmGas | 1:c92781bb4d5e | 450 | my=(int16_t)(myh<<8|myl); |
| ErmGas | 1:c92781bb4d5e | 451 | mz=(int16_t)(mzh<<8|mzl); |
| ErmGas | 1:c92781bb4d5e | 452 | |
| ErmGas | 1:c92781bb4d5e | 453 | minimu9V3.write(sensor_addr_LSM, &START_ACC, 1, true);//, true |
| ErmGas | 1:c92781bb4d5e | 454 | minimu9V3.read(sensor_addr_LSM, content, 6); |
| ErmGas | 1:c92781bb4d5e | 455 | axl = content[0]; |
| ErmGas | 1:c92781bb4d5e | 456 | axh = content[1]; |
| ErmGas | 1:c92781bb4d5e | 457 | ayl = content[2]; |
| ErmGas | 1:c92781bb4d5e | 458 | ayh = content[3]; |
| ErmGas | 1:c92781bb4d5e | 459 | azl = content[4]; |
| ErmGas | 1:c92781bb4d5e | 460 | azh = content[5]; |
| ErmGas | 1:c92781bb4d5e | 461 | |
| ErmGas | 1:c92781bb4d5e | 462 | |
| ErmGas | 1:c92781bb4d5e | 463 | ax=(int16_t)(axh<<8|axl); |
| ErmGas | 1:c92781bb4d5e | 464 | ay=(int16_t)(ayh<<8|ayl); |
| ErmGas | 1:c92781bb4d5e | 465 | az=(int16_t)(azh<<8|azl); |
| ErmGas | 1:c92781bb4d5e | 466 | |
| ErmGas | 1:c92781bb4d5e | 467 | |
| ErmGas | 1:c92781bb4d5e | 468 | |
| ErmGas | 1:c92781bb4d5e | 469 | |
| ErmGas | 1:c92781bb4d5e | 470 | testmap<<mx<<" "<<my<<" "<<current_pos_of_line<<" "<<gz<<" "<<left<<" "<<right<<" \n"; |
| ErmGas | 1:c92781bb4d5e | 471 | |
| ErmGas | 1:c92781bb4d5e | 472 | // read the sensors, values in [0,1000] with 1000=completely dark |
| ErmGas | 1:c92781bb4d5e | 473 | m3pi.readsensor(sensors); |
| ErmGas | 1:c92781bb4d5e | 474 | |
| ErmGas | 1:c92781bb4d5e | 475 | // startline is found if all sensor show black |
| ErmGas | 1:c92781bb4d5e | 476 | |
| ErmGas | 1:c92781bb4d5e | 477 | if (sensors[0]+sensors[1]+sensors[2]+sensors[3]+sensors[4]>4500 && clock()/CLOCKS_PER_SEC>=time+3){ |
| ErmGas | 1:c92781bb4d5e | 478 | rounds++; |
| ErmGas | 1:c92781bb4d5e | 479 | m3pi.playtune(dixie,numb); |
| ErmGas | 1:c92781bb4d5e | 480 | time=clock()/CLOCKS_PER_SEC; |
| ErmGas | 1:c92781bb4d5e | 481 | } |
| ErmGas | 1:c92781bb4d5e | 482 | size++; |
| ErmGas | 1:c92781bb4d5e | 483 | |
| ErmGas | 1:c92781bb4d5e | 484 | } |
| ErmGas | 1:c92781bb4d5e | 485 | testmap.close(); |
| ErmGas | 1:c92781bb4d5e | 486 | /* |
| ErmGas | 1:c92781bb4d5e | 487 | ---------------------------------------------------------------------------------------------------- |
| ErmGas | 1:c92781bb4d5e | 488 | Completely commented out, EKF-Loop, but is not working, because of the m3pi's small power |
| ErmGas | 1:c92781bb4d5e | 489 | ---------------------------------------------------------------------------------------------------- |
| ErmGas | 1:c92781bb4d5e | 490 | |
| ErmGas | 1:c92781bb4d5e | 491 | |
| ErmGas | 1:c92781bb4d5e | 492 | m3pi.forward(0); |
| ErmGas | 1:c92781bb4d5e | 493 | m3pi.locate(0,1); |
| chris | 0:4e756c4c88a7 | 494 | |
| ErmGas | 1:c92781bb4d5e | 495 | float x_kp1_k[3], x_k_k[3], y_kp1_k[4], y_k[4]; |
| ErmGas | 1:c92781bb4d5e | 496 | x_kp1_k[0]=0; |
| ErmGas | 1:c92781bb4d5e | 497 | x_kp1_k[1]=0; |
| ErmGas | 1:c92781bb4d5e | 498 | x_kp1_k[2]=0; |
| ErmGas | 1:c92781bb4d5e | 499 | // F is Jakobi of x |
| ErmGas | 1:c92781bb4d5e | 500 | float F[3][3]; |
| ErmGas | 1:c92781bb4d5e | 501 | F[0][0] = 1;//F11 |
| ErmGas | 1:c92781bb4d5e | 502 | F[0][1] = 0;//F12 |
| ErmGas | 1:c92781bb4d5e | 503 | F[1][0] = 0;//F21 |
| ErmGas | 1:c92781bb4d5e | 504 | F[1][1] = 1;//F22 |
| ErmGas | 1:c92781bb4d5e | 505 | F[2][0] = 0;//F31 |
| ErmGas | 1:c92781bb4d5e | 506 | F[2][1] = 0;//F32 |
| ErmGas | 1:c92781bb4d5e | 507 | F[2][2] = 1;//F33 |
| ErmGas | 1:c92781bb4d5e | 508 | // H is Jakobi of y |
| ErmGas | 1:c92781bb4d5e | 509 | float H[4][3]; |
| ErmGas | 1:c92781bb4d5e | 510 | H[0][0] = 0; |
| ErmGas | 1:c92781bb4d5e | 511 | H[0][1] = 0; |
| ErmGas | 1:c92781bb4d5e | 512 | H[1][0] = 0; |
| ErmGas | 1:c92781bb4d5e | 513 | H[1][1] = 0; |
| ErmGas | 1:c92781bb4d5e | 514 | H[3][0] = 0; |
| ErmGas | 1:c92781bb4d5e | 515 | H[3][1] = 0; |
| ErmGas | 1:c92781bb4d5e | 516 | H[3][2] = -1/dt; |
| ErmGas | 1:c92781bb4d5e | 517 | // P(k+1|k) = P11_kp1_k etc / P(k|k) = P11_k_k etc |
| ErmGas | 1:c92781bb4d5e | 518 | float P_kp1_k[3][3], P_k_k[3][3]; |
| ErmGas | 1:c92781bb4d5e | 519 | P_k_k[0][0] = 1; // P0 is eye(3) |
| ErmGas | 1:c92781bb4d5e | 520 | P_k_k[0][1] = 0; |
| ErmGas | 1:c92781bb4d5e | 521 | P_k_k[0][2] = 0; |
| ErmGas | 1:c92781bb4d5e | 522 | P_k_k[1][0] = 0; |
| ErmGas | 1:c92781bb4d5e | 523 | P_k_k[1][1] = 1; |
| ErmGas | 1:c92781bb4d5e | 524 | P_k_k[1][2] = 0; |
| ErmGas | 1:c92781bb4d5e | 525 | P_k_k[2][0] = 0; |
| ErmGas | 1:c92781bb4d5e | 526 | P_k_k[2][1] = 0; |
| ErmGas | 1:c92781bb4d5e | 527 | P_k_k[2][2] = 1; |
| ErmGas | 1:c92781bb4d5e | 528 | // Matrix Q |
| ErmGas | 1:c92781bb4d5e | 529 | float Q[3][3]; |
| ErmGas | 1:c92781bb4d5e | 530 | Q[0][0] = 2.1289; // is result from optimization on simulation |
| ErmGas | 1:c92781bb4d5e | 531 | Q[0][1] = 0.9727; |
| ErmGas | 1:c92781bb4d5e | 532 | Q[0][2] = 0.2097; |
| ErmGas | 1:c92781bb4d5e | 533 | Q[1][0] = 0.9727; |
| ErmGas | 1:c92781bb4d5e | 534 | Q[1][1] = 1.6290; |
| ErmGas | 1:c92781bb4d5e | 535 | Q[1][2] = 0.1533; |
| ErmGas | 1:c92781bb4d5e | 536 | Q[2][0] = 0.2097; |
| ErmGas | 1:c92781bb4d5e | 537 | Q[2][1] = 0.1533; |
| ErmGas | 1:c92781bb4d5e | 538 | Q[2][2] = 0.0443; |
| ErmGas | 1:c92781bb4d5e | 539 | // Helpmatrix M = HP(k+1|k)H + R |
| ErmGas | 1:c92781bb4d5e | 540 | float M[4][4], invM[4][4]; |
| ErmGas | 1:c92781bb4d5e | 541 | // Matrix R |
| ErmGas | 1:c92781bb4d5e | 542 | float R[4][4]; |
| ErmGas | 1:c92781bb4d5e | 543 | R[0][0] = 0.2039; // also result from optimization on simulation |
| ErmGas | 1:c92781bb4d5e | 544 | R[0][1] = 0; |
| ErmGas | 1:c92781bb4d5e | 545 | R[0][2] = 0; |
| ErmGas | 1:c92781bb4d5e | 546 | R[0][3] = 0; |
| ErmGas | 1:c92781bb4d5e | 547 | R[1][0] = 0; |
| ErmGas | 1:c92781bb4d5e | 548 | R[1][1] = 0.2039; |
| ErmGas | 1:c92781bb4d5e | 549 | R[1][2] = 0; |
| ErmGas | 1:c92781bb4d5e | 550 | R[1][3] = 0; |
| ErmGas | 1:c92781bb4d5e | 551 | R[2][0] = 0; |
| ErmGas | 1:c92781bb4d5e | 552 | R[2][1] = 0; |
| ErmGas | 1:c92781bb4d5e | 553 | R[2][2] = 0.5281; |
| ErmGas | 1:c92781bb4d5e | 554 | R[2][3] = 0; |
| ErmGas | 1:c92781bb4d5e | 555 | R[3][0] = 0; |
| ErmGas | 1:c92781bb4d5e | 556 | R[3][1] = 0; |
| ErmGas | 1:c92781bb4d5e | 557 | R[3][2] = 0; |
| ErmGas | 1:c92781bb4d5e | 558 | R[3][3] = 0.0216; |
| ErmGas | 1:c92781bb4d5e | 559 | // Matrix K |
| ErmGas | 1:c92781bb4d5e | 560 | float K[3][4]; |
| ErmGas | 1:c92781bb4d5e | 561 | // speed for phase 3 |
| ErmGas | 1:c92781bb4d5e | 562 | float speed3 = speed2; |
| ErmGas | 1:c92781bb4d5e | 563 | m3pi.forward(speed3); |
| ErmGas | 1:c92781bb4d5e | 564 | while(rounds<testrounds+2){ |
| ErmGas | 1:c92781bb4d5e | 565 | // PID Stuff |
| ErmGas | 1:c92781bb4d5e | 566 | // Get the position of the line. |
| ErmGas | 1:c92781bb4d5e | 567 | current_pos_of_line = m3pi.line_position(); |
| ErmGas | 1:c92781bb4d5e | 568 | proportional = current_pos_of_line; |
| ErmGas | 1:c92781bb4d5e | 569 | |
| ErmGas | 1:c92781bb4d5e | 570 | // Compute the derivative |
| ErmGas | 1:c92781bb4d5e | 571 | derivative = current_pos_of_line - previous_pos_of_line; |
| ErmGas | 1:c92781bb4d5e | 572 | |
| ErmGas | 1:c92781bb4d5e | 573 | // Compute the integral |
| ErmGas | 1:c92781bb4d5e | 574 | integral += proportional; |
| ErmGas | 1:c92781bb4d5e | 575 | |
| ErmGas | 1:c92781bb4d5e | 576 | // Remember the last position. |
| ErmGas | 1:c92781bb4d5e | 577 | previous_pos_of_line = current_pos_of_line; |
| ErmGas | 1:c92781bb4d5e | 578 | |
| ErmGas | 1:c92781bb4d5e | 579 | // Compute the power |
| ErmGas | 1:c92781bb4d5e | 580 | power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; |
| ErmGas | 1:c92781bb4d5e | 581 | |
| ErmGas | 1:c92781bb4d5e | 582 | // Compute new speeds |
| ErmGas | 1:c92781bb4d5e | 583 | right = speed3-power; |
| ErmGas | 1:c92781bb4d5e | 584 | left = speed3+power; |
| ErmGas | 1:c92781bb4d5e | 585 | // limit checks |
| ErmGas | 1:c92781bb4d5e | 586 | if (right < MIN) |
| ErmGas | 1:c92781bb4d5e | 587 | right = MIN; |
| ErmGas | 1:c92781bb4d5e | 588 | else if (right > MAX) |
| ErmGas | 1:c92781bb4d5e | 589 | right = MAX; |
| ErmGas | 1:c92781bb4d5e | 590 | |
| ErmGas | 1:c92781bb4d5e | 591 | if (left < MIN) |
| ErmGas | 1:c92781bb4d5e | 592 | left = MIN; |
| ErmGas | 1:c92781bb4d5e | 593 | else if (left > MAX) |
| ErmGas | 1:c92781bb4d5e | 594 | left = MAX; |
| ErmGas | 1:c92781bb4d5e | 595 | |
| ErmGas | 1:c92781bb4d5e | 596 | // set speed |
| ErmGas | 1:c92781bb4d5e | 597 | m3pi.left_motor(left); |
| ErmGas | 1:c92781bb4d5e | 598 | m3pi.right_motor(right); |
| ErmGas | 1:c92781bb4d5e | 599 | // Kalman Filter |
| ErmGas | 1:c92781bb4d5e | 600 | // step 1 x_hat(k+1|k)=f(u,x_hat(k|k)) |
| ErmGas | 1:c92781bb4d5e | 601 | if (right>left){ |
| ErmGas | 1:c92781bb4d5e | 602 | x_kp1_k[0] = x_k_k[0] + ((8.1*right/(right-left)-4.05)*sin((100*(right-left)/8.1 *dt)))*cos(x_k_k[2]) + ((8.1*right/(right-left)-4.05)*(1-cos((100*(right-left)/8.1 *dt))))*sin(x_k_k[2]); |
| ErmGas | 1:c92781bb4d5e | 603 | x_kp1_k[1] = x_k_k[1] + ((8.1*right/(right-left)-4.05)*sin((100*(right-left)/8.1 *dt)))*sin(x_k_k[2]) + ((8.1*right/(right-left)-4.05)*(1-cos((100*(right-left)/8.1 *dt))))*cos(x_k_k[2]); |
| ErmGas | 1:c92781bb4d5e | 604 | x_kp1_k[2] = x_k_k[2] + (100*(right-left)/8.1*dt); |
| ErmGas | 1:c92781bb4d5e | 605 | } |
| ErmGas | 1:c92781bb4d5e | 606 | else if (left>right){ |
| ErmGas | 1:c92781bb4d5e | 607 | x_kp1_k[0] = x_k_k[0] + ((8.1*left/(left-right)-4.05)*sin((100*(left-right)/8.1 *dt)))*cos(x_k_k[2]) + ((8.1*left/(left-right)-4.05)*(1-cos((100*(left-right)/8.1 *dt))))*sin(x_k_k[2]); |
| ErmGas | 1:c92781bb4d5e | 608 | x_kp1_k[1] = x_k_k[1] + ((8.1*left/(left-right)-4.05)*sin((100*(left-right)/8.1 *dt)))*sin(x_k_k[2]) + ((8.1*left/(left-right)-4.05)*(1-cos((100*(left-right)/8.1 *dt))))*cos(x_k_k[2]); |
| ErmGas | 1:c92781bb4d5e | 609 | x_kp1_k[2] = x_k_k[2] - (100*(left-right)/8.1*dt); |
| ErmGas | 1:c92781bb4d5e | 610 | } |
| ErmGas | 1:c92781bb4d5e | 611 | else { // left == right |
| ErmGas | 1:c92781bb4d5e | 612 | x_kp1_k[0] = x_k_k[0] + (100*right*dt)*cos(x_k_k[2]); |
| ErmGas | 1:c92781bb4d5e | 613 | x_kp1_k[1] = x_k_k[1] + (100*right*dt)*sin(x_k_k[2]); |
| ErmGas | 1:c92781bb4d5e | 614 | x_kp1_k[2] = x_k_k[2]; |
| ErmGas | 1:c92781bb4d5e | 615 | } |
| ErmGas | 1:c92781bb4d5e | 616 | // step 2 y_hat(k+1|k)=h(u(k+1),x_hat(k+1|k)) |
| ErmGas | 1:c92781bb4d5e | 617 | y_kp1_k[0] = -cos(x_kp1_k[2]); |
| ErmGas | 1:c92781bb4d5e | 618 | y_kp1_k[1] = -sin(x_kp1_k[2]); |
| ErmGas | 1:c92781bb4d5e | 619 | y_kp1_k[2] = Lookuptable(x_kp1_k[0],x_kp1_k[1],x_kp1_k[2], size); // problems, no LUT implemented! |
| ErmGas | 1:c92781bb4d5e | 620 | y_kp1_k[3] = -(x_kp1_k[2] - x_k_k[2])/dt; |
| ErmGas | 1:c92781bb4d5e | 621 | // step 3 F is Jakobi of x |
| ErmGas | 1:c92781bb4d5e | 622 | if (right>left){ |
| ErmGas | 1:c92781bb4d5e | 623 | F[0][2] = ((8.1*right/(right-left)-4.05)*(-1)*sin((100*(right-left)/8.1 *dt)))*sin(x_k_k[2]) + ((8.1*right/(right-left)-4.05)*(1-cos((100*(right-left)/8.1 *dt))))*cos(x_k_k[2]); |
| ErmGas | 1:c92781bb4d5e | 624 | F[1][2] = ((8.1*right/(right-left)-4.05)*sin((100*(right-left)/8.1 *dt)))*cos(x_k_k[2]) + ((8.1*right/(right-left)-4.05)*(1-cos((100*(right-left)/8.1 *dt))))*(-1)*sin(x_k_k[2]); |
| ErmGas | 1:c92781bb4d5e | 625 | } |
| ErmGas | 1:c92781bb4d5e | 626 | else if (left>right){ |
| ErmGas | 1:c92781bb4d5e | 627 | F[0][2] = ((8.1*left/(left-right)-4.05)*sin((100*(left-right)/8.1 *dt)))*(-1)*sin(x_k_k[2]) + ((8.1*left/(left-right)-4.05)*(1-cos((100*(left-right)/8.1 *dt))))*cos(x_k_k[2]); |
| ErmGas | 1:c92781bb4d5e | 628 | F[1][2] = ((8.1*left/(left-right)-4.05)*sin((100*(left-right)/8.1 *dt)))*cos(x_k_k[2]) + ((8.1*left/(left-right)-4.05)*(1-cos((100*(left-right)/8.1 *dt))))*(-1)*sin(x_k_k[2]); |
| ErmGas | 1:c92781bb4d5e | 629 | } |
| ErmGas | 1:c92781bb4d5e | 630 | else { // left == right |
| ErmGas | 1:c92781bb4d5e | 631 | F[0][2] = (-1)*right*100*dt*sin(x_k_k[2]); |
| ErmGas | 1:c92781bb4d5e | 632 | F[1][2] = 100*left*dt*cos(x_k_k[2]); |
| ErmGas | 1:c92781bb4d5e | 633 | } |
| ErmGas | 1:c92781bb4d5e | 634 | // step 4 H is Jakobi of y |
| ErmGas | 1:c92781bb4d5e | 635 | float LoTa = Lookuptable(x_k_k[0],x_k_k[1],x_kp1_k[2], size); |
| ErmGas | 1:c92781bb4d5e | 636 | H[0][2] = sin(x_k_k[2]); |
| ErmGas | 1:c92781bb4d5e | 637 | H[1][2] = -cos(x_k_k[2]); |
| ErmGas | 1:c92781bb4d5e | 638 | H[2][0] = (Lookuptable(x_k_k[0],x_k_k[1],x_k_k[2], size)-Lookuptable(x_kp1_k[0],x_k_k[1],x_k_k[2], size))/(x_k_k[0]-x_kp1_k[0]); |
| ErmGas | 1:c92781bb4d5e | 639 | H[2][1] = (Lookuptable(x_k_k[0],x_k_k[1],x_k_k[2], size)-Lookuptable(x_k_k[0],x_kp1_k[1],x_k_k[2], size))/(x_k_k[1]-x_kp1_k[1]); |
| ErmGas | 1:c92781bb4d5e | 640 | H[2][2] = std::pow((float)abs(LoTa),(float)(1.5/2.5))*2.5/pi*abs(LoTa)/LoTa; |
| ErmGas | 1:c92781bb4d5e | 641 | // step 5 P(k+1|k) = F*P(k|k)*F' + Q; |
| ErmGas | 1:c92781bb4d5e | 642 | // trust me, thats correct (says matlab) |
| ErmGas | 1:c92781bb4d5e | 643 | for (int i = 0; i<3; i++){ |
| ErmGas | 1:c92781bb4d5e | 644 | for (int j = 0; j<3; j++){ |
| ErmGas | 1:c92781bb4d5e | 645 | P_kp1_k[i][j] = F[j][2]*(F[i][2]*P_k_k[2][2] + F[i][1]*P_k_k[1][2] + F[i][0]*P_k_k[0][2]) + F[j][1]*(F[i][2]*P_k_k[2][1] + F[i][1]*P_k_k[1][1] + F[i][0]*P_k_k[0][1]) + F[j][0]*(F[i][2]*P_k_k[2][0] + F[i][1]*P_k_k[1][0] + F[i][0]*P_k_k[0][0]) + Q[i][j]; |
| ErmGas | 1:c92781bb4d5e | 646 | } |
| ErmGas | 1:c92781bb4d5e | 647 | } |
| ErmGas | 1:c92781bb4d5e | 648 | |
| ErmGas | 1:c92781bb4d5e | 649 | // step 6 K = P(k+1|k)H' * (HP(k+1|k)H'+R)^(-1) |
| ErmGas | 1:c92781bb4d5e | 650 | // same as above, blame matlab for errors! |
| ErmGas | 1:c92781bb4d5e | 651 | // M = H*P(k+1|k)*H' + R |
| ErmGas | 1:c92781bb4d5e | 652 | for (int i= 0; i<4; i++ ){ |
| ErmGas | 1:c92781bb4d5e | 653 | for (int j = 0; j<4; j++){ |
| ErmGas | 1:c92781bb4d5e | 654 | M[i][j] = H[j][2]*(H[i][2]*P_kp1_k[2][2] + H[i][1]*P_kp1_k[1][2] + H[i][0]*P_kp1_k[0][2]) + H[j][1]*(H[i][2]*P_kp1_k[2][1] + H[i][1]*P_kp1_k[1][1] + H[i][0]*P_kp1_k[0][1]) + H[j][0]*(H[i][2]*P_kp1_k[2][0] + H[i][1]*P_kp1_k[1][0] + H[i][0]*P_kp1_k[0][0]) + R[i][j]; |
| ErmGas | 1:c92781bb4d5e | 655 | } |
| ErmGas | 1:c92781bb4d5e | 656 | } |
| ErmGas | 1:c92781bb4d5e | 657 | // same goes here, tested with MatLab! |
| ErmGas | 1:c92781bb4d5e | 658 | gluInvertMatrix(M, invM); |
| ErmGas | 1:c92781bb4d5e | 659 | // K = P(k+1|k) * H' * invM |
| ErmGas | 1:c92781bb4d5e | 660 | for (int i= 0; i<3; i++ ){ |
| ErmGas | 1:c92781bb4d5e | 661 | for (int j = 0; j<4; j++){ |
| ErmGas | 1:c92781bb4d5e | 662 | K[i][j] = invM[0][j]*(P_kp1_k[i][0]*H[0][0] + P_kp1_k[i][1]*H[0][1] + P_kp1_k[i][2]*H[0][2]) + invM[1][j]*(P_kp1_k[i][0]*H[1][0] + P_kp1_k[i][1]*H[1][1] + P_kp1_k[i][2]*H[1][2]) + invM[2][j]*(P_kp1_k[i][0]*H[2][0] + P_kp1_k[i][1]*H[2][1] + P_kp1_k[i][2]*H[2][2]) + invM[3][j]*(P_kp1_k[i][0]*H[3][0] + P_kp1_k[i][1]*H[3][1] + P_kp1_k[i][2]*H[3][2]); |
| ErmGas | 1:c92781bb4d5e | 663 | } |
| ErmGas | 1:c92781bb4d5e | 664 | } |
| ErmGas | 1:c92781bb4d5e | 665 | |
| ErmGas | 1:c92781bb4d5e | 666 | // Measurementupdate needs measurmentdata! |
| ErmGas | 1:c92781bb4d5e | 667 | // sensors |
| ErmGas | 1:c92781bb4d5e | 668 | minimu9V3.write(sensor_addr_L3G, &START_GYRO,1,true); |
| ErmGas | 1:c92781bb4d5e | 669 | minimu9V3.read(sensor_addr_L3G, content, 6); |
| ErmGas | 1:c92781bb4d5e | 670 | |
| ErmGas | 1:c92781bb4d5e | 671 | gxl = content[0]; |
| ErmGas | 1:c92781bb4d5e | 672 | gxh = content[1]; |
| ErmGas | 1:c92781bb4d5e | 673 | gyl = content[2]; |
| ErmGas | 1:c92781bb4d5e | 674 | gyh = content[3]; |
| ErmGas | 1:c92781bb4d5e | 675 | gzl = content[4]; |
| ErmGas | 1:c92781bb4d5e | 676 | gzh = content[5]; |
| ErmGas | 1:c92781bb4d5e | 677 | |
| ErmGas | 1:c92781bb4d5e | 678 | |
| ErmGas | 1:c92781bb4d5e | 679 | gx=(int16_t)(gxh<<8|gxl); |
| ErmGas | 1:c92781bb4d5e | 680 | gy=(int16_t)(gyh<<8|gyl); |
| ErmGas | 1:c92781bb4d5e | 681 | gz=(int16_t)(gzh<<8|gzl); |
| ErmGas | 1:c92781bb4d5e | 682 | |
| ErmGas | 1:c92781bb4d5e | 683 | minimu9V3.write(sensor_addr_LSM, &START_MAG, 1, true);//, true |
| ErmGas | 1:c92781bb4d5e | 684 | minimu9V3.read(sensor_addr_LSM, content,6); |
| ErmGas | 1:c92781bb4d5e | 685 | // |
| ErmGas | 1:c92781bb4d5e | 686 | mxl = content[0]; |
| ErmGas | 1:c92781bb4d5e | 687 | mxh = content[1]; |
| ErmGas | 1:c92781bb4d5e | 688 | myl = content[2]; |
| ErmGas | 1:c92781bb4d5e | 689 | myh = content[3]; |
| ErmGas | 1:c92781bb4d5e | 690 | mzl = content[4]; |
| ErmGas | 1:c92781bb4d5e | 691 | mzh = content[5]; |
| ErmGas | 1:c92781bb4d5e | 692 | |
| ErmGas | 1:c92781bb4d5e | 693 | |
| ErmGas | 1:c92781bb4d5e | 694 | mx=(int16_t)(mxh<<8|mxl); |
| ErmGas | 1:c92781bb4d5e | 695 | my=(int16_t)(myh<<8|myl); |
| ErmGas | 1:c92781bb4d5e | 696 | mz=(int16_t)(mzh<<8|mzl); |
| ErmGas | 1:c92781bb4d5e | 697 | // |
| ErmGas | 1:c92781bb4d5e | 698 | y_k[0] = (mx - min_mx)/(max_mx-min_mx)*2-1; |
| ErmGas | 1:c92781bb4d5e | 699 | y_k[1] = (my - min_my)/(max_my-min_my)*2-1; |
| ErmGas | 1:c92781bb4d5e | 700 | y_k[2] = current_pos_of_line; |
| ErmGas | 1:c92781bb4d5e | 701 | y_k[3] = gz/faktor; |
| ErmGas | 1:c92781bb4d5e | 702 | // step 7 x_hat(k+1|k+1) = x_hat(k+1|k) + K*(y(k+1) - y_hat(k+1|k) |
| ErmGas | 1:c92781bb4d5e | 703 | for (int i = 0; i<3; i++){ |
| ErmGas | 1:c92781bb4d5e | 704 | x_k_k[i] = x_kp1_k[i] + K[i][0]*(y_k[0] - y_kp1_k[0]) + K[i][1]*(y_k[1] - y_kp1_k[1]) + K[i][2]*(y_k[2] - y_kp1_k[2]) + K[i][3]*(y_k[3] - y_kp1_k[3]); |
| ErmGas | 1:c92781bb4d5e | 705 | } |
| ErmGas | 1:c92781bb4d5e | 706 | |
| ErmGas | 1:c92781bb4d5e | 707 | // step 8 P(k+1|k+1) = P(k+1|k) - K*H*P(k+1|k) |
| ErmGas | 1:c92781bb4d5e | 708 | // except of numerical problems, matlab proves the correctness here also |
| ErmGas | 1:c92781bb4d5e | 709 | for (int i = 0; i<3; i++){ |
| ErmGas | 1:c92781bb4d5e | 710 | for (int j = 0; j<3; j++){ |
| ErmGas | 1:c92781bb4d5e | 711 | P_k_k[i][j] = P_kp1_k[i][j] -(P_kp1_k[2][j]*(H[3][2]*K[i][3] + H[2][2]*K[i][2] + H[1][2]*K[i][1] + H[0][2]*K[i][0]) + P_kp1_k[1][j]*(H[3][1]*K[i][3] + H[2][1]*K[i][2] + H[1][1]*K[i][1] + H[0][1]*K[i][0]) + P_kp1_k[1][j]*(H[3][0]*K[i][3] + H[2][0]*K[i][2] + H[1][0]*K[i][1] + H[0][0]*K[i][0])); |
| ErmGas | 1:c92781bb4d5e | 712 | } |
| ErmGas | 1:c92781bb4d5e | 713 | } |
| ErmGas | 1:c92781bb4d5e | 714 | |
| ErmGas | 1:c92781bb4d5e | 715 | |
| ErmGas | 1:c92781bb4d5e | 716 | // read the sensors, values in [0,1000] with 1000=completely dark |
| ErmGas | 1:c92781bb4d5e | 717 | m3pi.readsensor(sensors); |
| ErmGas | 1:c92781bb4d5e | 718 | |
| ErmGas | 1:c92781bb4d5e | 719 | // startline is found if all sensor show black |
| ErmGas | 1:c92781bb4d5e | 720 | |
| ErmGas | 1:c92781bb4d5e | 721 | if (sensors[0]+sensors[1]+sensors[2]+sensors[3]+sensors[4]>4500 && clock()/CLOCKS_PER_SEC>=time+3){ |
| ErmGas | 1:c92781bb4d5e | 722 | rounds++; |
| ErmGas | 1:c92781bb4d5e | 723 | m3pi.playtune(dixie,numb); |
| ErmGas | 1:c92781bb4d5e | 724 | time=clock()/CLOCKS_PER_SEC; |
| ErmGas | 1:c92781bb4d5e | 725 | |
| ErmGas | 1:c92781bb4d5e | 726 | } |
| ErmGas | 1:c92781bb4d5e | 727 | } |
| ErmGas | 1:c92781bb4d5e | 728 | |
| ErmGas | 1:c92781bb4d5e | 729 | |
| ErmGas | 1:c92781bb4d5e | 730 | ---------------------------------------------------------------------------------------------------- |
| ErmGas | 1:c92781bb4d5e | 731 | End of out commented EKF-Loop |
| ErmGas | 1:c92781bb4d5e | 732 | ---------------------------------------------------------------------------------------------------- |
| ErmGas | 1:c92781bb4d5e | 733 | */ |
| ErmGas | 1:c92781bb4d5e | 734 | /* mapping is over, stay stuck in empty loop*/ |
| ErmGas | 1:c92781bb4d5e | 735 | m3pi.locate(0,1); |
| ErmGas | 1:c92781bb4d5e | 736 | m3pi.printf("EmptyEnd"); |
| ErmGas | 1:c92781bb4d5e | 737 | while(1) { |
| ErmGas | 1:c92781bb4d5e | 738 | // empty loop |
| ErmGas | 1:c92781bb4d5e | 739 | m3pi.left_motor(0.1); |
| ErmGas | 1:c92781bb4d5e | 740 | m3pi.right_motor(0); |
| ErmGas | 1:c92781bb4d5e | 741 | } |
| chris | 0:4e756c4c88a7 | 742 | |
| chris | 0:4e756c4c88a7 | 743 | } |