File content as of revision 1:ea4ac32aabf9:

#include "mbed.h"
#include "m3pi_ng.h"
#include "MSCFileSystem.h"
#include <string.h>
#include <fstream>
#include <sstream>
#include <math.h>


m3pi m3pi;
MSCFileSystem msc("usb");
I2C minimu9V3(p28,p27);

#define pi 3.14159265358979323846

int main() {


         
    // beepsound
    char dixie[]={'V','1','5','O','5','G','1','6','C','1','6'};  
    //the number of characters in the array
    int numb=10;

    ofstream testmap;
    testmap.open ("/usb/rauschen.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);
    int sensors [5] = {0, 0, 0, 0, 0}; 
    // 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 var = 0;
    wait(2.0);
    m3pi.playtune(dixie,numb);
    float time=clock()/CLOCKS_PER_SEC;
    while(var < 1){
        float position_of_line = m3pi.line_position();
        // 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<<" "<<position_of_line<<" "<<gz<<" \n";
       testmap<<position_of_line<<" \n";
       // end the loop if startline for manual ending
       // m3pi.readsensor(sensors);
        

        // 20 seconds are over
        if ( clock()/CLOCKS_PER_SEC > time + 20){
            var=2; 
            m3pi.playtune(dixie,numb);
        
        }
    }
    testmap.close();

    /* mapping is over, stay stuck in empty loop*/
    m3pi.locate(0,1);
    m3pi.printf("EmptyEnd");
    while(1) {
      // empty loop 
        m3pi.left_motor(0);
        m3pi.right_motor(0);  
        }

}