Rifletool
Dependencies: Goldelox_SerialLCD HIH6130 MPL3115A2 MPU9150_DMP NeatGUI mbed
main.cpp@1:fd1fa09f69da, 2016-03-20 (annotated)
- Committer:
- Lockdog
- Date:
- Sun Mar 20 17:02:16 2016 +0000
- Revision:
- 1:fd1fa09f69da
- Parent:
- 0:399c828618ea
Rifletool
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Lockdog | 0:399c828618ea | 1 | #include "mbed.h" |
Lockdog | 1:fd1fa09f69da | 2 | //#include "Goldelox_Serial_4DLib.h" |
Lockdog | 1:fd1fa09f69da | 3 | #include "HIH6130.h" |
Lockdog | 1:fd1fa09f69da | 4 | |
Lockdog | 1:fd1fa09f69da | 5 | #include "I2Cdev.h" |
Lockdog | 1:fd1fa09f69da | 6 | #include "MPU6050_6Axis_MotionApps20.h" |
Lockdog | 1:fd1fa09f69da | 7 | //#include "MPU6050.h" |
Lockdog | 1:fd1fa09f69da | 8 | |
Lockdog | 1:fd1fa09f69da | 9 | #include "NeatGUI.h" |
Lockdog | 1:fd1fa09f69da | 10 | #include "MPL3115A2.h" |
Lockdog | 1:fd1fa09f69da | 11 | |
Lockdog | 1:fd1fa09f69da | 12 | #define LENGHT 20//0.06 |
Lockdog | 1:fd1fa09f69da | 13 | |
Lockdog | 1:fd1fa09f69da | 14 | #define WORK |
Lockdog | 1:fd1fa09f69da | 15 | |
Lockdog | 1:fd1fa09f69da | 16 | #define PIN_SDA p28 |
Lockdog | 1:fd1fa09f69da | 17 | #define PIN_SCL p27 |
Lockdog | 1:fd1fa09f69da | 18 | |
Lockdog | 1:fd1fa09f69da | 19 | const float M_PI = 3.14159265; |
Lockdog | 1:fd1fa09f69da | 20 | |
Lockdog | 1:fd1fa09f69da | 21 | //Black_wire On sf10 Laser Rangefinder - GND (Vss On Some Boards) |
Lockdog | 1:fd1fa09f69da | 22 | //Red_wire On sf10 Laser Rangefinder - +5V (Vdd On Some Boards) |
Lockdog | 1:fd1fa09f69da | 23 | //Yellow_wire On sf10 Laser Rangefinder - Arduino RX Pin (14) |
Lockdog | 1:fd1fa09f69da | 24 | //Orange_wire On sf10 Laser Rangefinder - Arduino TX Pin (13) |
Lockdog | 1:fd1fa09f69da | 25 | |
Lockdog | 1:fd1fa09f69da | 26 | InterruptIn Sen_1(p29); |
Lockdog | 1:fd1fa09f69da | 27 | InterruptIn Sen_2(p30); |
Lockdog | 1:fd1fa09f69da | 28 | InterruptIn MeasureF(p23); |
Lockdog | 1:fd1fa09f69da | 29 | InterruptIn mpuInt(p26); |
Lockdog | 1:fd1fa09f69da | 30 | |
Lockdog | 1:fd1fa09f69da | 31 | DigitalOut RangeKey(p22); |
Lockdog | 1:fd1fa09f69da | 32 | DigitalIn Button(p21); |
Lockdog | 1:fd1fa09f69da | 33 | |
Lockdog | 1:fd1fa09f69da | 34 | MPU6050 mpu; |
Lockdog | 1:fd1fa09f69da | 35 | |
Lockdog | 1:fd1fa09f69da | 36 | Serial pc(USBTX, USBRX); |
Lockdog | 1:fd1fa09f69da | 37 | Serial RangeFinder(p13, p14); |
Lockdog | 1:fd1fa09f69da | 38 | Timer MainClock; |
Lockdog | 1:fd1fa09f69da | 39 | DigitalOut myled(LED1); |
Lockdog | 1:fd1fa09f69da | 40 | I2C i2c(p9, p10); |
Lockdog | 1:fd1fa09f69da | 41 | MPL3115A2 mpl(&i2c); |
Lockdog | 1:fd1fa09f69da | 42 | HIH6130 hih6130(p9, p10); |
Lockdog | 1:fd1fa09f69da | 43 | SSD1351_SPI OLED128(p5, p6, p7, p8, p12); |
Lockdog | 0:399c828618ea | 44 | |
Lockdog | 1:fd1fa09f69da | 45 | volatile int time_stamp1, time_stamp2, flag, m_flag; |
Lockdog | 1:fd1fa09f69da | 46 | volatile float speed; |
Lockdog | 1:fd1fa09f69da | 47 | float result_time; |
Lockdog | 1:fd1fa09f69da | 48 | float dist; // The Laser Range Finder Distance Variable |
Lockdog | 1:fd1fa09f69da | 49 | char sf10_string[16], c; // Search the ASCII string from the sf10 using the second serial port |
Lockdog | 1:fd1fa09f69da | 50 | float humidity, temperature; |
Lockdog | 1:fd1fa09f69da | 51 | |
Lockdog | 1:fd1fa09f69da | 52 | int a_xBias, a_yBias; |
Lockdog | 1:fd1fa09f69da | 53 | int readings[3] = {0, 0, 0}; |
Lockdog | 1:fd1fa09f69da | 54 | float x,y,z; |
Lockdog | 1:fd1fa09f69da | 55 | Altitude alt; |
Lockdog | 1:fd1fa09f69da | 56 | double LastAngleX; |
Lockdog | 1:fd1fa09f69da | 57 | |
Lockdog | 1:fd1fa09f69da | 58 | double xAngle, yAngle; |
Lockdog | 1:fd1fa09f69da | 59 | |
Lockdog | 1:fd1fa09f69da | 60 | void RdShRange(void); |
Lockdog | 1:fd1fa09f69da | 61 | void DrawTable(void); |
Lockdog | 1:fd1fa09f69da | 62 | void MovePoint(void); |
Lockdog | 1:fd1fa09f69da | 63 | short getRGB(char red, char green, char blue); |
Lockdog | 1:fd1fa09f69da | 64 | |
Lockdog | 1:fd1fa09f69da | 65 | |
Lockdog | 1:fd1fa09f69da | 66 | // MPU control/status vars |
Lockdog | 1:fd1fa09f69da | 67 | bool dmpReady = false; // set true if DMP init was successful |
Lockdog | 1:fd1fa09f69da | 68 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU |
Lockdog | 1:fd1fa09f69da | 69 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) |
Lockdog | 1:fd1fa09f69da | 70 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) |
Lockdog | 1:fd1fa09f69da | 71 | uint16_t fifoCount; // count of all bytes currently in FIFO |
Lockdog | 1:fd1fa09f69da | 72 | uint8_t fifoBuffer[64]; // FIFO storage buffer |
Lockdog | 1:fd1fa09f69da | 73 | |
Lockdog | 1:fd1fa09f69da | 74 | // orientation/motion vars |
Lockdog | 1:fd1fa09f69da | 75 | Quaternion q; // [w, x, y, z] quaternion container |
Lockdog | 1:fd1fa09f69da | 76 | VectorInt16 aa; // [x, y, z] accel sensor measurements |
Lockdog | 1:fd1fa09f69da | 77 | VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements |
Lockdog | 1:fd1fa09f69da | 78 | VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements |
Lockdog | 1:fd1fa09f69da | 79 | VectorFloat gravity; // [x, y, z] gravity vector |
Lockdog | 1:fd1fa09f69da | 80 | float euler[3]; // [psi, theta, phi] Euler angle container |
Lockdog | 1:fd1fa09f69da | 81 | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector |
Lockdog | 0:399c828618ea | 82 | |
Lockdog | 0:399c828618ea | 83 | |
Lockdog | 1:fd1fa09f69da | 84 | volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high |
Lockdog | 1:fd1fa09f69da | 85 | void dmpDataReady() { |
Lockdog | 1:fd1fa09f69da | 86 | mpuInterrupt = true; |
Lockdog | 1:fd1fa09f69da | 87 | } |
Lockdog | 1:fd1fa09f69da | 88 | |
Lockdog | 1:fd1fa09f69da | 89 | void Time1() |
Lockdog | 1:fd1fa09f69da | 90 | { |
Lockdog | 1:fd1fa09f69da | 91 | if ((flag == 0)||(flag == -1)) |
Lockdog | 1:fd1fa09f69da | 92 | { |
Lockdog | 1:fd1fa09f69da | 93 | time_stamp1 = MainClock.read_us(); |
Lockdog | 1:fd1fa09f69da | 94 | flag = 1; |
Lockdog | 1:fd1fa09f69da | 95 | } |
Lockdog | 1:fd1fa09f69da | 96 | } |
Lockdog | 0:399c828618ea | 97 | |
Lockdog | 1:fd1fa09f69da | 98 | void Time2() |
Lockdog | 1:fd1fa09f69da | 99 | { |
Lockdog | 1:fd1fa09f69da | 100 | if (flag == 1) |
Lockdog | 1:fd1fa09f69da | 101 | { |
Lockdog | 1:fd1fa09f69da | 102 | time_stamp2 = MainClock.read_us(); |
Lockdog | 1:fd1fa09f69da | 103 | flag = 2; |
Lockdog | 1:fd1fa09f69da | 104 | result_time = (time_stamp2 - time_stamp1)*0.000001; |
Lockdog | 1:fd1fa09f69da | 105 | speed = (LENGHT/result_time)*3.2808; |
Lockdog | 1:fd1fa09f69da | 106 | //if (m_flag == 0) speed = (LENGHT/result_time)*3.2808; //fps |
Lockdog | 1:fd1fa09f69da | 107 | // else speed = (LENGHT/result_time); //m/s |
Lockdog | 1:fd1fa09f69da | 108 | } |
Lockdog | 1:fd1fa09f69da | 109 | } |
Lockdog | 1:fd1fa09f69da | 110 | |
Lockdog | 1:fd1fa09f69da | 111 | void ChangeMF() |
Lockdog | 1:fd1fa09f69da | 112 | { |
Lockdog | 1:fd1fa09f69da | 113 | if (m_flag == 0) m_flag = 1; else m_flag = 0; |
Lockdog | 1:fd1fa09f69da | 114 | } |
Lockdog | 0:399c828618ea | 115 | |
Lockdog | 0:399c828618ea | 116 | int main() { |
Lockdog | 1:fd1fa09f69da | 117 | char buf[30]; |
Lockdog | 1:fd1fa09f69da | 118 | flag = -1; |
Lockdog | 1:fd1fa09f69da | 119 | m_flag = 0; |
Lockdog | 1:fd1fa09f69da | 120 | speed = 0; |
Lockdog | 1:fd1fa09f69da | 121 | RangeKey = 0; |
Lockdog | 1:fd1fa09f69da | 122 | MainClock.start(); |
Lockdog | 1:fd1fa09f69da | 123 | |
Lockdog | 1:fd1fa09f69da | 124 | Sen_1.rise(&Time1); |
Lockdog | 1:fd1fa09f69da | 125 | Sen_2.rise(&Time2); |
Lockdog | 1:fd1fa09f69da | 126 | MeasureF.rise(ChangeMF); |
Lockdog | 1:fd1fa09f69da | 127 | RangeFinder.baud(19200); |
Lockdog | 1:fd1fa09f69da | 128 | mpu.initialize(); |
Lockdog | 1:fd1fa09f69da | 129 | pc.printf("Testing device connections...\r\n"); |
Lockdog | 1:fd1fa09f69da | 130 | if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n"); |
Lockdog | 1:fd1fa09f69da | 131 | else pc.printf("MPU6050 connection failed\r\n"); |
Lockdog | 1:fd1fa09f69da | 132 | mpl.init(); |
Lockdog | 1:fd1fa09f69da | 133 | mpl.setModeStandby(); |
Lockdog | 1:fd1fa09f69da | 134 | mpl.setModeAltimeter(); |
Lockdog | 1:fd1fa09f69da | 135 | mpl.setModeActive(); |
Lockdog | 1:fd1fa09f69da | 136 | OLED128.open(); |
Lockdog | 1:fd1fa09f69da | 137 | OLED128.state(Display::DISPLAY_ON); |
Lockdog | 1:fd1fa09f69da | 138 | OLED128.drawCircle(10,10,5,0xff345463); |
Lockdog | 1:fd1fa09f69da | 139 | |
Lockdog | 1:fd1fa09f69da | 140 | // supply your own gyro offsets here, scaled for min sensitivity |
Lockdog | 1:fd1fa09f69da | 141 | //mpu.setXGyroOffset(220); |
Lockdog | 1:fd1fa09f69da | 142 | //mpu.setYGyroOffset(76); |
Lockdog | 1:fd1fa09f69da | 143 | //mpu.setZGyroOffset(-85); |
Lockdog | 1:fd1fa09f69da | 144 | //mpu.setZAccelOffset(1788); // 1688 factory default for my test chip |
Lockdog | 1:fd1fa09f69da | 145 | devStatus = mpu.dmpInitialize(); |
Lockdog | 1:fd1fa09f69da | 146 | |
Lockdog | 1:fd1fa09f69da | 147 | // make sure it worked (returns 0 if so) |
Lockdog | 1:fd1fa09f69da | 148 | if (devStatus == 0) { |
Lockdog | 1:fd1fa09f69da | 149 | // turn on the DMP, now that it's ready |
Lockdog | 1:fd1fa09f69da | 150 | pc.printf("Enabling DMP...\r\n"); |
Lockdog | 1:fd1fa09f69da | 151 | mpu.setDMPEnabled(true); |
Lockdog | 1:fd1fa09f69da | 152 | |
Lockdog | 1:fd1fa09f69da | 153 | // enable Arduino interrupt detection |
Lockdog | 1:fd1fa09f69da | 154 | pc.printf("Enabling interrupt detection...\r\n"); |
Lockdog | 1:fd1fa09f69da | 155 | mpuInt.rise(&dmpDataReady); |
Lockdog | 1:fd1fa09f69da | 156 | mpuIntStatus = mpu.getIntStatus(); |
Lockdog | 1:fd1fa09f69da | 157 | // set our DMP Ready flag so the main loop() function knows it's okay to use it |
Lockdog | 1:fd1fa09f69da | 158 | pc.printf("DMP ready! Waiting for first interrupt...\r\n"); |
Lockdog | 1:fd1fa09f69da | 159 | dmpReady = true; |
Lockdog | 1:fd1fa09f69da | 160 | |
Lockdog | 1:fd1fa09f69da | 161 | // get expected DMP packet size for later comparison |
Lockdog | 1:fd1fa09f69da | 162 | packetSize = mpu.dmpGetFIFOPacketSize(); |
Lockdog | 1:fd1fa09f69da | 163 | } else { |
Lockdog | 1:fd1fa09f69da | 164 | // ERROR! |
Lockdog | 1:fd1fa09f69da | 165 | // 1 = initial memory load failed |
Lockdog | 1:fd1fa09f69da | 166 | // 2 = DMP configuration updates failed |
Lockdog | 1:fd1fa09f69da | 167 | // (if it's going to break, usually the code will be 1) |
Lockdog | 1:fd1fa09f69da | 168 | |
Lockdog | 1:fd1fa09f69da | 169 | pc.printf("DDMP Initialization failed (code "); |
Lockdog | 1:fd1fa09f69da | 170 | pc.printf("%d", devStatus); |
Lockdog | 1:fd1fa09f69da | 171 | pc.printf(")\r\n"); |
Lockdog | 1:fd1fa09f69da | 172 | } |
Lockdog | 1:fd1fa09f69da | 173 | // get expected DMP packet size for later comparison |
Lockdog | 1:fd1fa09f69da | 174 | pc.printf("Ready\r\n"); |
Lockdog | 0:399c828618ea | 175 | while(1) { |
Lockdog | 1:fd1fa09f69da | 176 | // while (!mpuInterrupt && fifoCount < packetSize) { |
Lockdog | 1:fd1fa09f69da | 177 | if (flag == 2) |
Lockdog | 1:fd1fa09f69da | 178 | { |
Lockdog | 1:fd1fa09f69da | 179 | pc.printf("Speed: %.0f fps\r\n",speed); |
Lockdog | 1:fd1fa09f69da | 180 | flag = 0; |
Lockdog | 1:fd1fa09f69da | 181 | |
Lockdog | 1:fd1fa09f69da | 182 | } |
Lockdog | 1:fd1fa09f69da | 183 | /* |
Lockdog | 1:fd1fa09f69da | 184 | if (flag != -1) |
Lockdog | 1:fd1fa09f69da | 185 | { |
Lockdog | 1:fd1fa09f69da | 186 | if (m_flag == 0) speed = (LENGHT/result_time)*3.2808; //fps |
Lockdog | 1:fd1fa09f69da | 187 | else speed = (LENGHT/result_time); //m/s |
Lockdog | 1:fd1fa09f69da | 188 | sprintf(buf,"%4.0f\r\n",speed); |
Lockdog | 1:fd1fa09f69da | 189 | }*/ |
Lockdog | 1:fd1fa09f69da | 190 | //RdShRange(); |
Lockdog | 1:fd1fa09f69da | 191 | //hih6130.ReadData(&temperature, &humidity); |
Lockdog | 1:fd1fa09f69da | 192 | //mpl.readAltitude(&alt); |
Lockdog | 1:fd1fa09f69da | 193 | |
Lockdog | 1:fd1fa09f69da | 194 | memset(buf,0,30); |
Lockdog | 1:fd1fa09f69da | 195 | if (m_flag==0) sprintf(buf,"%.1f yd ",dist*1.0936); //yards |
Lockdog | 1:fd1fa09f69da | 196 | else sprintf(buf,"%.1f m ",dist); //meters |
Lockdog | 1:fd1fa09f69da | 197 | |
Lockdog | 1:fd1fa09f69da | 198 | wait(0.4); |
Lockdog | 1:fd1fa09f69da | 199 | // } |
Lockdog | 1:fd1fa09f69da | 200 | mpuInterrupt = false; |
Lockdog | 1:fd1fa09f69da | 201 | mpuIntStatus = mpu.getIntStatus(); |
Lockdog | 1:fd1fa09f69da | 202 | |
Lockdog | 1:fd1fa09f69da | 203 | // get current FIFO count |
Lockdog | 1:fd1fa09f69da | 204 | fifoCount = mpu.getFIFOCount(); |
Lockdog | 1:fd1fa09f69da | 205 | |
Lockdog | 1:fd1fa09f69da | 206 | // check for overflow (this should never happen unless our code is too inefficient) |
Lockdog | 1:fd1fa09f69da | 207 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) { |
Lockdog | 1:fd1fa09f69da | 208 | // reset so we can continue cleanly |
Lockdog | 1:fd1fa09f69da | 209 | mpu.resetFIFO(); |
Lockdog | 1:fd1fa09f69da | 210 | |
Lockdog | 1:fd1fa09f69da | 211 | // otherwise, check for DMP data ready interrupt (this should happen frequently) |
Lockdog | 1:fd1fa09f69da | 212 | } else if (mpuIntStatus & 0x02) { |
Lockdog | 1:fd1fa09f69da | 213 | // wait for correct available data length, should be a VERY short wait |
Lockdog | 1:fd1fa09f69da | 214 | while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); |
Lockdog | 1:fd1fa09f69da | 215 | |
Lockdog | 1:fd1fa09f69da | 216 | // read a packet from FIFO |
Lockdog | 1:fd1fa09f69da | 217 | mpu.getFIFOBytes(fifoBuffer, packetSize); |
Lockdog | 1:fd1fa09f69da | 218 | |
Lockdog | 1:fd1fa09f69da | 219 | // track FIFO count here in case there is > 1 packet available |
Lockdog | 1:fd1fa09f69da | 220 | // (this lets us immediately read more without waiting for an interrupt) |
Lockdog | 1:fd1fa09f69da | 221 | fifoCount -= packetSize; |
Lockdog | 1:fd1fa09f69da | 222 | |
Lockdog | 1:fd1fa09f69da | 223 | // display Euler angles in degrees |
Lockdog | 1:fd1fa09f69da | 224 | mpu.dmpGetQuaternion(&q, fifoBuffer); |
Lockdog | 1:fd1fa09f69da | 225 | mpu.dmpGetGravity(&gravity, &q); |
Lockdog | 1:fd1fa09f69da | 226 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); |
Lockdog | 1:fd1fa09f69da | 227 | } |
Lockdog | 1:fd1fa09f69da | 228 | |
Lockdog | 1:fd1fa09f69da | 229 | 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); |
Lockdog | 0:399c828618ea | 230 | } |
Lockdog | 0:399c828618ea | 231 | } |
Lockdog | 1:fd1fa09f69da | 232 | |
Lockdog | 1:fd1fa09f69da | 233 | void RdShRange(void) |
Lockdog | 1:fd1fa09f69da | 234 | { |
Lockdog | 1:fd1fa09f69da | 235 | RangeFinder.putc('d'); // Send "D" trigger to receive data back from sensor |
Lockdog | 1:fd1fa09f69da | 236 | while (!RangeFinder.readable()); // Wait until the next distance measurement is ready |
Lockdog | 1:fd1fa09f69da | 237 | // Prepare to read the serial port... |
Lockdog | 1:fd1fa09f69da | 238 | int i=0; // indexer for the string storage variable |
Lockdog | 1:fd1fa09f69da | 239 | int c=0; // c holds the latest ASCII character from the sf10 |
Lockdog | 1:fd1fa09f69da | 240 | |
Lockdog | 1:fd1fa09f69da | 241 | while(c != 10) // Read the ASCII string from the sf10 until a line feed character (\n) is detected |
Lockdog | 1:fd1fa09f69da | 242 | { |
Lockdog | 1:fd1fa09f69da | 243 | while (!RangeFinder.readable()); // Wait here for the next character |
Lockdog | 1:fd1fa09f69da | 244 | c = RangeFinder.getc(); // store the values in c |
Lockdog | 1:fd1fa09f69da | 245 | sf10_string[i] = c; // Add the character to the existing string from the sf10 |
Lockdog | 1:fd1fa09f69da | 246 | i++; // Add the next character, until full string is captured |
Lockdog | 1:fd1fa09f69da | 247 | } // Once the string has been captured.. |
Lockdog | 1:fd1fa09f69da | 248 | sf10_string[i-2] = 0; // Create a null terminated string and remove the \r\n characters from the end |
Lockdog | 1:fd1fa09f69da | 249 | dist = atof(sf10_string); // Convert the ASCII string to distance in meters |
Lockdog | 1:fd1fa09f69da | 250 | } |
Lockdog | 1:fd1fa09f69da | 251 | |
Lockdog | 1:fd1fa09f69da | 252 | |
Lockdog | 1:fd1fa09f69da | 253 | |
Lockdog | 1:fd1fa09f69da | 254 | short getRGB(char red, char green, char blue) { |
Lockdog | 1:fd1fa09f69da | 255 | int outR = ((red * 31) / 255); |
Lockdog | 1:fd1fa09f69da | 256 | int outG = ((green * 63) / 255); |
Lockdog | 1:fd1fa09f69da | 257 | int outB = ((blue * 31) / 255); |
Lockdog | 1:fd1fa09f69da | 258 | |
Lockdog | 1:fd1fa09f69da | 259 | return (outR << 11) | (outG << 5) | outB; |
Lockdog | 1:fd1fa09f69da | 260 | } |