Psi Swarm robot library version 0.9
Fork of PsiSwarmV9 by
colour.cpp@17:bf614e28668f, 2017-06-04 (annotated)
- Committer:
- jah128
- Date:
- Sun Jun 04 13:11:09 2017 +0000
- Revision:
- 17:bf614e28668f
- Parent:
- 16:50686c07ad07
- Child:
- 18:9204f74069b4
Updated calibration menus, fixed some bugs in demo, create new store_line_position routine using calibrated values
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jah128 | 0:d6269d17c8cf | 1 | /* University of York Robotics Laboratory PsiSwarm Library: Colour Sensors Source File |
jah128 | 15:66be5ec52c3b | 2 | * |
jah128 | 14:2f1ad77d281e | 3 | * Copyright 2017 University of York |
jah128 | 6:b340a527add9 | 4 | * |
jah128 | 15:66be5ec52c3b | 5 | * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. |
jah128 | 6:b340a527add9 | 6 | * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 |
jah128 | 6:b340a527add9 | 7 | * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS |
jah128 | 15:66be5ec52c3b | 8 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
jah128 | 6:b340a527add9 | 9 | * See the License for the specific language governing permissions and limitations under the License. |
jah128 | 6:b340a527add9 | 10 | * |
jah128 | 0:d6269d17c8cf | 11 | * File: colour.cpp |
jah128 | 0:d6269d17c8cf | 12 | * |
jah128 | 0:d6269d17c8cf | 13 | * (C) Dept. Electronics & Computer Science, University of York |
jah128 | 0:d6269d17c8cf | 14 | * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis |
jah128 | 0:d6269d17c8cf | 15 | * |
jah128 | 14:2f1ad77d281e | 16 | * PsiSwarm Library Version: 0.9 |
jah128 | 0:d6269d17c8cf | 17 | * |
jah128 | 14:2f1ad77d281e | 18 | * June 2017 |
jah128 | 0:d6269d17c8cf | 19 | * |
jah128 | 0:d6269d17c8cf | 20 | */ |
jah128 | 0:d6269d17c8cf | 21 | |
jah128 | 0:d6269d17c8cf | 22 | |
jah128 | 0:d6269d17c8cf | 23 | // Base colour sensor is a TCS34725 |
jah128 | 0:d6269d17c8cf | 24 | // Top colour sensor (if fitted) is a TCS34721 |
jah128 | 0:d6269d17c8cf | 25 | #include "psiswarm.h" |
jah128 | 0:d6269d17c8cf | 26 | |
jah128 | 15:66be5ec52c3b | 27 | int last_detected_colour = -1; |
jah128 | 0:d6269d17c8cf | 28 | |
jah128 | 17:bf614e28668f | 29 | int cs_c_black,cs_r_black,cs_g_black,cs_b_black,cs_c_white,cs_r_white,cs_g_white,cs_b_white; |
jah128 | 17:bf614e28668f | 30 | |
jah128 | 17:bf614e28668f | 31 | void Colour::set_calibration_values(int c_black,int r_black,int g_black,int b_black,int c_white,int r_white,int g_white,int b_white){ |
jah128 | 17:bf614e28668f | 32 | cs_c_black = c_black; |
jah128 | 17:bf614e28668f | 33 | cs_r_black = r_black; |
jah128 | 17:bf614e28668f | 34 | cs_g_black = g_black; |
jah128 | 17:bf614e28668f | 35 | cs_b_black = b_black; |
jah128 | 17:bf614e28668f | 36 | cs_c_white = c_white; |
jah128 | 17:bf614e28668f | 37 | cs_r_white = r_white; |
jah128 | 17:bf614e28668f | 38 | cs_g_white = g_white; |
jah128 | 17:bf614e28668f | 39 | cs_b_white = b_white; |
jah128 | 17:bf614e28668f | 40 | } |
jah128 | 17:bf614e28668f | 41 | |
jah128 | 16:50686c07ad07 | 42 | void Colour::colour_sensor_init() |
jah128 | 16:50686c07ad07 | 43 | { |
jah128 | 17:bf614e28668f | 44 | colour.set_base_colour_sensor_integration_time(BASE_COLOUR_SENSOR_INTEGRATION_TIME); |
jah128 | 17:bf614e28668f | 45 | colour.set_base_colour_sensor_gain(BASE_COLOUR_SENSOR_GAIN); |
jah128 | 16:50686c07ad07 | 46 | } |
jah128 | 16:50686c07ad07 | 47 | |
jah128 | 15:66be5ec52c3b | 48 | void Colour::read_base_colour_sensor_values(int * store_array) |
jah128 | 15:66be5ec52c3b | 49 | { |
jah128 | 15:66be5ec52c3b | 50 | char buffer[8] = { 0 }; |
jah128 | 15:66be5ec52c3b | 51 | IF_readMultipleRegisters( CDATA, buffer, 8 ); |
jah128 | 15:66be5ec52c3b | 52 | store_array[0] = (int)buffer[1] << 8 | (int)buffer[0]; |
jah128 | 15:66be5ec52c3b | 53 | store_array[1] = (int)buffer[3] << 8 | (int)buffer[2]; |
jah128 | 15:66be5ec52c3b | 54 | store_array[2] = (int)buffer[5] << 8 | (int)buffer[4]; |
jah128 | 15:66be5ec52c3b | 55 | store_array[3] = (int)buffer[7] << 8 | (int)buffer[6]; |
jah128 | 15:66be5ec52c3b | 56 | } |
jah128 | 15:66be5ec52c3b | 57 | |
jah128 | 15:66be5ec52c3b | 58 | void Colour::set_base_colour_sensor_gain(char gain) |
jah128 | 15:66be5ec52c3b | 59 | { |
jah128 | 15:66be5ec52c3b | 60 | char control; |
jah128 | 15:66be5ec52c3b | 61 | int ack = 0; |
jah128 | 15:66be5ec52c3b | 62 | switch (gain) { |
jah128 | 15:66be5ec52c3b | 63 | case 1: |
jah128 | 15:66be5ec52c3b | 64 | control = 0; |
jah128 | 15:66be5ec52c3b | 65 | break; |
jah128 | 15:66be5ec52c3b | 66 | case 4: |
jah128 | 15:66be5ec52c3b | 67 | control = 1; |
jah128 | 15:66be5ec52c3b | 68 | break; |
jah128 | 15:66be5ec52c3b | 69 | case 16: |
jah128 | 15:66be5ec52c3b | 70 | control = 2; |
jah128 | 15:66be5ec52c3b | 71 | break; |
jah128 | 15:66be5ec52c3b | 72 | case 60: |
jah128 | 15:66be5ec52c3b | 73 | control = 3; |
jah128 | 15:66be5ec52c3b | 74 | break; |
jah128 | 15:66be5ec52c3b | 75 | default: |
jah128 | 15:66be5ec52c3b | 76 | ack = 2; // 2 used to indicate invalid entry |
jah128 | 15:66be5ec52c3b | 77 | break; |
jah128 | 15:66be5ec52c3b | 78 | } |
jah128 | 15:66be5ec52c3b | 79 | if ( ack != 2 ) { |
jah128 | 15:66be5ec52c3b | 80 | ack = IF_writeSingleRegister( CONTROL, control ); |
jah128 | 15:66be5ec52c3b | 81 | } |
jah128 | 0:d6269d17c8cf | 82 | } |
jah128 | 0:d6269d17c8cf | 83 | |
jah128 | 15:66be5ec52c3b | 84 | void Colour::set_base_colour_sensor_integration_time(char int_time) |
jah128 | 15:66be5ec52c3b | 85 | { |
jah128 | 15:66be5ec52c3b | 86 | char atime = 256 - IF_roundTowardsZero( int_time / 2.4 ); // rounding ensures nearest value of atime is used |
jah128 | 15:66be5ec52c3b | 87 | int ack = IF_writeSingleRegister( ATIME, atime ); |
jah128 | 15:66be5ec52c3b | 88 | } |
jah128 | 15:66be5ec52c3b | 89 | |
jah128 | 15:66be5ec52c3b | 90 | float Colour::IF_roundTowardsZero( const float value ) |
jah128 | 15:66be5ec52c3b | 91 | { |
jah128 | 15:66be5ec52c3b | 92 | float result = 0; |
jah128 | 15:66be5ec52c3b | 93 | if ( ( value >= 0 && ( value - (int)value ) < 0.5 ) || ( value < 0 && ( abs(value) - (int)abs(value) ) >= 0.5 ) ) { |
jah128 | 15:66be5ec52c3b | 94 | result = floor(value); |
jah128 | 15:66be5ec52c3b | 95 | } else { |
jah128 | 15:66be5ec52c3b | 96 | result = ceil(value); |
jah128 | 15:66be5ec52c3b | 97 | } |
jah128 | 15:66be5ec52c3b | 98 | return result; |
jah128 | 15:66be5ec52c3b | 99 | } |
jah128 | 15:66be5ec52c3b | 100 | |
jah128 | 15:66be5ec52c3b | 101 | void Colour::enable_base_colour_sensor(void) |
jah128 | 15:66be5ec52c3b | 102 | { |
jah128 | 15:66be5ec52c3b | 103 | char enable_old = IF_readSingleRegister( ENABLE ); |
jah128 | 15:66be5ec52c3b | 104 | char enable_new = enable_old | 3; // sets PON (bit 0) and AEN (bit 1) to 1 |
jah128 | 15:66be5ec52c3b | 105 | int ack = IF_writeSingleRegister( ENABLE, enable_new ); |
jah128 | 15:66be5ec52c3b | 106 | } |
jah128 | 15:66be5ec52c3b | 107 | |
jah128 | 15:66be5ec52c3b | 108 | |
jah128 | 15:66be5ec52c3b | 109 | void Colour::disable_base_colour_sensor(void) |
jah128 | 15:66be5ec52c3b | 110 | { |
jah128 | 15:66be5ec52c3b | 111 | char enable_old = IF_readSingleRegister( ENABLE ); |
jah128 | 15:66be5ec52c3b | 112 | char enable_new = enable_old & 252; // sets PON (bit 0) and AEN (bit 1) to 0 |
jah128 | 15:66be5ec52c3b | 113 | int ack = IF_writeSingleRegister( ENABLE, enable_new ); |
jah128 | 15:66be5ec52c3b | 114 | } |
jah128 | 15:66be5ec52c3b | 115 | |
jah128 | 15:66be5ec52c3b | 116 | Timeout colour_ticker; |
jah128 | 15:66be5ec52c3b | 117 | int colour_ticker_period; |
jah128 | 15:66be5ec52c3b | 118 | int colour_ticker_on = 0; |
jah128 | 15:66be5ec52c3b | 119 | |
jah128 | 15:66be5ec52c3b | 120 | void Colour::start_colour_ticker(int period_ms) |
jah128 | 15:66be5ec52c3b | 121 | { |
jah128 | 15:66be5ec52c3b | 122 | colour_ticker_on = 1; |
jah128 | 15:66be5ec52c3b | 123 | colour_ticker_period = period_ms; |
jah128 | 15:66be5ec52c3b | 124 | colour_ticker.attach_us(this,&Colour::IF_colour_ticker_start, 100); |
jah128 | 15:66be5ec52c3b | 125 | } |
jah128 | 15:66be5ec52c3b | 126 | |
jah128 | 16:50686c07ad07 | 127 | void Colour::stop_colour_ticker() |
jah128 | 16:50686c07ad07 | 128 | { |
jah128 | 16:50686c07ad07 | 129 | colour_ticker_on = 0; |
jah128 | 16:50686c07ad07 | 130 | colour_ticker.detach(); |
jah128 | 16:50686c07ad07 | 131 | } |
jah128 | 16:50686c07ad07 | 132 | |
jah128 | 15:66be5ec52c3b | 133 | void Colour::IF_colour_ticker_start() |
jah128 | 15:66be5ec52c3b | 134 | { |
jah128 | 15:66be5ec52c3b | 135 | led.set_base_led(1); |
jah128 | 15:66be5ec52c3b | 136 | enable_base_colour_sensor(); |
jah128 | 15:66be5ec52c3b | 137 | colour_ticker.attach_us(this,&Colour::IF_colour_ticker_main, 25000); |
jah128 | 0:d6269d17c8cf | 138 | } |
jah128 | 0:d6269d17c8cf | 139 | |
jah128 | 15:66be5ec52c3b | 140 | void Colour::IF_colour_ticker_main() |
jah128 | 15:66be5ec52c3b | 141 | { |
jah128 | 15:66be5ec52c3b | 142 | int rgb_readings [4]; |
jah128 | 15:66be5ec52c3b | 143 | read_base_colour_sensor_values( rgb_readings ); |
jah128 | 15:66be5ec52c3b | 144 | disable_base_colour_sensor(); |
jah128 | 15:66be5ec52c3b | 145 | led.set_base_led(0); |
jah128 | 15:66be5ec52c3b | 146 | if(rgb_readings[1] > 0 && rgb_readings[1] < 1024 && rgb_readings[2] > 0 && rgb_readings[2] < 1024 && rgb_readings[3] > 0 && rgb_readings[3] < 1024) { |
jah128 | 15:66be5ec52c3b | 147 | float adjusted[4]; |
jah128 | 15:66be5ec52c3b | 148 | get_calibrated_colour(rgb_readings,adjusted); |
jah128 | 15:66be5ec52c3b | 149 | last_detected_colour = identify_colour_from_calibrated_colour_scores(adjusted); |
jah128 | 15:66be5ec52c3b | 150 | } |
jah128 | 15:66be5ec52c3b | 151 | if(colour_ticker_on == 1)colour_ticker.attach_us(this,&Colour::IF_colour_ticker_start, colour_ticker_period * 1000); |
jah128 | 15:66be5ec52c3b | 152 | } |
jah128 | 15:66be5ec52c3b | 153 | |
jah128 | 15:66be5ec52c3b | 154 | |
jah128 | 15:66be5ec52c3b | 155 | |
jah128 | 15:66be5ec52c3b | 156 | int Colour::detect_colour_once() |
jah128 | 15:66be5ec52c3b | 157 | { |
jah128 | 15:66be5ec52c3b | 158 | int rgb_readings [4]; |
jah128 | 15:66be5ec52c3b | 159 | led.set_base_led(1); |
jah128 | 15:66be5ec52c3b | 160 | enable_base_colour_sensor(); |
jah128 | 15:66be5ec52c3b | 161 | wait(0.03); |
jah128 | 15:66be5ec52c3b | 162 | read_base_colour_sensor_values( rgb_readings ); |
jah128 | 15:66be5ec52c3b | 163 | disable_base_colour_sensor(); |
jah128 | 15:66be5ec52c3b | 164 | led.set_base_led(0); |
jah128 | 15:66be5ec52c3b | 165 | if(rgb_readings[1] < 1 || rgb_readings[1] > 1022 || rgb_readings[2] < 1 || rgb_readings[2] > 1022 || rgb_readings[3] < 1 || rgb_readings[3] > 1022) return -1; |
jah128 | 15:66be5ec52c3b | 166 | float adjusted[4]; |
jah128 | 15:66be5ec52c3b | 167 | get_calibrated_colour(rgb_readings,adjusted); |
jah128 | 15:66be5ec52c3b | 168 | last_detected_colour = identify_colour_from_calibrated_colour_scores(adjusted); |
jah128 | 15:66be5ec52c3b | 169 | return last_detected_colour; |
jah128 | 15:66be5ec52c3b | 170 | } |
jah128 | 15:66be5ec52c3b | 171 | |
jah128 | 15:66be5ec52c3b | 172 | int Colour::get_detected_colour() |
jah128 | 15:66be5ec52c3b | 173 | { |
jah128 | 15:66be5ec52c3b | 174 | return last_detected_colour; |
jah128 | 0:d6269d17c8cf | 175 | } |
jah128 | 0:d6269d17c8cf | 176 | |
jah128 | 15:66be5ec52c3b | 177 | const char * Colour::get_colour_string(int colour_index) |
jah128 | 15:66be5ec52c3b | 178 | { |
jah128 | 15:66be5ec52c3b | 179 | switch(colour_index) { |
jah128 | 15:66be5ec52c3b | 180 | case 0: |
jah128 | 15:66be5ec52c3b | 181 | return "RED "; |
jah128 | 15:66be5ec52c3b | 182 | case 1: |
jah128 | 15:66be5ec52c3b | 183 | return "YELLOW "; |
jah128 | 15:66be5ec52c3b | 184 | case 2: |
jah128 | 15:66be5ec52c3b | 185 | return "GREEN "; |
jah128 | 15:66be5ec52c3b | 186 | case 3: |
jah128 | 15:66be5ec52c3b | 187 | return "CYAN "; |
jah128 | 15:66be5ec52c3b | 188 | case 4: |
jah128 | 15:66be5ec52c3b | 189 | return "BLUE "; |
jah128 | 15:66be5ec52c3b | 190 | case 5: |
jah128 | 15:66be5ec52c3b | 191 | return "MAGENTA"; |
jah128 | 15:66be5ec52c3b | 192 | case 6: |
jah128 | 15:66be5ec52c3b | 193 | return "WHITE "; |
jah128 | 15:66be5ec52c3b | 194 | case 7: |
jah128 | 15:66be5ec52c3b | 195 | return "BLACK "; |
jah128 | 15:66be5ec52c3b | 196 | } |
jah128 | 15:66be5ec52c3b | 197 | return "-------"; |
jah128 | 0:d6269d17c8cf | 198 | } |
jah128 | 0:d6269d17c8cf | 199 | |
jah128 | 15:66be5ec52c3b | 200 | void Colour::get_calibrated_colour(int * colour_array_in, float * colour_array_out) |
jah128 | 15:66be5ec52c3b | 201 | { |
jah128 | 15:66be5ec52c3b | 202 | int colour_temp = colour_array_in[0]; |
jah128 | 17:bf614e28668f | 203 | if(colour_temp < cs_c_black) colour_temp = cs_c_black; |
jah128 | 17:bf614e28668f | 204 | if(colour_temp > cs_c_white) colour_temp = cs_c_white; |
jah128 | 17:bf614e28668f | 205 | colour_array_out[0] = (colour_temp - cs_c_black) / (float) (cs_c_white - cs_c_black); |
jah128 | 15:66be5ec52c3b | 206 | float black_level = 1.0 - colour_array_out[0]; |
jah128 | 17:bf614e28668f | 207 | colour_array_out[1] = ((colour_array_in[1] / (float)cs_r_white) * colour_array_out[0]) + ((colour_array_in[1] / (float)cs_r_black) * black_level); |
jah128 | 17:bf614e28668f | 208 | colour_array_out[2] = ((colour_array_in[2] / (float)cs_g_white) * colour_array_out[0]) + ((colour_array_in[2] / (float)cs_g_black) * black_level); |
jah128 | 17:bf614e28668f | 209 | colour_array_out[3] = ((colour_array_in[3] / (float)cs_b_white) * colour_array_out[0]) + ((colour_array_in[3] / (float)cs_b_black) * black_level); |
jah128 | 15:66be5ec52c3b | 210 | // Normalise array |
jah128 | 15:66be5ec52c3b | 211 | float norm_factor = 3.0/(colour_array_out[1] + colour_array_out[2] + colour_array_out[3]); |
jah128 | 15:66be5ec52c3b | 212 | colour_array_out[1] *= norm_factor; |
jah128 | 15:66be5ec52c3b | 213 | colour_array_out[2] *= norm_factor; |
jah128 | 15:66be5ec52c3b | 214 | colour_array_out[3] *= norm_factor; |
jah128 | 17:bf614e28668f | 215 | // int sum_black = cs_r_black + cs_g_black + CS_ |
jah128 | 15:66be5ec52c3b | 216 | // colour_array_out[1] = |
jah128 | 15:66be5ec52c3b | 217 | } |
jah128 | 15:66be5ec52c3b | 218 | |
jah128 | 15:66be5ec52c3b | 219 | int Colour::identify_colour_from_calibrated_colour_scores(float * calibrate_colour_array_in) |
jah128 | 15:66be5ec52c3b | 220 | { |
jah128 | 15:66be5ec52c3b | 221 | float scores[8]; |
jah128 | 15:66be5ec52c3b | 222 | scores[0] = ((calibrate_colour_array_in[1] * 2) * (calibrate_colour_array_in[1] * 2)) / ((calibrate_colour_array_in[2] + calibrate_colour_array_in[3]) * (calibrate_colour_array_in[2] + calibrate_colour_array_in[3])); |
jah128 | 15:66be5ec52c3b | 223 | scores[1] = ((calibrate_colour_array_in[1] + calibrate_colour_array_in[2]) * (calibrate_colour_array_in[1] + calibrate_colour_array_in[2])) / (calibrate_colour_array_in[3] * calibrate_colour_array_in[3] * 4); |
jah128 | 15:66be5ec52c3b | 224 | scores[2] = ((calibrate_colour_array_in[2] * 2) * (calibrate_colour_array_in[2] * 2)) / ((calibrate_colour_array_in[1] + calibrate_colour_array_in[3]) * (calibrate_colour_array_in[1] + calibrate_colour_array_in[3])); |
jah128 | 15:66be5ec52c3b | 225 | scores[3] = ((calibrate_colour_array_in[2] + calibrate_colour_array_in[3]) * (calibrate_colour_array_in[2] + calibrate_colour_array_in[3])) / (calibrate_colour_array_in[1] * calibrate_colour_array_in[1] * 4); |
jah128 | 15:66be5ec52c3b | 226 | scores[4] = ((calibrate_colour_array_in[3] * 2) * (calibrate_colour_array_in[3] * 2)) / ((calibrate_colour_array_in[2] + calibrate_colour_array_in[1]) * (calibrate_colour_array_in[2] + calibrate_colour_array_in[1])); |
jah128 | 15:66be5ec52c3b | 227 | scores[5] = ((calibrate_colour_array_in[3] + calibrate_colour_array_in[1]) * (calibrate_colour_array_in[3] + calibrate_colour_array_in[1])) / (calibrate_colour_array_in[2] * calibrate_colour_array_in[2] * 4); |
jah128 | 15:66be5ec52c3b | 228 | float grey_factor = 1.0 / (1 + ((((calibrate_colour_array_in[1] - 1) * 10) * ((calibrate_colour_array_in[1] - 1) * 10)) * (((calibrate_colour_array_in[2] - 1) * 10) * ((calibrate_colour_array_in[2] - 1) * 10)) * (((calibrate_colour_array_in[3] - 1) * 10) * ((calibrate_colour_array_in[3] - 1) * 10)))); |
jah128 | 15:66be5ec52c3b | 229 | scores[6] = calibrate_colour_array_in[0] * calibrate_colour_array_in[0] * grey_factor * 4; |
jah128 | 15:66be5ec52c3b | 230 | scores[7] = (1-calibrate_colour_array_in[0]) * (1-calibrate_colour_array_in[0]) * grey_factor * 4; |
jah128 | 15:66be5ec52c3b | 231 | //pc.printf("R:%1.2f Y:%1.2f G:%1.2f C:%1.2f B:%1.2f M:%1.2f W:%1.2f B:%1.2f G:%1.2f\n\n", scores[0],scores[1],scores[2],scores[3],scores[4],scores[5],scores[6],scores[7],grey_factor); |
jah128 | 15:66be5ec52c3b | 232 | float max = 2; |
jah128 | 15:66be5ec52c3b | 233 | int max_index = 8; |
jah128 | 15:66be5ec52c3b | 234 | for(int i=0; i<8; i++) { |
jah128 | 15:66be5ec52c3b | 235 | if(scores[i] > max) { |
jah128 | 15:66be5ec52c3b | 236 | max=scores[i]; |
jah128 | 15:66be5ec52c3b | 237 | max_index=i; |
jah128 | 15:66be5ec52c3b | 238 | } |
jah128 | 15:66be5ec52c3b | 239 | } |
jah128 | 15:66be5ec52c3b | 240 | return max_index; |
jah128 | 15:66be5ec52c3b | 241 | } |
jah128 | 15:66be5ec52c3b | 242 | |
jah128 | 15:66be5ec52c3b | 243 | char Colour::IF_check_base_colour_sensor(void) |
jah128 | 15:66be5ec52c3b | 244 | { |
jah128 | 0:d6269d17c8cf | 245 | //Reads the device ID flag of the colour sensor [0xB2] |
jah128 | 0:d6269d17c8cf | 246 | //This should equal 0x44 for both TCS34721 (top) and TCS34725 (base) sensors |
jah128 | 0:d6269d17c8cf | 247 | //Return a 1 if successful or a 0 otherwise |
jah128 | 0:d6269d17c8cf | 248 | char return_value = 0; |
jah128 | 0:d6269d17c8cf | 249 | char data[1] = {0x00}; |
jah128 | 0:d6269d17c8cf | 250 | char command[1] = {0xB2}; |
jah128 | 0:d6269d17c8cf | 251 | primary_i2c.write(BASE_COLOUR_ADDRESS, command, 1, false); |
jah128 | 0:d6269d17c8cf | 252 | primary_i2c.read(BASE_COLOUR_ADDRESS, data, 1, false); |
jah128 | 0:d6269d17c8cf | 253 | if(data[0] == 0x44) return_value = 1; |
jah128 | 12:878c6e9d9e60 | 254 | else psi.debug("Invalid response from colour sensor:%X\n",data[0]); |
jah128 | 0:d6269d17c8cf | 255 | return return_value; |
jah128 | 0:d6269d17c8cf | 256 | } |
jah128 | 15:66be5ec52c3b | 257 | |
jah128 | 15:66be5ec52c3b | 258 | int Colour::IF_writeSingleRegister( char address, char data ) |
jah128 | 15:66be5ec52c3b | 259 | { |
jah128 | 15:66be5ec52c3b | 260 | char tx[2] = { address | 160, data }; //0d160 = 0b10100000 |
jah128 | 15:66be5ec52c3b | 261 | int ack = primary_i2c.write(BASE_COLOUR_ADDRESS, tx, 2, false); |
jah128 | 15:66be5ec52c3b | 262 | return ack; |
jah128 | 15:66be5ec52c3b | 263 | } |
jah128 | 15:66be5ec52c3b | 264 | |
jah128 | 15:66be5ec52c3b | 265 | int Colour::IF_writeMultipleRegisters( char address, char* data, int quantity ) |
jah128 | 15:66be5ec52c3b | 266 | { |
jah128 | 15:66be5ec52c3b | 267 | char tx[ quantity + 1 ]; |
jah128 | 15:66be5ec52c3b | 268 | tx[0] = address | 160; |
jah128 | 15:66be5ec52c3b | 269 | for ( int i = 1; i <= quantity; i++ ) { |
jah128 | 15:66be5ec52c3b | 270 | tx[ i ] = data[ i - 1 ]; |
jah128 | 15:66be5ec52c3b | 271 | } |
jah128 | 15:66be5ec52c3b | 272 | int ack = primary_i2c.write(BASE_COLOUR_ADDRESS, tx, quantity + 1, false); |
jah128 | 15:66be5ec52c3b | 273 | return ack; |
jah128 | 15:66be5ec52c3b | 274 | } |
jah128 | 15:66be5ec52c3b | 275 | |
jah128 | 15:66be5ec52c3b | 276 | char Colour::IF_readSingleRegister( char address ) |
jah128 | 15:66be5ec52c3b | 277 | { |
jah128 | 15:66be5ec52c3b | 278 | char output = 255; |
jah128 | 15:66be5ec52c3b | 279 | char command = address | 160; //0d160 = 0b10100000 |
jah128 | 15:66be5ec52c3b | 280 | primary_i2c.write( BASE_COLOUR_ADDRESS, &command, 1, true ); |
jah128 | 15:66be5ec52c3b | 281 | primary_i2c.read( BASE_COLOUR_ADDRESS, &output, 1 ); |
jah128 | 15:66be5ec52c3b | 282 | return output; |
jah128 | 15:66be5ec52c3b | 283 | } |
jah128 | 15:66be5ec52c3b | 284 | |
jah128 | 15:66be5ec52c3b | 285 | int Colour::IF_readMultipleRegisters( char address, char* output, int quantity ) |
jah128 | 15:66be5ec52c3b | 286 | { |
jah128 | 15:66be5ec52c3b | 287 | char command = address | 160; //0d160 = 0b10100000 |
jah128 | 15:66be5ec52c3b | 288 | primary_i2c.write(BASE_COLOUR_ADDRESS, &command, 1, true ); |
jah128 | 15:66be5ec52c3b | 289 | int ack = primary_i2c.read( BASE_COLOUR_ADDRESS, output, quantity ); |
jah128 | 15:66be5ec52c3b | 290 | return ack; |
jah128 | 15:66be5ec52c3b | 291 | } |