Anatoly Zimin
Rifletool
Dependencies: Goldelox_SerialLCD HIH6130 MPL3115A2 MPU9150_DMP NeatGUI mbed
Revision 1:fd1fa09f69da, committed 2016-03-20
- Comitter:
- Lockdog
- Date:
- Sun Mar 20 17:02:16 2016 +0000
- Parent:
- 0:399c828618ea
- Commit message:
- Rifletool
Changed in this revision
--- a/ADXL345.lib Tue Aug 04 07:02:25 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/aberk/code/ADXL345/#bd8f0f20f433
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Goldelox_SerialLCD.lib Sun Mar 20 17:02:16 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sailing_Nut/code/Goldelox_SerialLCD/#6987c004e123
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HIH6130.lib Sun Mar 20 17:02:16 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/spiridion/code/HIH6130/#ed5a906c8e44
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPL3115A2.lib Sun Mar 20 17:02:16 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/sophtware/code/MPL3115A2/#7c7c1ea6fc33
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.lib Sun Mar 20 17:02:16 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/syundo0730/code/MPU6050-DMP/#7d1bf3ce0053
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU9150_DMP.lib Sun Mar 20 17:02:16 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/p3p/code/MPU9150_DMP/#e523a92390b6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/NeatGUI.lib Sun Mar 20 17:02:16 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/neilt6/code/NeatGUI/#a8f72d4864e6
--- a/SSD1308_128x64_I2C.lib Tue Aug 04 07:02:25 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/wim/code/SSD1308_128x64_I2C/#df92b0c0cb92
--- 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