Team Walter / Mbed OS Low_Power_Long_Distance_IR_Vision_Robot

LP Long Distance IR Vision Robot

Dependencies:   max77650_charger_sample BufferedSerial SX1276GenericLib Adafruit-MotorShield NEO-6m-GPS MAX17055_EZconfig Adafruit_GFX USBDeviceHT Adafruit-PWM-Servo-Driver

GridEye/GridEye.cpp@23:f74a50977593, 2018-07-18 (annotated)

Committer:
dev_alexander
Date:
Wed Jul 18 18:52:30 2018 +0000
Revision:
23:f74a50977593
Child:
28:0ed92c590607
I added file global_buffers.h and also reworked a portion of the main.cpp to adjust to the new method of using global buffers defined in a different file.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dev_alexander 23:f74a50977593 1 /**********************************************************************
dev_alexander 23:f74a50977593 2 * Copyright (C) 2016 Maxim Integrated Products, Inc., All Rights Reserved.
dev_alexander 23:f74a50977593 3 *
dev_alexander 23:f74a50977593 4 * Permission is hereby granted, free of charge, to any person obtaining a
dev_alexander 23:f74a50977593 5 * copy of this software and associated documentation files (the "Software"),
dev_alexander 23:f74a50977593 6 * to deal in the Software without restriction, including without limitation
dev_alexander 23:f74a50977593 7 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
dev_alexander 23:f74a50977593 8 * and/or sell copies of the Software, and to permit persons to whom the
dev_alexander 23:f74a50977593 9 * Software is furnished to do so, subject to the following conditions:
dev_alexander 23:f74a50977593 10 *
dev_alexander 23:f74a50977593 11 * The above copyright notice and this permission notice shall be included
dev_alexander 23:f74a50977593 12 * in all copies or substantial portions of the Software.
dev_alexander 23:f74a50977593 13 *
dev_alexander 23:f74a50977593 14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
dev_alexander 23:f74a50977593 15 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
dev_alexander 23:f74a50977593 16 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
dev_alexander 23:f74a50977593 17 * IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
dev_alexander 23:f74a50977593 18 * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
dev_alexander 23:f74a50977593 19 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
dev_alexander 23:f74a50977593 20 * OTHER DEALINGS IN THE SOFTWARE.
dev_alexander 23:f74a50977593 21 *
dev_alexander 23:f74a50977593 22 * Except as contained in this notice, the name of Maxim Integrated
dev_alexander 23:f74a50977593 23 * Products, Inc. shall not be used except as stated in the Maxim Integrated
dev_alexander 23:f74a50977593 24 * Products, Inc. Branding Policy.
dev_alexander 23:f74a50977593 25 *
dev_alexander 23:f74a50977593 26 * The mere transfer of this software does not imply any licenses
dev_alexander 23:f74a50977593 27 * of trade secrets, proprietary technology, copyrights, patents,
dev_alexander 23:f74a50977593 28 * trademarks, maskwork rights, or any other form of intellectual
dev_alexander 23:f74a50977593 29 * property whatsoever. Maxim Integrated Products, Inc. retains all
dev_alexander 23:f74a50977593 30 * ownership rights.
dev_alexander 23:f74a50977593 31 **********************************************************************/
dev_alexander 23:f74a50977593 32
dev_alexander 23:f74a50977593 33
dev_alexander 23:f74a50977593 34 #include "GridEye/GridEye.h"
dev_alexander 23:f74a50977593 35
dev_alexander 23:f74a50977593 36
dev_alexander 23:f74a50977593 37
dev_alexander 23:f74a50977593 38 GridEye::GridEye(I2C &i2c)
dev_alexander 23:f74a50977593 39 : m_i2cBus(i2c)
dev_alexander 23:f74a50977593 40 {
dev_alexander 23:f74a50977593 41 }
dev_alexander 23:f74a50977593 42
dev_alexander 23:f74a50977593 43
dev_alexander 23:f74a50977593 44
dev_alexander 23:f74a50977593 45
dev_alexander 23:f74a50977593 46 //*********************************************************************
dev_alexander 23:f74a50977593 47 int8_t GridEye::gridEyeReadReg(GridEyeRegister reg_addr, int num_bytes, char * data_buf)
dev_alexander 23:f74a50977593 48 {
dev_alexander 23:f74a50977593 49 int8_t result;
dev_alexander 23:f74a50977593 50 char read_pointer[1];
dev_alexander 23:f74a50977593 51 read_pointer[0] = reg_addr;
dev_alexander 23:f74a50977593 52
dev_alexander 23:f74a50977593 53 result = GridEye::m_i2cBus.write(I2C_W_ADRS, read_pointer, 1);
dev_alexander 23:f74a50977593 54 if (result == I2C_WR_SUCCESS)
dev_alexander 23:f74a50977593 55 {
dev_alexander 23:f74a50977593 56 result = GridEye::m_i2cBus.read(I2C_R_ADRS, data_buf , num_bytes, false);
dev_alexander 23:f74a50977593 57 if (result == I2C_WR_SUCCESS)
dev_alexander 23:f74a50977593 58 return result;
dev_alexander 23:f74a50977593 59 }
dev_alexander 23:f74a50977593 60 return I2C_WR_ERROR;
dev_alexander 23:f74a50977593 61 }
dev_alexander 23:f74a50977593 62
dev_alexander 23:f74a50977593 63
dev_alexander 23:f74a50977593 64
dev_alexander 23:f74a50977593 65 //*********************************************************************
dev_alexander 23:f74a50977593 66 int8_t GridEye::gridEyeWriteReg(GridEyeRegister reg_addr, int num_bytes, char * data_buf)
dev_alexander 23:f74a50977593 67 {
dev_alexander 23:f74a50977593 68 int8_t result;
dev_alexander 23:f74a50977593 69 int num_indices = num_bytes + 1;
dev_alexander 23:f74a50977593 70 char write_buffer[num_indices];
dev_alexander 23:f74a50977593 71
dev_alexander 23:f74a50977593 72 //construct write buffer with the address followed by the data to be written
dev_alexander 23:f74a50977593 73 write_buffer[0] = reg_addr;
dev_alexander 23:f74a50977593 74 for (int idx=0; idx<num_bytes; idx++)
dev_alexander 23:f74a50977593 75 write_buffer[idx+1] = data_buf[idx];
dev_alexander 23:f74a50977593 76
dev_alexander 23:f74a50977593 77 result = GridEye::m_i2cBus.write(I2C_W_ADRS, write_buffer, num_indices);
dev_alexander 23:f74a50977593 78 if (result == I2C_WR_SUCCESS)
dev_alexander 23:f74a50977593 79 return result;
dev_alexander 23:f74a50977593 80
dev_alexander 23:f74a50977593 81 //else, return -1
dev_alexander 23:f74a50977593 82 return I2C_WR_ERROR;
dev_alexander 23:f74a50977593 83 }
dev_alexander 23:f74a50977593 84
dev_alexander 23:f74a50977593 85
dev_alexander 23:f74a50977593 86 //*********************************************************************
dev_alexander 23:f74a50977593 87 int8_t GridEye::getThermistorTemperature(int16_t & therm_temp)
dev_alexander 23:f74a50977593 88 {
dev_alexander 23:f74a50977593 89 int8_t result;
dev_alexander 23:f74a50977593 90 char data[2];
dev_alexander 23:f74a50977593 91
dev_alexander 23:f74a50977593 92 result = this->gridEyeReadReg(GridEye::THERMISTOR_LOW, 2, data);
dev_alexander 23:f74a50977593 93
dev_alexander 23:f74a50977593 94 if(result == I2C_WR_SUCCESS)
dev_alexander 23:f74a50977593 95 {
dev_alexander 23:f74a50977593 96 convSingleRawTempData2Int(data, therm_temp);
dev_alexander 23:f74a50977593 97 return therm_temp;
dev_alexander 23:f74a50977593 98 }
dev_alexander 23:f74a50977593 99 return I2C_WR_ERROR;
dev_alexander 23:f74a50977593 100 }
dev_alexander 23:f74a50977593 101
dev_alexander 23:f74a50977593 102
dev_alexander 23:f74a50977593 103 //*********************************************************************
dev_alexander 23:f74a50977593 104 int8_t GridEye::getPixelTemperature(uint8_t pixel_addr, int16_t & pixel_temp)
dev_alexander 23:f74a50977593 105 {
dev_alexander 23:f74a50977593 106 int8_t result;
dev_alexander 23:f74a50977593 107 char data[2];
dev_alexander 23:f74a50977593 108
dev_alexander 23:f74a50977593 109 result = this->gridEyeReadReg((GridEyeRegister) pixel_addr, 2, data);
dev_alexander 23:f74a50977593 110 if(result == I2C_WR_SUCCESS)
dev_alexander 23:f74a50977593 111 {
dev_alexander 23:f74a50977593 112 convSingleRawTempData2Int(data, pixel_temp);
dev_alexander 23:f74a50977593 113 return pixel_temp;
dev_alexander 23:f74a50977593 114 }
dev_alexander 23:f74a50977593 115 return I2C_WR_ERROR;
dev_alexander 23:f74a50977593 116 }
dev_alexander 23:f74a50977593 117
dev_alexander 23:f74a50977593 118
dev_alexander 23:f74a50977593 119 //*********************************************************************
dev_alexander 23:f74a50977593 120 int8_t GridEye::getRaw8x8FrameData(char * raw_frame_data)
dev_alexander 23:f74a50977593 121 {
dev_alexander 23:f74a50977593 122 int8_t result;
dev_alexander 23:f74a50977593 123 result = gridEyeReadReg((GridEyeRegister) GridEye::PIXEL_BASE_ADRS, 128, raw_frame_data);
dev_alexander 23:f74a50977593 124 return result;
dev_alexander 23:f74a50977593 125 }
dev_alexander 23:f74a50977593 126
dev_alexander 23:f74a50977593 127
dev_alexander 23:f74a50977593 128 //*********************************************************************
dev_alexander 23:f74a50977593 129 int8_t GridEye::setOperatingMode(GridEye::OperatingMode mode)
dev_alexander 23:f74a50977593 130 {
dev_alexander 23:f74a50977593 131 int8_t result;
dev_alexander 23:f74a50977593 132 char set_mode[1];
dev_alexander 23:f74a50977593 133 set_mode[0] = (char)mode;
dev_alexander 23:f74a50977593 134 result = this->gridEyeWriteReg(GridEye::OPERATING_MODE, 1, set_mode);
dev_alexander 23:f74a50977593 135 if (result == I2C_WR_SUCCESS)
dev_alexander 23:f74a50977593 136 return result;
dev_alexander 23:f74a50977593 137 return I2C_WR_ERROR;
dev_alexander 23:f74a50977593 138 }
dev_alexander 23:f74a50977593 139
dev_alexander 23:f74a50977593 140 //*********************************************************************
dev_alexander 23:f74a50977593 141 int8_t GridEye::setFrameRate(GridEye::FrameRate rate)
dev_alexander 23:f74a50977593 142 {
dev_alexander 23:f74a50977593 143 int8_t result;
dev_alexander 23:f74a50977593 144 char set_rate[1];
dev_alexander 23:f74a50977593 145 set_rate[0] = (char)rate;
dev_alexander 23:f74a50977593 146 result = this->gridEyeWriteReg(GridEye::FRAME_RATE, 1, set_rate);
dev_alexander 23:f74a50977593 147 if (result == I2C_WR_SUCCESS)
dev_alexander 23:f74a50977593 148 return result;
dev_alexander 23:f74a50977593 149 return I2C_WR_ERROR;
dev_alexander 23:f74a50977593 150 }
dev_alexander 23:f74a50977593 151
dev_alexander 23:f74a50977593 152 //*********************************************************************
dev_alexander 23:f74a50977593 153 void convSingleRawTempData2Int(char * data, int16_t & pixel_temp)
dev_alexander 23:f74a50977593 154 {
dev_alexander 23:f74a50977593 155 int8_t upper_byte = data[1];
dev_alexander 23:f74a50977593 156 int8_t lower_byte = data[0];
dev_alexander 23:f74a50977593 157 int16_t upper_byte_mask = 0x0F00;
dev_alexander 23:f74a50977593 158 int16_t sign_bit = 0x0200;
dev_alexander 23:f74a50977593 159 int16_t finish_neg_val = 0xFC00;
dev_alexander 23:f74a50977593 160 int16_t pixel;
dev_alexander 23:f74a50977593 161
dev_alexander 23:f74a50977593 162 //construct the pixel based off the 12 bit signed data
dev_alexander 23:f74a50977593 163 pixel = (upper_byte << 8);
dev_alexander 23:f74a50977593 164 pixel &= upper_byte_mask;
dev_alexander 23:f74a50977593 165 pixel |= lower_byte;
dev_alexander 23:f74a50977593 166
dev_alexander 23:f74a50977593 167 //shift it over to gain integer value of the pixel
dev_alexander 23:f74a50977593 168 pixel = pixel >> 2;
dev_alexander 23:f74a50977593 169
dev_alexander 23:f74a50977593 170 //if negative, properly convert to 16 bit int format to represent 2's compliment
dev_alexander 23:f74a50977593 171 if (pixel&sign_bit)
dev_alexander 23:f74a50977593 172 pixel |= finish_neg_val;
dev_alexander 23:f74a50977593 173
dev_alexander 23:f74a50977593 174 //set the coresponding pixel to be in the passed in array
dev_alexander 23:f74a50977593 175 pixel_temp = pixel;
dev_alexander 23:f74a50977593 176 }
dev_alexander 23:f74a50977593 177
dev_alexander 23:f74a50977593 178 //*********************************************************************
dev_alexander 23:f74a50977593 179 void convRaw8x8Data2Int(char * data, int16_t * frame_temp) {
dev_alexander 23:f74a50977593 180 int idx = 0;
dev_alexander 23:f74a50977593 181 int8_t upper_byte;
dev_alexander 23:f74a50977593 182 int8_t lower_byte;
dev_alexander 23:f74a50977593 183 int16_t upper_byte_mask = 0x0F00;
dev_alexander 23:f74a50977593 184 int16_t sign_bit = 0x0200;
dev_alexander 23:f74a50977593 185 int16_t finish_neg_val = 0xFC00;
dev_alexander 23:f74a50977593 186 int16_t pixel;
dev_alexander 23:f74a50977593 187
dev_alexander 23:f74a50977593 188 for (idx=0; idx<64; idx++)
dev_alexander 23:f74a50977593 189 {
dev_alexander 23:f74a50977593 190 //construct the pixel based off the 12 bit signed data
dev_alexander 23:f74a50977593 191 upper_byte = data[idx*2+1];
dev_alexander 23:f74a50977593 192 lower_byte = data[idx*2+0];
dev_alexander 23:f74a50977593 193 pixel = (upper_byte << 8);
dev_alexander 23:f74a50977593 194 pixel &= upper_byte_mask;
dev_alexander 23:f74a50977593 195 pixel |= lower_byte;
dev_alexander 23:f74a50977593 196
dev_alexander 23:f74a50977593 197 //shift it over to gain integer value of the pixel
dev_alexander 23:f74a50977593 198 pixel = pixel >> 2;
dev_alexander 23:f74a50977593 199
dev_alexander 23:f74a50977593 200 //if negative, properly convert to 16 bit int format to represent 2's compliment
dev_alexander 23:f74a50977593 201 if (pixel&sign_bit)
dev_alexander 23:f74a50977593 202 pixel |= finish_neg_val;
dev_alexander 23:f74a50977593 203
dev_alexander 23:f74a50977593 204 //set the coresponding pixel to be in the passed in array
dev_alexander 23:f74a50977593 205 frame_temp[idx] = pixel;
dev_alexander 23:f74a50977593 206 }
dev_alexander 23:f74a50977593 207 }
dev_alexander 23:f74a50977593 208
dev_alexander 23:f74a50977593 209 //*********************************************************************
dev_alexander 23:f74a50977593 210 void convRaw8x8Data2Point25degC(char * data, int16_t * frame_temp)
dev_alexander 23:f74a50977593 211 {
dev_alexander 23:f74a50977593 212 int idx = 0;
dev_alexander 23:f74a50977593 213 int8_t upper_byte;
dev_alexander 23:f74a50977593 214 int8_t lower_byte;
dev_alexander 23:f74a50977593 215 int16_t upper_byte_mask = 0x0F00;
dev_alexander 23:f74a50977593 216 int16_t sign_bit = 0x0800;
dev_alexander 23:f74a50977593 217 int16_t finish_neg_val = 0xF000;
dev_alexander 23:f74a50977593 218 int16_t pixel;
dev_alexander 23:f74a50977593 219
dev_alexander 23:f74a50977593 220 for (idx=0; idx<64; idx++)
dev_alexander 23:f74a50977593 221 {
dev_alexander 23:f74a50977593 222 //construct the pixel based off the 12 bit signed data
dev_alexander 23:f74a50977593 223 upper_byte = data[idx*2+1];
dev_alexander 23:f74a50977593 224 lower_byte = data[idx*2+0];
dev_alexander 23:f74a50977593 225 pixel = (upper_byte << 8);
dev_alexander 23:f74a50977593 226 pixel &= upper_byte_mask;
dev_alexander 23:f74a50977593 227 pixel |= lower_byte;
dev_alexander 23:f74a50977593 228
dev_alexander 23:f74a50977593 229 //no shift needed since we would lose the two lsb that give 0.25*C precision
dev_alexander 23:f74a50977593 230
dev_alexander 23:f74a50977593 231 //if negative, properly convert to 16 bit int format to represent 2's compliment
dev_alexander 23:f74a50977593 232 if (pixel & sign_bit)
dev_alexander 23:f74a50977593 233 pixel |= finish_neg_val;
dev_alexander 23:f74a50977593 234
dev_alexander 23:f74a50977593 235 //set the coresponding pixel to be in the passed in array
dev_alexander 23:f74a50977593 236 frame_temp[idx] = pixel;
dev_alexander 23:f74a50977593 237 }
dev_alexander 23:f74a50977593 238 }