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
Diff: main.cpp
- Revision:
- 1:c92781bb4d5e
- Parent:
- 0:4e756c4c88a7
--- a/main.cpp Mon Sep 13 14:20:21 2010 +0000
+++ b/main.cpp Sat Jan 30 18:56:10 2016 +0000
@@ -1,12 +1,743 @@
#include "mbed.h"
+#include "m3pi_ng.h"
#include "MSCFileSystem.h"
+#include <string.h>
+#include <fstream>
+//#include <vector>
+#include <sstream>
+//#include <algorithm>
+//#include <functional>
+#include <math.h>
-MSCFileSystem fs ("fs");
+
+m3pi m3pi;
+MSCFileSystem msc("usb");
+I2C minimu9V3(p28,p27);
+
+#define pi 3.14159265358979323846
+/*
+----------------------------------------------------------------------------------------------------
+Completely commented out, needed for EKF, but is not working, because of the m3pi's small power
+----------------------------------------------------------------------------------------------------
+// Lookuptable with map from txt-file, will be facking slow at runtime
+float Lookuptable(float x_pos, float y_pos, float alpha, int size)
+{ ifstream map("/usb/wheelmap.txt");
+ m3pi.locate(0,1);
+ m3pi.printf("TestLoTa");
+ float dalpha = pi;
+ float POL = -1;
+ int counter = 0;
+ float alpha_corr = atan2(sin(alpha), cos(alpha));
+ float x_abs, y_abs, dist, dy, dx, alpha_map;
+ string ssx_abs, ssy_abs;
+ map>>ssx_abs;
+ map>>ssy_abs;
+ x_abs = std::atof(ssx_abs.c_str());
+ y_abs = std::atof(ssy_abs.c_str());
+ float dist_o = sqrt((x_abs-x_pos)*(x_abs-x_pos) + (y_abs-y_pos)*(y_abs-y_pos));
+ ssx_abs.clear();
+ ssy_abs.clear();
+ while (abs(dalpha)>pi/5 && counter< size){
+ string ssx_abs, ssy_abs;
+ map>>ssx_abs;
+ map>>ssy_abs;
+ x_abs = std::atof(ssx_abs.c_str());
+ y_abs = std::atof(ssy_abs.c_str());
+ dist = sqrt((x_abs-x_pos)*(x_abs-x_pos) + (y_abs-y_pos)*(y_abs-y_pos));
+ // if measurementradius is crossed, compute angle
+ if ( (dist >4.05 && dist_o <4.05) || (dist <4.05 && dist_o >4.05)){
+ dx = x_abs-x_pos;
+ dy = y_abs-y_pos;
+ alpha_map = atan2(dy, dx);
+ dalpha = alpha_corr - alpha_map;
+ // correct it, if the angle is obtuse
+ if (dalpha<-pi){
+ dalpha=2*pi + dalpha;
+ }
+ else if (dalpha>pi){
+ dalpha = 2*pi - dalpha;
+ }
+ }
+ // if the end of the txt is reached, we need to break out
+ counter++;
+ m3pi.locate(0,0);
+ m3pi.printf("%i",(float)dist);
+ ssx_abs.clear();
+ ssy_abs.clear();
+ }
+ if (abs(dalpha)<pi/5){POL = std::pow(abs(dalpha*5/pi),2.5) * dalpha/abs(dalpha);}
+ map.close();
+ return POL;
+
+
+ }
+// 4x4 Matrix inversion
+bool gluInvertMatrix(float M[4][4], float INVOut[4][4])
+{
+ double inv[16], m[16], invOut[16], det;
+ // conversion, because too lazy to change the whole code
+ for (int i = 0; i<4; i++){
+ for (int j = 0; j<4; j++){
+ m[4*i + j] = M[i][j];
+ }
+ }
+ int i;
+
+ inv[0] = m[5] * m[10] * m[15] -
+ m[5] * m[11] * m[14] -
+ m[9] * m[6] * m[15] +
+ m[9] * m[7] * m[14] +
+ m[13] * m[6] * m[11] -
+ m[13] * m[7] * m[10];
+
+ inv[4] = -m[4] * m[10] * m[15] +
+ m[4] * m[11] * m[14] +
+ m[8] * m[6] * m[15] -
+ m[8] * m[7] * m[14] -
+ m[12] * m[6] * m[11] +
+ m[12] * m[7] * m[10];
+
+ inv[8] = m[4] * m[9] * m[15] -
+ m[4] * m[11] * m[13] -
+ m[8] * m[5] * m[15] +
+ m[8] * m[7] * m[13] +
+ m[12] * m[5] * m[11] -
+ m[12] * m[7] * m[9];
+
+ inv[12] = -m[4] * m[9] * m[14] +
+ m[4] * m[10] * m[13] +
+ m[8] * m[5] * m[14] -
+ m[8] * m[6] * m[13] -
+ m[12] * m[5] * m[10] +
+ m[12] * m[6] * m[9];
+
+ inv[1] = -m[1] * m[10] * m[15] +
+ m[1] * m[11] * m[14] +
+ m[9] * m[2] * m[15] -
+ m[9] * m[3] * m[14] -
+ m[13] * m[2] * m[11] +
+ m[13] * m[3] * m[10];
+
+ inv[5] = m[0] * m[10] * m[15] -
+ m[0] * m[11] * m[14] -
+ m[8] * m[2] * m[15] +
+ m[8] * m[3] * m[14] +
+ m[12] * m[2] * m[11] -
+ m[12] * m[3] * m[10];
+
+ inv[9] = -m[0] * m[9] * m[15] +
+ m[0] * m[11] * m[13] +
+ m[8] * m[1] * m[15] -
+ m[8] * m[3] * m[13] -
+ m[12] * m[1] * m[11] +
+ m[12] * m[3] * m[9];
+
+ inv[13] = m[0] * m[9] * m[14] -
+ m[0] * m[10] * m[13] -
+ m[8] * m[1] * m[14] +
+ m[8] * m[2] * m[13] +
+ m[12] * m[1] * m[10] -
+ m[12] * m[2] * m[9];
+
+ inv[2] = m[1] * m[6] * m[15] -
+ m[1] * m[7] * m[14] -
+ m[5] * m[2] * m[15] +
+ m[5] * m[3] * m[14] +
+ m[13] * m[2] * m[7] -
+ m[13] * m[3] * m[6];
+
+ inv[6] = -m[0] * m[6] * m[15] +
+ m[0] * m[7] * m[14] +
+ m[4] * m[2] * m[15] -
+ m[4] * m[3] * m[14] -
+ m[12] * m[2] * m[7] +
+ m[12] * m[3] * m[6];
+
+ inv[10] = m[0] * m[5] * m[15] -
+ m[0] * m[7] * m[13] -
+ m[4] * m[1] * m[15] +
+ m[4] * m[3] * m[13] +
+ m[12] * m[1] * m[7] -
+ m[12] * m[3] * m[5];
+
+ inv[14] = -m[0] * m[5] * m[14] +
+ m[0] * m[6] * m[13] +
+ m[4] * m[1] * m[14] -
+ m[4] * m[2] * m[13] -
+ m[12] * m[1] * m[6] +
+ m[12] * m[2] * m[5];
+
+ inv[3] = -m[1] * m[6] * m[11] +
+ m[1] * m[7] * m[10] +
+ m[5] * m[2] * m[11] -
+ m[5] * m[3] * m[10] -
+ m[9] * m[2] * m[7] +
+ m[9] * m[3] * m[6];
+
+ inv[7] = m[0] * m[6] * m[11] -
+ m[0] * m[7] * m[10] -
+ m[4] * m[2] * m[11] +
+ m[4] * m[3] * m[10] +
+ m[8] * m[2] * m[7] -
+ m[8] * m[3] * m[6];
+
+ inv[11] = -m[0] * m[5] * m[11] +
+ m[0] * m[7] * m[9] +
+ m[4] * m[1] * m[11] -
+ m[4] * m[3] * m[9] -
+ m[8] * m[1] * m[7] +
+ m[8] * m[3] * m[5];
+
+ inv[15] = m[0] * m[5] * m[10] -
+ m[0] * m[6] * m[9] -
+ m[4] * m[1] * m[10] +
+ m[4] * m[2] * m[9] +
+ m[8] * m[1] * m[6] -
+ m[8] * m[2] * m[5];
+
+ det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
+
+ if (det == 0)
+ return false;
+
+ det = 1.0 / det;
+
+ for (i = 0; i < 16; i++)
+ invOut[i] = inv[i] * det;
+
+
+ for (int i = 0; i<4; i++){
+ for (int j = 0; j<4; j++){
+ INVOut[i][j]=invOut[4*i + j];
+ }
+ }
+ return true;
+}
+
+----------------------------------------------------------------------------------------------------
+End of out commented subfunctions
+----------------------------------------------------------------------------------------------------
+*/
+int main() {
+
+ /* PHASE 1 */
+
+ // beepsound
+ char dixie[]={'V','1','5','O','5','G','1','6','C','1','6'};
+ //the number of characters in the array
+ int numb=10;
+ float mu = 0.4; // mu between m3pi and ground
+ float g = 9.81; // you know it!
+ // system time
+ float time;
+ // sensorarray
+ int sensors [5] = {0, 0, 0, 0, 0};
+ // number of rounds driven with PID-Controller to map the track
+ int testrounds = 1;
+ // max velocity in phase 1
+ float speed1 = 0.1;
+ // how strong will the outer motor be slowed to get back on track / the inner motor be fastend
+ float correction = speed1;
+ // how far away from the black line is tolerable
+ float threshold = 0.2;
+
+ m3pi.locate(0,1); // x,y-Position on LCD
+ m3pi.printf("Line Flw"); // display output
+
+ // wait. REMIND: While waiting, the drive-order will be fullfilled
+ wait(2.0);
+
+ // robot turns left and right, looking for a line
+ m3pi.sensor_auto_calibrate();
+
+ // this first part should go on as long as the startline isn't crossed
+ int rounds = 0;
+ while (rounds == 0) {
+
+ // -1.0 is far left, 1.0 is far right, 0.0 in the middle
+ float position_of_line = m3pi.line_position();
+
+ // Line is more than the threshold to the right, slow the left motor
+ if (position_of_line > threshold) {
+ m3pi.left_motor(speed1+correction);
+ m3pi.right_motor(speed1-correction);
+ }
+
+ // Line is more than 50% to the left, slow the right motor
+ else if (position_of_line < -threshold) {
+ m3pi.right_motor(speed1+correction);
+ m3pi.left_motor(speed1-correction);
+ }
+
+ // Line is in the middle
+ else {
+ m3pi.forward(speed1);
+ }
+
+
+
+
+ // read the sensors, values in [0,1000] with 1000=completely dark
+ m3pi.readsensor(sensors);
+
+
+ // startline is found if all sensor show black
+ if (sensors[0]+sensors[1]+sensors[2]+sensors[3]+sensors[4]>4500){
+ rounds=1;
+ m3pi.playtune(dixie,numb);
+ time = clock()/CLOCKS_PER_SEC;
+
+
+ }
+ }
+
+ // startline is crossed now for the first time
+ // now its time to follow the track with PID-Controll while mapping it
+ ofstream testmap;
+ testmap.open ("/usb/map.txt");
+
+ /* LSM303D initialization*/
+ int sensor_addr_LSM = 0x3A; // 0x3C;
+ // request for x_l_m, results in sending: xlm,xhm,ylm,yhm,zlm,zhm
+ char START_MAG = 0x08;
+ // request for x_l_a, same as line 13
+ char START_ACC = 0x28;
+ // request for x_l_g, same as line 13
+ char START_GYRO = 0x28;
+ // empty array for readingvalues
+ char content[6] = {0x00,0x00,0x00,0x00,0x00,0x00};
+ // array for setting properties
+ char ctrl[2];
+ // set properties
+ ctrl[0] = 0x21; // propertyadress
+ ctrl[1] = 0x00; // AFS = 0
+ minimu9V3.write(sensor_addr_LSM, ctrl, 2);
+ ctrl[0] = 0x20; // propertyadress
+ ctrl[1] = 0x57; // = 0b01010111, AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)
+ minimu9V3.write(sensor_addr_LSM, ctrl, 2);
+ ctrl[0] = 0x24; // propertyadress
+ ctrl[1] = 0x64; // = 0b01100100, M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR)
+ minimu9V3.write(sensor_addr_LSM, ctrl, 2);
+ ctrl[0] = 0x25; // propertyadress
+ ctrl[1] = 0x20; // 0b00100000, MFS = 01 (+/- 4 gauss full scale)
+ minimu9V3.write(sensor_addr_LSM, ctrl, 2);
+ ctrl[0] = 0x26; // propertyadress
+ ctrl[1] = 0x00; // = 0b00100000, MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode)
+ minimu9V3.write(sensor_addr_LSM, ctrl, 2);
+ /* L3GD20H initialization*/
+ int sensor_addr_L3G = 0xD6;
+ // set properties
+ ctrl[0] = 0x39; // propertyadress
+ ctrl[1] = 0x00; // Low_ODR = 0 (low speed ODR disabled)
+ minimu9V3.write(sensor_addr_L3G,ctrl,2);
+ ctrl[0] = 0x23; // propertyadress
+ ctrl[1] = 0x00; // FS = 00 (+/- 250 dps full scale)
+ minimu9V3.write(sensor_addr_L3G,ctrl,2);
+ ctrl[0] = 0x20; // propertyadress
+ 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)
+ minimu9V3.write(sensor_addr_L3G,ctrl,2);
+
+ // MAY be unneeded
+ START_MAG |= 0x80;
+ START_ACC |= 0x80;
+ START_GYRO |= 0x80;
+
+ uint8_t axl, axh, ayl, ayh, azl, azh;
+ uint8_t mxl, mxh, myl, myh, mzl, mzh;
+ uint8_t gxl, gxh, gyl, gyh, gzl, gzh;
+ int16_t ax, ay, az;
+ int16_t mx, my, mz;
+ int16_t gx, gy, gz;
+
+ m3pi.locate(0,1);
+ m3pi.printf("Line PID");
-int main () {
+ // params for the PID-Controller
+ float P_TERM=1;
+ float I_TERM=0;
+ float D_TERM=20;
+
+ float speed2 = 5*speed1; // max velocity in phase 2
+ float right;
+ float left;
+ float current_pos_of_line = 0.0;
+ float previous_pos_of_line = 0.0;
+ float derivative,proportional,integral = 0;
+ float power;
+ float MAX = 2*speed2;
+ if(MAX>1) MAX = 1;
+ float MIN = 0;
+ // mapping variables
+ float x_abs = 0;
+ float y_abs = 0;
+ float alpha_abs = 0;
+ //float x_rel, dx_abs;
+ //float y_rel, dy_abs;
+ float min_mx=0;
+ float min_my=0;
+ float max_mx=0;
+ float max_my=0;
+ float faktor = 893562.2181291698;
+
+ float dt = 0.0046;
+
+ int size = 0;
+ while (rounds < testrounds+1){
+
+ // Get the position of the line.
+ current_pos_of_line = m3pi.line_position();
+ proportional = current_pos_of_line;
+
+ // Compute the derivative
+ derivative = current_pos_of_line - previous_pos_of_line;
+
+ // Compute the integral
+ integral += proportional;
+
+ // Remember the last position.
+ previous_pos_of_line = current_pos_of_line;
+
+ // Compute the power
+ power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
+
+ // Compute new speeds
+ right = speed2-power;
+ left = speed2+power;
+
+ // limit checks
+ if (right < MIN)
+ right = MIN;
+ else if (right > MAX)
+ right = MAX;
+
+ if (left < MIN)
+ left = MIN;
+ else if (left > MAX)
+ left = MAX;
+
+ // set speed
+ m3pi.left_motor(left);
+ m3pi.right_motor(right);
+
+ // sensors
+ minimu9V3.write(sensor_addr_L3G, &START_GYRO,1,true);
+ minimu9V3.read(sensor_addr_L3G, content, 6);
+
+ gxl = content[0];
+ gxh = content[1];
+ gyl = content[2];
+ gyh = content[3];
+ gzl = content[4];
+ gzh = content[5];
+
+
+ gx=(int16_t)(gxh<<8|gxl);
+ gy=(int16_t)(gyh<<8|gyl);
+ gz=(int16_t)(gzh<<8|gzl);
+
+ minimu9V3.write(sensor_addr_LSM, &START_MAG, 1, true);//, true
+ minimu9V3.read(sensor_addr_LSM, content,6);
+
+ mxl = content[0];
+ mxh = content[1];
+ myl = content[2];
+ myh = content[3];
+ mzl = content[4];
+ mzh = content[5];
+
+
+ mx=(int16_t)(mxh<<8|mxl);
+ my=(int16_t)(myh<<8|myl);
+ mz=(int16_t)(mzh<<8|mzl);
+
+ minimu9V3.write(sensor_addr_LSM, &START_ACC, 1, true);//, true
+ minimu9V3.read(sensor_addr_LSM, content, 6);
+ axl = content[0];
+ axh = content[1];
+ ayl = content[2];
+ ayh = content[3];
+ azl = content[4];
+ azh = content[5];
+
+
+ ax=(int16_t)(axh<<8|axl);
+ ay=(int16_t)(ayh<<8|ayl);
+ az=(int16_t)(azh<<8|azl);
+
+
+
+
+ testmap<<mx<<" "<<my<<" "<<current_pos_of_line<<" "<<gz<<" "<<left<<" "<<right<<" \n";
+
+ // read the sensors, values in [0,1000] with 1000=completely dark
+ m3pi.readsensor(sensors);
+
+ // startline is found if all sensor show black
+
+ if (sensors[0]+sensors[1]+sensors[2]+sensors[3]+sensors[4]>4500 && clock()/CLOCKS_PER_SEC>=time+3){
+ rounds++;
+ m3pi.playtune(dixie,numb);
+ time=clock()/CLOCKS_PER_SEC;
+ }
+ size++;
+
+ }
+ testmap.close();
+/*
+----------------------------------------------------------------------------------------------------
+Completely commented out, EKF-Loop, but is not working, because of the m3pi's small power
+----------------------------------------------------------------------------------------------------
+
+
+ m3pi.forward(0);
+ m3pi.locate(0,1);
- FILE *fp = fopen("/fs/hello.txt","w");
- fprintf(fp,"Hello world!\n");
- fclose (fp);
+ float x_kp1_k[3], x_k_k[3], y_kp1_k[4], y_k[4];
+ x_kp1_k[0]=0;
+ x_kp1_k[1]=0;
+ x_kp1_k[2]=0;
+ // F is Jakobi of x
+ float F[3][3];
+ F[0][0] = 1;//F11
+ F[0][1] = 0;//F12
+ F[1][0] = 0;//F21
+ F[1][1] = 1;//F22
+ F[2][0] = 0;//F31
+ F[2][1] = 0;//F32
+ F[2][2] = 1;//F33
+ // H is Jakobi of y
+ float H[4][3];
+ H[0][0] = 0;
+ H[0][1] = 0;
+ H[1][0] = 0;
+ H[1][1] = 0;
+ H[3][0] = 0;
+ H[3][1] = 0;
+ H[3][2] = -1/dt;
+ // P(k+1|k) = P11_kp1_k etc / P(k|k) = P11_k_k etc
+ float P_kp1_k[3][3], P_k_k[3][3];
+ P_k_k[0][0] = 1; // P0 is eye(3)
+ P_k_k[0][1] = 0;
+ P_k_k[0][2] = 0;
+ P_k_k[1][0] = 0;
+ P_k_k[1][1] = 1;
+ P_k_k[1][2] = 0;
+ P_k_k[2][0] = 0;
+ P_k_k[2][1] = 0;
+ P_k_k[2][2] = 1;
+ // Matrix Q
+ float Q[3][3];
+ Q[0][0] = 2.1289; // is result from optimization on simulation
+ Q[0][1] = 0.9727;
+ Q[0][2] = 0.2097;
+ Q[1][0] = 0.9727;
+ Q[1][1] = 1.6290;
+ Q[1][2] = 0.1533;
+ Q[2][0] = 0.2097;
+ Q[2][1] = 0.1533;
+ Q[2][2] = 0.0443;
+ // Helpmatrix M = HP(k+1|k)H + R
+ float M[4][4], invM[4][4];
+ // Matrix R
+ float R[4][4];
+ R[0][0] = 0.2039; // also result from optimization on simulation
+ R[0][1] = 0;
+ R[0][2] = 0;
+ R[0][3] = 0;
+ R[1][0] = 0;
+ R[1][1] = 0.2039;
+ R[1][2] = 0;
+ R[1][3] = 0;
+ R[2][0] = 0;
+ R[2][1] = 0;
+ R[2][2] = 0.5281;
+ R[2][3] = 0;
+ R[3][0] = 0;
+ R[3][1] = 0;
+ R[3][2] = 0;
+ R[3][3] = 0.0216;
+ // Matrix K
+ float K[3][4];
+ // speed for phase 3
+ float speed3 = speed2;
+ m3pi.forward(speed3);
+ while(rounds<testrounds+2){
+ // PID Stuff
+ // Get the position of the line.
+ current_pos_of_line = m3pi.line_position();
+ proportional = current_pos_of_line;
+
+ // Compute the derivative
+ derivative = current_pos_of_line - previous_pos_of_line;
+
+ // Compute the integral
+ integral += proportional;
+
+ // Remember the last position.
+ previous_pos_of_line = current_pos_of_line;
+
+ // Compute the power
+ power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
+
+ // Compute new speeds
+ right = speed3-power;
+ left = speed3+power;
+ // limit checks
+ if (right < MIN)
+ right = MIN;
+ else if (right > MAX)
+ right = MAX;
+
+ if (left < MIN)
+ left = MIN;
+ else if (left > MAX)
+ left = MAX;
+
+ // set speed
+ m3pi.left_motor(left);
+ m3pi.right_motor(right);
+ // Kalman Filter
+ // step 1 x_hat(k+1|k)=f(u,x_hat(k|k))
+ if (right>left){
+ 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]);
+ 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]);
+ x_kp1_k[2] = x_k_k[2] + (100*(right-left)/8.1*dt);
+ }
+ else if (left>right){
+ 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]);
+ 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]);
+ x_kp1_k[2] = x_k_k[2] - (100*(left-right)/8.1*dt);
+ }
+ else { // left == right
+ x_kp1_k[0] = x_k_k[0] + (100*right*dt)*cos(x_k_k[2]);
+ x_kp1_k[1] = x_k_k[1] + (100*right*dt)*sin(x_k_k[2]);
+ x_kp1_k[2] = x_k_k[2];
+ }
+ // step 2 y_hat(k+1|k)=h(u(k+1),x_hat(k+1|k))
+ y_kp1_k[0] = -cos(x_kp1_k[2]);
+ y_kp1_k[1] = -sin(x_kp1_k[2]);
+ y_kp1_k[2] = Lookuptable(x_kp1_k[0],x_kp1_k[1],x_kp1_k[2], size); // problems, no LUT implemented!
+ y_kp1_k[3] = -(x_kp1_k[2] - x_k_k[2])/dt;
+ // step 3 F is Jakobi of x
+ if (right>left){
+ 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]);
+ 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]);
+ }
+ else if (left>right){
+ 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]);
+ 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]);
+ }
+ else { // left == right
+ F[0][2] = (-1)*right*100*dt*sin(x_k_k[2]);
+ F[1][2] = 100*left*dt*cos(x_k_k[2]);
+ }
+ // step 4 H is Jakobi of y
+ float LoTa = Lookuptable(x_k_k[0],x_k_k[1],x_kp1_k[2], size);
+ H[0][2] = sin(x_k_k[2]);
+ H[1][2] = -cos(x_k_k[2]);
+ 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]);
+ 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]);
+ H[2][2] = std::pow((float)abs(LoTa),(float)(1.5/2.5))*2.5/pi*abs(LoTa)/LoTa;
+ // step 5 P(k+1|k) = F*P(k|k)*F' + Q;
+ // trust me, thats correct (says matlab)
+ for (int i = 0; i<3; i++){
+ for (int j = 0; j<3; j++){
+ 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];
+ }
+ }
+
+ // step 6 K = P(k+1|k)H' * (HP(k+1|k)H'+R)^(-1)
+ // same as above, blame matlab for errors!
+ // M = H*P(k+1|k)*H' + R
+ for (int i= 0; i<4; i++ ){
+ for (int j = 0; j<4; j++){
+ 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];
+ }
+ }
+ // same goes here, tested with MatLab!
+ gluInvertMatrix(M, invM);
+ // K = P(k+1|k) * H' * invM
+ for (int i= 0; i<3; i++ ){
+ for (int j = 0; j<4; j++){
+ 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]);
+ }
+ }
+
+ // Measurementupdate needs measurmentdata!
+ // sensors
+ minimu9V3.write(sensor_addr_L3G, &START_GYRO,1,true);
+ minimu9V3.read(sensor_addr_L3G, content, 6);
+
+ gxl = content[0];
+ gxh = content[1];
+ gyl = content[2];
+ gyh = content[3];
+ gzl = content[4];
+ gzh = content[5];
+
+
+ gx=(int16_t)(gxh<<8|gxl);
+ gy=(int16_t)(gyh<<8|gyl);
+ gz=(int16_t)(gzh<<8|gzl);
+
+ minimu9V3.write(sensor_addr_LSM, &START_MAG, 1, true);//, true
+ minimu9V3.read(sensor_addr_LSM, content,6);
+ //
+ mxl = content[0];
+ mxh = content[1];
+ myl = content[2];
+ myh = content[3];
+ mzl = content[4];
+ mzh = content[5];
+
+
+ mx=(int16_t)(mxh<<8|mxl);
+ my=(int16_t)(myh<<8|myl);
+ mz=(int16_t)(mzh<<8|mzl);
+ //
+ y_k[0] = (mx - min_mx)/(max_mx-min_mx)*2-1;
+ y_k[1] = (my - min_my)/(max_my-min_my)*2-1;
+ y_k[2] = current_pos_of_line;
+ y_k[3] = gz/faktor;
+ // step 7 x_hat(k+1|k+1) = x_hat(k+1|k) + K*(y(k+1) - y_hat(k+1|k)
+ for (int i = 0; i<3; i++){
+ 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]);
+ }
+
+ // step 8 P(k+1|k+1) = P(k+1|k) - K*H*P(k+1|k)
+ // except of numerical problems, matlab proves the correctness here also
+ for (int i = 0; i<3; i++){
+ for (int j = 0; j<3; j++){
+ 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]));
+ }
+ }
+
+
+ // read the sensors, values in [0,1000] with 1000=completely dark
+ m3pi.readsensor(sensors);
+
+ // startline is found if all sensor show black
+
+ if (sensors[0]+sensors[1]+sensors[2]+sensors[3]+sensors[4]>4500 && clock()/CLOCKS_PER_SEC>=time+3){
+ rounds++;
+ m3pi.playtune(dixie,numb);
+ time=clock()/CLOCKS_PER_SEC;
+
+ }
+ }
+
+
+----------------------------------------------------------------------------------------------------
+End of out commented EKF-Loop
+----------------------------------------------------------------------------------------------------
+*/
+ /* mapping is over, stay stuck in empty loop*/
+ m3pi.locate(0,1);
+ m3pi.printf("EmptyEnd");
+ while(1) {
+ // empty loop
+ m3pi.left_motor(0.1);
+ m3pi.right_motor(0);
+ }
}
\ No newline at end of file