Rifletool
Dependencies: Goldelox_SerialLCD HIH6130 MPL3115A2 MPU9150_DMP NeatGUI mbed
main.cpp
- Committer:
- Lockdog
- Date:
- 2016-03-20
- Revision:
- 1:fd1fa09f69da
- Parent:
- 0:399c828618ea
File content as of revision 1:fd1fa09f69da:
#include "mbed.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); 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 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; } } 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() { 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) { // 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; }