Diff: main.cpp

Revision:

1:ea4ac32aabf9

Parent:

0:4e756c4c88a7

--- a/main.cpp	Mon Sep 13 14:20:21 2010 +0000
+++ b/main.cpp	Sat Jan 30 22:28:14 2016 +0000
@@ -1,12 +1,161 @@
 #include "mbed.h"
+#include "m3pi_ng.h"
 #include "MSCFileSystem.h"
+#include <string.h>
+#include <fstream>
+#include <sstream>
+#include <math.h>
 
-MSCFileSystem fs ("fs");
+
+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");
 
-int main () {
+    /* 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);
 
-    FILE *fp = fopen("/fs/hello.txt","w");
-    fprintf(fp,"Hello world!\n");
-    fclose (fp);
+       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);  
+        }
 
 }
\ No newline at end of file