Diff: main.cpp

Revision:

1:fd1fa09f69da

Parent:

0:399c828618ea

--- a/main.cpp	Tue Aug 04 07:02:25 2015 +0000
+++ b/main.cpp	Sun Mar 20 17:02:16 2016 +0000
@@ -1,26 +1,260 @@
 #include "mbed.h"
-#include "ADXL345.h"
-#include "SSD1308.h"
+//#include "Goldelox_Serial_4DLib.h"
+#include "HIH6130.h"
+
+#include "I2Cdev.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+//#include "MPU6050.h"
+
+#include "NeatGUI.h"
+#include "MPL3115A2.h"
+
+#define LENGHT 20//0.06 
+
+#define WORK
+
+#define PIN_SDA p28
+#define PIN_SCL p27
+
+const float M_PI = 3.14159265;
+
+//Black_wire  On sf10 Laser Rangefinder - GND (Vss On Some Boards)
+//Red_wire    On sf10 Laser Rangefinder - +5V (Vdd On Some Boards)
+//Yellow_wire On sf10 Laser Rangefinder - Arduino RX Pin (14)
+//Orange_wire On sf10 Laser Rangefinder - Arduino TX Pin (13)
+
+InterruptIn Sen_1(p29);
+InterruptIn Sen_2(p30);
+InterruptIn MeasureF(p23);
+InterruptIn mpuInt(p26);
+
+DigitalOut RangeKey(p22);
+DigitalIn Button(p21);
+
+MPU6050 mpu;
+
+Serial pc(USBTX, USBRX);
+Serial RangeFinder(p13, p14);
+Timer MainClock;
+DigitalOut myled(LED1);
+I2C i2c(p9, p10);
+MPL3115A2 mpl(&i2c);
+HIH6130 hih6130(p9, p10);
+SSD1351_SPI OLED128(p5, p6, p7, p8, p12);
  
-//Pin Defines for I2C Bus
-#define D_SDA                  p28
-#define D_SCL                  p27
+volatile int time_stamp1, time_stamp2, flag, m_flag;
+volatile float speed;
+float result_time;
+float dist;                                                        // The Laser Range Finder Distance Variable
+char sf10_string[16], c;                                               // Search the ASCII string from the sf10 using the second serial port
+float humidity, temperature;
+
+int   a_xBias, a_yBias;
+int readings[3] = {0, 0, 0};
+float x,y,z;
+Altitude alt;
+double LastAngleX;
+
+double xAngle, yAngle;
+
+void RdShRange(void);
+void DrawTable(void);
+void MovePoint(void);
+short getRGB(char red, char green, char blue);
+
+
+// MPU control/status vars
+bool dmpReady = false;  // set true if DMP init was successful
+uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
+uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount;     // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64]; // FIFO storage buffer
+
+// orientation/motion vars
+Quaternion q;           // [w, x, y, z]         quaternion container
+VectorInt16 aa;         // [x, y, z]            accel sensor measurements
+VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
+VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
+VectorFloat gravity;    // [x, y, z]            gravity vector
+float euler[3];         // [psi, theta, phi]    Euler angle container
+float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
 
 
-I2C i2c(D_SDA, D_SCL);
-DigitalOut RangeKey(p9);
-ADXL345 accelerometer(p5, p6, p7, p8);
-Serial pc(USBTX, USBRX);
-Serial RangeFinder(p13, p14);
+volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
+void dmpDataReady() {
+    mpuInterrupt = true;
+}
+
+void Time1()
+{
+    if ((flag == 0)||(flag == -1))
+    {
+        time_stamp1 = MainClock.read_us();
+        flag = 1;
+    }
+}
 
