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@44:8be011a3fba0, 2021-12-29 (annotated)

Committer:
walterluu
Date:
Wed Dec 29 06:53:23 2021 +0000
Revision:
44:8be011a3fba0
Parent:
40:6f8744c366c2 
LP Long Distance IR Vision Robot;

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 28:0ed92c590607 41 //Perform a software reset upon power on.
dev_alexander 28:0ed92c590607 42 softwareReset();
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
dev_alexander 23:f74a50977593 48 //*********************************************************************
dev_alexander 23:f74a50977593 49 int8_t GridEye::gridEyeReadReg(GridEyeRegister reg_addr, int num_bytes, char * data_buf)
dev_alexander 23:f74a50977593 50 {
dev_alexander 23:f74a50977593 51 int8_t result;
dev_alexander 23:f74a50977593 52 char read_pointer[1];
dev_alexander 23:f74a50977593 53 read_pointer[0] = reg_addr;
dev_alexander 23:f74a50977593 54
dev_alexander 40:6f8744c366c2 55 result = GridEye::m_i2cBus.write(I2C_W_ADRS_GRIDEYE, read_pointer, 1);
dev_alexander 23:f74a50977593 56 if (result == I2C_WR_SUCCESS)
dev_alexander 23:f74a50977593 57 {
dev_alexander 40:6f8744c366c2 58 result = GridEye::m_i2cBus.read(I2C_R_ADRS_GRIDEYE, data_buf , num_bytes, false);
dev_alexander 23:f74a50977593 59 if (result == I2C_WR_SUCCESS)
dev_alexander 23:f74a50977593 60 return result;
dev_alexander 23:f74a50977593 61 }
dev_alexander 23:f74a50977593 62 return I2C_WR_ERROR;
dev_alexander 23:f74a50977593 63 }
dev_alexander 23:f74a50977593 64
dev_alexander 23:f74a50977593 65
dev_alexander 23:f74a50977593 66
dev_alexander 23:f74a50977593 67 //*********************************************************************
dev_alexander 23:f74a50977593 68 int8_t GridEye::gridEyeWriteReg(GridEyeRegister reg_addr, int num_bytes, char * data_buf)
dev_alexander 23:f74a50977593 69 {
dev_alexander 23:f74a50977593 70 int8_t result;
dev_alexander 23:f74a50977593 71 int num_indices = num_bytes + 1;
dev_alexander 23:f74a50977593 72 char write_buffer[num_indices];
dev_alexander 23:f74a50977593 73
dev_alexander 23:f74a50977593 74 //construct write buffer with the address followed by the data to be written
dev_alexander 23:f74a50977593 75 write_buffer[0] = reg_addr;
dev_alexander 23:f74a50977593 76 for (int idx=0; idx<num_bytes; idx++)
dev_alexander 23:f74a50977593 77 write_buffer[idx+1] = data_buf[idx];
dev_alexander 23:f74a50977593 78
dev_alexander 40:6f8744c366c2 79 result = GridEye::m_i2cBus.write(I2C_W_ADRS_GRIDEYE, write_buffer, num_indices);
dev_alexander 23:f74a50977593 80 if (result == I2C_WR_SUCCESS)
dev_alexander 23:f74a50977593 81 return result;
dev_alexander 23:f74a50977593 82
dev_alexander 23:f74a50977593 83 //else, return -1
dev_alexander 23:f74a50977593 84 return I2C_WR_ERROR;
dev_alexander 23:f74a50977593 85 }
dev_alexander 23:f74a50977593 86
dev_alexander 23:f74a50977593 87
dev_alexander 23:f74a50977593 88 //*********************************************************************
dev_alexander 23:f74a50977593 89 int8_t GridEye::getThermistorTemperature(int16_t & therm_temp)
dev_alexander 23:f74a50977593 90 {
dev_alexander 23:f74a50977593 91 int8_t result;
dev_alexander 23:f74a50977593 92 char data[2];
dev_alexander 23:f74a50977593 93
dev_alexander 23:f74a50977593 94 result = this->gridEyeReadReg(GridEye::THERMISTOR_LOW, 2, data);
dev_alexander 23:f74a50977593 95
dev_alexander 23:f74a50977593 96 if(result == I2C_WR_SUCCESS)
dev_alexander 23:f74a50977593 97 {
dev_alexander 23:f74a50977593 98 convSingleRawTempData2Int(data, therm_temp);
dev_alexander 23:f74a50977593 99 return therm_temp;
dev_alexander 23:f74a50977593 100 }
dev_alexander 23:f74a50977593 101 return I2C_WR_ERROR;
dev_alexander 23:f74a50977593 102 }
dev_alexander 23:f74a50977593 103
dev_alexander 23:f74a50977593 104
dev_alexander 23:f74a50977593 105 //*********************************************************************
dev_alexander 23:f74a50977593 106 int8_t GridEye::getPixelTemperature(uint8_t pixel_addr, int16_t & pixel_temp)
dev_alexander 23:f74a50977593 107 {
dev_alexander 23:f74a50977593 108 int8_t result;
dev_alexander 23:f74a50977593 109 char data[2];
dev_alexander 23:f74a50977593 110
dev_alexander 23:f74a50977593 111 result = this->gridEyeReadReg((GridEyeRegister) pixel_addr, 2, data);
dev_alexander 23:f74a50977593 112 if(result == I2C_WR_SUCCESS)
dev_alexander 23:f74a50977593 113 {
dev_alexander 23:f74a50977593 114 convSingleRawTempData2Int(data, pixel_temp);
dev_alexander 23:f74a50977593 115 return pixel_temp;
dev_alexander 23:f74a50977593 116 }
dev_alexander 23:f74a50977593 117 return I2C_WR_ERROR;
dev_alexander 23:f74a50977593 118 }
dev_alexander 23:f74a50977593 119
dev_alexander 23:f74a50977593 120
dev_alexander 23:f74a50977593 121 //*********************************************************************
dev_alexander 23:f74a50977593 122 int8_t GridEye::getRaw8x8FrameData(char * raw_frame_data)
dev_alexander 23:f74a50977593 123 {
dev_alexander 23:f74a50977593 124 int8_t result;
dev_alexander 23:f74a50977593 125 result = gridEyeReadReg((GridEyeRegister) GridEye::PIXEL_BASE_ADRS, 128, raw_frame_data);
dev_alexander 23:f74a50977593 126 return result;
dev_alexander 23:f74a50977593 127 }
dev_alexander 23:f74a50977593 128
dev_alexander 28:0ed92c590607 129 //*********************************************************************
dev_alexander 28:0ed92c590607 130 int8_t GridEye::softwareReset()
dev_alexander 28:0ed92c590607 131 {
dev_alexander 28:0ed92c590607 132 int8_t result;
dev_alexander 28:0ed92c590607 133 char initial_reset[1];
dev_alexander 28:0ed92c590607 134 //This value when programmed to the Grid Eye's RESET reg will perform initial reset on device
dev_alexander 28:0ed92c590607 135 initial_reset[0] = 0x3F;
dev_alexander 28:0ed92c590607 136 result = this->gridEyeWriteReg(GridEye::RESET, 1, initial_reset);
dev_alexander 28:0ed92c590607 137 if (result == I2C_WR_SUCCESS)
dev_alexander 28:0ed92c590607 138 return result;
dev_alexander 28:0ed92c590607 139 return I2C_WR_ERROR;
dev_alexander 28:0ed92c590607 140 }
dev_alexander 23:f74a50977593 141
dev_alexander 23:f74a50977593 142 //*********************************************************************
dev_alexander 23:f74a50977593 143 int8_t GridEye::setOperatingMode(GridEye::OperatingMode mode)
dev_alexander 23:f74a50977593 144 {
dev_alexander 23:f74a50977593 145 int8_t result;
dev_alexander 23:f74a50977593 146 char set_mode[1];
dev_alexander 23:f74a50977593 147 set_mode[0] = (char)mode;
dev_alexander 23:f74a50977593 148 result = this->gridEyeWriteReg(GridEye::OPERATING_MODE, 1, set_mode);
dev_alexander 23:f74a50977593 149 if (result == I2C_WR_SUCCESS)
dev_alexander 23:f74a50977593 150 return result;
dev_alexander 23:f74a50977593 151 return I2C_WR_ERROR;
dev_alexander 23:f74a50977593 152 }
dev_alexander 23:f74a50977593 153
dev_alexander 23:f74a50977593 154 //*********************************************************************
dev_alexander 23:f74a50977593 155 int8_t GridEye::setFrameRate(GridEye::FrameRate rate)
dev_alexander 23:f74a50977593 156 {
dev_alexander 23:f74a50977593 157 int8_t result;
dev_alexander 23:f74a50977593 158 char set_rate[1];
dev_alexander 23:f74a50977593 159 set_rate[0] = (char)rate;
dev_alexander 23:f74a50977593 160 result = this->gridEyeWriteReg(GridEye::FRAME_RATE, 1, set_rate);
dev_alexander 23:f74a50977593 161 if (result == I2C_WR_SUCCESS)
dev_alexander 23:f74a50977593 162 return result;
dev_alexander 23:f74a50977593 163 return I2C_WR_ERROR;
dev_alexander 23:f74a50977593 164 }
dev_alexander 23:f74a50977593 165
dev_alexander 23:f74a50977593 166 //*********************************************************************
dev_alexander 23:f74a50977593 167 void convSingleRawTempData2Int(char * data, int16_t & pixel_temp)
dev_alexander 23:f74a50977593 168 {
dev_alexander 23:f74a50977593 169 int8_t upper_byte = data[1];
dev_alexander 23:f74a50977593 170 int8_t lower_byte = data[0];
dev_alexander 23:f74a50977593 171 int16_t upper_byte_mask = 0x0F00;
dev_alexander 28:0ed92c590607 172 //int16_t sign_bit = 0x0400;
dev_alexander 23:f74a50977593 173 int16_t sign_bit = 0x0200;
dev_alexander 23:f74a50977593 174 int16_t finish_neg_val = 0xFC00;
dev_alexander 23:f74a50977593 175 int16_t pixel;
dev_alexander 23:f74a50977593 176
dev_alexander 23:f74a50977593 177 //construct the pixel based off the 12 bit signed data
dev_alexander 23:f74a50977593 178 pixel = (upper_byte << 8);
dev_alexander 23:f74a50977593 179 pixel &= upper_byte_mask;
dev_alexander 23:f74a50977593 180 pixel |= lower_byte;
dev_alexander 23:f74a50977593 181
dev_alexander 23:f74a50977593 182 //shift it over to gain integer value of the pixel
dev_alexander 23:f74a50977593 183 pixel = pixel >> 2;
dev_alexander 23:f74a50977593 184
dev_alexander 23:f74a50977593 185 //if negative, properly convert to 16 bit int format to represent 2's compliment
dev_alexander 23:f74a50977593 186 if (pixel&sign_bit)
dev_alexander 23:f74a50977593 187 pixel |= finish_neg_val;
dev_alexander 23:f74a50977593 188
dev_alexander 23:f74a50977593 189 //set the coresponding pixel to be in the passed in array
dev_alexander 23:f74a50977593 190 pixel_temp = pixel;
dev_alexander 23:f74a50977593 191 }
dev_alexander 23:f74a50977593 192
dev_alexander 23:f74a50977593 193 //*********************************************************************
dev_alexander 23:f74a50977593 194 void convRaw8x8Data2Int(char * data, int16_t * frame_temp) {
dev_alexander 23:f74a50977593 195 int idx = 0;
dev_alexander 23:f74a50977593 196 int8_t upper_byte;
dev_alexander 23:f74a50977593 197 int8_t lower_byte;
dev_alexander 23:f74a50977593 198 int16_t upper_byte_mask = 0x0F00;
dev_alexander 28:0ed92c590607 199 //int16_t sign_bit = 0x0400;
dev_alexander 23:f74a50977593 200 int16_t sign_bit = 0x0200;
dev_alexander 23:f74a50977593 201 int16_t finish_neg_val = 0xFC00;
dev_alexander 23:f74a50977593 202 int16_t pixel;
dev_alexander 23:f74a50977593 203
dev_alexander 23:f74a50977593 204 for (idx=0; idx<64; idx++)
dev_alexander 23:f74a50977593 205 {
dev_alexander 23:f74a50977593 206 //construct the pixel based off the 12 bit signed data
dev_alexander 23:f74a50977593 207 upper_byte = data[idx*2+1];
dev_alexander 23:f74a50977593 208 lower_byte = data[idx*2+0];
dev_alexander 23:f74a50977593 209 pixel = (upper_byte << 8);
dev_alexander 23:f74a50977593 210 pixel &= upper_byte_mask;
dev_alexander 23:f74a50977593 211 pixel |= lower_byte;
dev_alexander 23:f74a50977593 212
dev_alexander 23:f74a50977593 213 //shift it over to gain integer value of the pixel
dev_alexander 23:f74a50977593 214 pixel = pixel >> 2;
dev_alexander 23:f74a50977593 215
dev_alexander 23:f74a50977593 216 //if negative, properly convert to 16 bit int format to represent 2's compliment
dev_alexander 23:f74a50977593 217 if (pixel&sign_bit)
dev_alexander 23:f74a50977593 218 pixel |= finish_neg_val;
dev_alexander 23:f74a50977593 219
dev_alexander 23:f74a50977593 220 //set the coresponding pixel to be in the passed in array
dev_alexander 23:f74a50977593 221 frame_temp[idx] = pixel;
dev_alexander 23:f74a50977593 222 }
dev_alexander 23:f74a50977593 223 }
dev_alexander 23:f74a50977593 224
dev_alexander 23:f74a50977593 225 //*********************************************************************
dev_alexander 23:f74a50977593 226 void convRaw8x8Data2Point25degC(char * data, int16_t * frame_temp)
dev_alexander 23:f74a50977593 227 {
dev_alexander 23:f74a50977593 228 int idx = 0;
dev_alexander 23:f74a50977593 229 int8_t upper_byte;
dev_alexander 23:f74a50977593 230 int8_t lower_byte;
dev_alexander 23:f74a50977593 231 int16_t upper_byte_mask = 0x0F00;
dev_alexander 28:0ed92c590607 232 //int16_t sign_bit = 0x1000;
dev_alexander 23:f74a50977593 233 int16_t sign_bit = 0x0800;
dev_alexander 23:f74a50977593 234 int16_t finish_neg_val = 0xF000;
dev_alexander 23:f74a50977593 235 int16_t pixel;
dev_alexander 23:f74a50977593 236
dev_alexander 23:f74a50977593 237 for (idx=0; idx<64; idx++)
dev_alexander 23:f74a50977593 238 {
dev_alexander 23:f74a50977593 239 //construct the pixel based off the 12 bit signed data
dev_alexander 23:f74a50977593 240 upper_byte = data[idx*2+1];
dev_alexander 23:f74a50977593 241 lower_byte = data[idx*2+0];
dev_alexander 23:f74a50977593 242 pixel = (upper_byte << 8);
dev_alexander 28:0ed92c590607 243 //pixel &= upper_byte_mask;
dev_alexander 23:f74a50977593 244 pixel |= lower_byte;
dev_alexander 23:f74a50977593 245 //no shift needed since we would lose the two lsb that give 0.25*C precision
dev_alexander 23:f74a50977593 246
dev_alexander 23:f74a50977593 247 //if negative, properly convert to 16 bit int format to represent 2's compliment
dev_alexander 23:f74a50977593 248 if (pixel & sign_bit)
dev_alexander 23:f74a50977593 249 pixel |= finish_neg_val;
dev_alexander 28:0ed92c590607 250 else
dev_alexander 28:0ed92c590607 251 pixel &= 0x0FFF;
dev_alexander 23:f74a50977593 252
dev_alexander 23:f74a50977593 253 //set the coresponding pixel to be in the passed in array
dev_alexander 23:f74a50977593 254 frame_temp[idx] = pixel;
dev_alexander 23:f74a50977593 255 }
dev_alexander 23:f74a50977593 256 }