-SSD1308 oled = SSD1308(i2c, SSD1308_SA0);
+void Time2()
+{
+    if (flag == 1)
+    {
+        time_stamp2 = MainClock.read_us();
+        flag = 2;
+        result_time = (time_stamp2 - time_stamp1)*0.000001;
+        speed = (LENGHT/result_time)*3.2808;
+        //if (m_flag == 0) speed = (LENGHT/result_time)*3.2808; //fps
+        // else  speed = (LENGHT/result_time); //m/s         
+    }
+}
+
+void ChangeMF()
+{
+    if (m_flag == 0) m_flag = 1; else m_flag = 0;
+}
 
 int main() {
-    oled.writeString(0, 0, "Hello World !");
+    char buf[30];
+    flag = -1;
+    m_flag = 0;
+    speed = 0;
+    RangeKey = 0;
+    MainClock.start();
+    
+    Sen_1.rise(&Time1);
+    Sen_2.rise(&Time2);
+    MeasureF.rise(ChangeMF);
+    RangeFinder.baud(19200);
+    mpu.initialize();
+    pc.printf("Testing device connections...\r\n");
+    if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n");
+    else pc.printf("MPU6050 connection failed\r\n");
+    mpl.init();
+    mpl.setModeStandby();
+    mpl.setModeAltimeter();
+    mpl.setModeActive();
+    OLED128.open();
+    OLED128.state(Display::DISPLAY_ON);
+    OLED128.drawCircle(10,10,5,0xff345463);
+    
+    // supply your own gyro offsets here, scaled for min sensitivity
+    //mpu.setXGyroOffset(220);
+    //mpu.setYGyroOffset(76);
+    //mpu.setZGyroOffset(-85);
+    //mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
+     devStatus = mpu.dmpInitialize();
+    
+    // make sure it worked (returns 0 if so)
+    if (devStatus == 0) {
+        // turn on the DMP, now that it's ready
+        pc.printf("Enabling DMP...\r\n");
+        mpu.setDMPEnabled(true);
+ 
+        // enable Arduino interrupt detection
+        pc.printf("Enabling interrupt detection...\r\n");
+        mpuInt.rise(&dmpDataReady);
+        mpuIntStatus = mpu.getIntStatus();
+        // set our DMP Ready flag so the main loop() function knows it's okay to use it
+        pc.printf("DMP ready! Waiting for first interrupt...\r\n");
+        dmpReady = true;
+ 
+        // get expected DMP packet size for later comparison
+        packetSize = mpu.dmpGetFIFOPacketSize();
+    } else {
+        // ERROR!
+        // 1 = initial memory load failed
+        // 2 = DMP configuration updates failed
+        // (if it's going to break, usually the code will be 1)
+        
+        pc.printf("DDMP Initialization failed (code ");
+        pc.printf("%d", devStatus);
+        pc.printf(")\r\n");
+    }
+    // get expected DMP packet size for later comparison
+    pc.printf("Ready\r\n");
     while(1) {
-        //myled = 1;
-        //wait(0.2);
-        //myled = 0;
-        //wait(0.2);
+       // while (!mpuInterrupt && fifoCount < packetSize) {
+        if (flag == 2)
+        {        
+            pc.printf("Speed: %.0f fps\r\n",speed);
+            flag = 0;
+
+        }
+        /*
+        if (flag != -1)
+        {
+            if (m_flag == 0) speed = (LENGHT/result_time)*3.2808; //fps
+            else  speed = (LENGHT/result_time); //m/s  
+            sprintf(buf,"%4.0f\r\n",speed);
+        }*/
+    //RdShRange();
+    //hih6130.ReadData(&temperature, &humidity);
+    //mpl.readAltitude(&alt);
+        
+    memset(buf,0,30);
+    if (m_flag==0) sprintf(buf,"%.1f yd     ",dist*1.0936); //yards
+    else sprintf(buf,"%.1f m      ",dist); //meters
+         
+    wait(0.4);      
+    // }
+    mpuInterrupt = false;
+    mpuIntStatus = mpu.getIntStatus();
+
+    // get current FIFO count
+    fifoCount = mpu.getFIFOCount();
+
+    // check for overflow (this should never happen unless our code is too inefficient)
+    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
+        // reset so we can continue cleanly
+        mpu.resetFIFO();
+
+    // otherwise, check for DMP data ready interrupt (this should happen frequently)
+    } else if (mpuIntStatus & 0x02) {
+        // wait for correct available data length, should be a VERY short wait
+        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
+
+        // read a packet from FIFO
+        mpu.getFIFOBytes(fifoBuffer, packetSize);
+        
+        // track FIFO count here in case there is > 1 packet available
+        // (this lets us immediately read more without waiting for an interrupt)
+        fifoCount -= packetSize;
+
+            // display Euler angles in degrees
+            mpu.dmpGetQuaternion(&q, fifoBuffer);
+            mpu.dmpGetGravity(&gravity, &q);
+            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+       } 
+        
+    pc.printf("Distance: %.1fyd;  Temperature is: %.1fC, X:%.1f, Y:%.1f, Altitude:%s Feet, Humidity:%.1f\r\n", dist*1.0936, temperature, ypr[0] * 180/M_PI, ypr[1] * 180/M_PI, alt.print(), humidity);//xAngle+4, yAngle-7);     
     }
 }
+
+void RdShRange(void)
+{
+    RangeFinder.putc('d');   // Send "D" trigger to receive data back from sensor
+  while (!RangeFinder.readable());    // Wait until the next distance measurement is ready
+                                                                        // Prepare to read the serial port...
+  int i=0;                                                              // indexer for the string storage variable
+  int c=0;                                                              // c holds the latest ASCII character from the sf10
+  
+  while(c != 10)    // Read the ASCII string from the sf10 until a line feed character (\n) is detected
+  {
+    while (!RangeFinder.readable());       // Wait here for the next character
+    c = RangeFinder.getc();                                             // store the values in c
+    sf10_string[i] = c;                                                 // Add the character to the existing string from the sf10
+    i++;                                                                // Add the next character, until full string is captured
+  }                                                                     // Once the string has been captured..
+  sf10_string[i-2] = 0;                                                 // Create a null terminated string and remove the \r\n characters from the end 
+  dist = atof(sf10_string);   // Convert the ASCII string to distance in meters                                    
+}
+
+
+
+short getRGB(char red, char green, char blue) {
+    int outR = ((red * 31) / 255);
+    int outG = ((green * 63) / 255);
+    int outB = ((blue * 31) / 255);
+    
+    return (outR << 11) | (outG << 5) | outB;
+}
\ No newline at end of file