Psi Swarm robot library version 0.9
Fork of PsiSwarmV9 by
colour.cpp@15:66be5ec52c3b, 2017-06-01 (annotated)
- Committer:
- jah128
- Date:
- Thu Jun 01 21:58:14 2017 +0000
- Revision:
- 15:66be5ec52c3b
- Parent:
- 14:2f1ad77d281e
- Child:
- 16:50686c07ad07
Added colour detection code
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 CS_C_BLACK = 63; |
jah128 | 15:66be5ec52c3b | 28 | int CS_C_WHITE = 802; |
jah128 | 15:66be5ec52c3b | 29 | int CS_R_BLACK = 22; |
jah128 | 15:66be5ec52c3b | 30 | int CS_R_WHITE = 244; |
jah128 | 15:66be5ec52c3b | 31 | int CS_G_BLACK = 24; |
jah128 | 15:66be5ec52c3b | 32 | int CS_G_WHITE = 297; |
jah128 | 15:66be5ec52c3b | 33 | int CS_B_BLACK = 16; |
jah128 | 15:66be5ec52c3b | 34 | int CS_B_WHITE = 232; |
jah128 | 15:66be5ec52c3b | 35 | int last_detected_colour = -1; |
jah128 | 0:d6269d17c8cf | 36 | |
jah128 | 15:66be5ec52c3b | 37 | void Colour::read_base_colour_sensor_values(int * store_array) |
jah128 | 15:66be5ec52c3b | 38 | { |
jah128 | 15:66be5ec52c3b | 39 | char buffer[8] = { 0 }; |
jah128 | 15:66be5ec52c3b | 40 | IF_readMultipleRegisters( CDATA, buffer, 8 ); |
jah128 | 15:66be5ec52c3b | 41 | store_array[0] = (int)buffer[1] << 8 | (int)buffer[0]; |
jah128 | 15:66be5ec52c3b | 42 | store_array[1] = (int)buffer[3] << 8 | (int)buffer[2]; |
jah128 | 15:66be5ec52c3b | 43 | store_array[2] = (int)buffer[5] << 8 | (int)buffer[4]; |
jah128 | 15:66be5ec52c3b | 44 | store_array[3] = (int)buffer[7] << 8 | (int)buffer[6]; |
jah128 | 15:66be5ec52c3b | 45 | } |
jah128 | 15:66be5ec52c3b | 46 | |
jah128 | 15:66be5ec52c3b | 47 | void Colour::set_base_colour_sensor_gain(char gain) |
jah128 | 15:66be5ec52c3b | 48 | { |
jah128 | 15:66be5ec52c3b | 49 | char control; |
jah128 | 15:66be5ec52c3b | 50 | int ack = 0; |
jah128 | 15:66be5ec52c3b | 51 | switch (gain) { |
jah128 | 15:66be5ec52c3b | 52 | case 1: |
jah128 | 15:66be5ec52c3b | 53 | control = 0; |
jah128 | 15:66be5ec52c3b | 54 | break; |
jah128 | 15:66be5ec52c3b | 55 | case 4: |
jah128 | 15:66be5ec52c3b | 56 | control = 1; |
jah128 | 15:66be5ec52c3b | 57 | break; |
jah128 | 15:66be5ec52c3b | 58 | case 16: |
jah128 | 15:66be5ec52c3b | 59 | control = 2; |
jah128 | 15:66be5ec52c3b | 60 | break; |
jah128 | 15:66be5ec52c3b | 61 | case 60: |
jah128 | 15:66be5ec52c3b | 62 | control = 3; |
jah128 | 15:66be5ec52c3b | 63 | break; |
jah128 | 15:66be5ec52c3b | 64 | default: |
jah128 | 15:66be5ec52c3b | 65 | ack = 2; // 2 used to indicate invalid entry |
jah128 | 15:66be5ec52c3b | 66 | break; |
jah128 | 15:66be5ec52c3b | 67 | } |
jah128 | 15:66be5ec52c3b | 68 | if ( ack != 2 ) { |
jah128 | 15:66be5ec52c3b | 69 | ack = IF_writeSingleRegister( CONTROL, control ); |
jah128 | 15:66be5ec52c3b | 70 | } |
jah128 | 0:d6269d17c8cf | 71 | } |
jah128 | 0:d6269d17c8cf | 72 | |
jah128 | 15:66be5ec52c3b | 73 | void Colour::set_base_colour_sensor_integration_time(char int_time) |
jah128 | 15:66be5ec52c3b | 74 | { |
jah128 | 15:66be5ec52c3b | 75 | char atime = 256 - IF_roundTowardsZero( int_time / 2.4 ); // rounding ensures nearest value of atime is used |
jah128 | 15:66be5ec52c3b | 76 | int ack = IF_writeSingleRegister( ATIME, atime ); |
jah128 | 15:66be5ec52c3b | 77 | } |
jah128 | 15:66be5ec52c3b | 78 | |
jah128 | 15:66be5ec52c3b | 79 | float Colour::IF_roundTowardsZero( const float value ) |
jah128 | 15:66be5ec52c3b | 80 | { |
jah128 | 15:66be5ec52c3b | 81 | float result = 0; |
jah128 | 15:66be5ec52c3b | 82 | if ( ( value >= 0 && ( value - (int)value ) < 0.5 ) || ( value < 0 && ( abs(value) - (int)abs(value) ) >= 0.5 ) ) { |
jah128 | 15:66be5ec52c3b | 83 | result = floor(value); |
jah128 | 15:66be5ec52c3b | 84 | } else { |
jah128 | 15:66be5ec52c3b | 85 | result = ceil(value); |
jah128 | 15:66be5ec52c3b | 86 | } |
jah128 | 15:66be5ec52c3b | 87 | return result; |
jah128 | 15:66be5ec52c3b | 88 | } |
jah128 | 15:66be5ec52c3b | 89 | |
jah128 | 15:66be5ec52c3b | 90 | void Colour::enable_base_colour_sensor(void) |
jah128 | 15:66be5ec52c3b | 91 | { |
jah128 | 15:66be5ec52c3b | 92 | char enable_old = IF_readSingleRegister( ENABLE ); |
jah128 | 15:66be5ec52c3b | 93 | char enable_new = enable_old | 3; // sets PON (bit 0) and AEN (bit 1) to 1 |
jah128 | 15:66be5ec52c3b | 94 | int ack = IF_writeSingleRegister( ENABLE, enable_new ); |
jah128 | 15:66be5ec52c3b | 95 | } |
jah128 | 15:66be5ec52c3b | 96 | |
jah128 | 15:66be5ec52c3b | 97 | |
jah128 | 15:66be5ec52c3b | 98 | void Colour::disable_base_colour_sensor(void) |
jah128 | 15:66be5ec52c3b | 99 | { |
jah128 | 15:66be5ec52c3b | 100 | char enable_old = IF_readSingleRegister( ENABLE ); |
jah128 | 15:66be5ec52c3b | 101 | char enable_new = enable_old & 252; // sets PON (bit 0) and AEN (bit 1) to 0 |
jah128 | 15:66be5ec52c3b | 102 | int ack = IF_writeSingleRegister( ENABLE, enable_new ); |
jah128 | 15:66be5ec52c3b | 103 | } |
jah128 | 15:66be5ec52c3b | 104 | |
jah128 | 15:66be5ec52c3b | 105 | Timeout colour_ticker; |
jah128 | 15:66be5ec52c3b | 106 | int colour_ticker_period; |
jah128 | 15:66be5ec52c3b | 107 | int colour_ticker_on = 0; |
jah128 | 15:66be5ec52c3b | 108 | |
jah128 | 15:66be5ec52c3b | 109 | void Colour::start_colour_ticker(int period_ms) |
jah128 | 15:66be5ec52c3b | 110 | { |
jah128 | 15:66be5ec52c3b | 111 | colour_ticker_on = 1; |
jah128 | 15:66be5ec52c3b | 112 | colour_ticker_period = period_ms; |
jah128 | 15:66be5ec52c3b | 113 | colour_ticker.attach_us(this,&Colour::IF_colour_ticker_start, 100); |
jah128 | 15:66be5ec52c3b | 114 | } |
jah128 | 15:66be5ec52c3b | 115 | |
jah128 | 15:66be5ec52c3b | 116 | void Colour::IF_colour_ticker_start() |
jah128 | 15:66be5ec52c3b | 117 | { |
jah128 | 15:66be5ec52c3b | 118 | led.set_base_led(1); |
jah128 | 15:66be5ec52c3b | 119 | enable_base_colour_sensor(); |
jah128 | 15:66be5ec52c3b | 120 | colour_ticker.attach_us(this,&Colour::IF_colour_ticker_main, 25000); |
jah128 | 0:d6269d17c8cf | 121 | } |
jah128 | 0:d6269d17c8cf | 122 | |
jah128 | 15:66be5ec52c3b | 123 | void Colour::IF_colour_ticker_main() |
jah128 | 15:66be5ec52c3b | 124 | { |
jah128 | 15:66be5ec52c3b | 125 | int rgb_readings [4]; |
jah128 | 15:66be5ec52c3b | 126 | read_base_colour_sensor_values( rgb_readings ); |
jah128 | 15:66be5ec52c3b | 127 | disable_base_colour_sensor(); |
jah128 | 15:66be5ec52c3b | 128 | led.set_base_led(0); |
jah128 | 15:66be5ec52c3b | 129 | 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 | 130 | float adjusted[4]; |
jah128 | 15:66be5ec52c3b | 131 | get_calibrated_colour(rgb_readings,adjusted); |
jah128 | 15:66be5ec52c3b | 132 | last_detected_colour = identify_colour_from_calibrated_colour_scores(adjusted); |
jah128 | 15:66be5ec52c3b | 133 | } |
jah128 | 15:66be5ec52c3b | 134 | if(colour_ticker_on == 1)colour_ticker.attach_us(this,&Colour::IF_colour_ticker_start, colour_ticker_period * 1000); |
jah128 | 15:66be5ec52c3b | 135 | } |
jah128 | 15:66be5ec52c3b | 136 | |
jah128 | 15:66be5ec52c3b | 137 | |
jah128 | 15:66be5ec52c3b | 138 | |
jah128 | 15:66be5ec52c3b | 139 | int Colour::detect_colour_once() |
jah128 | 15:66be5ec52c3b | 140 | { |
jah128 | 15:66be5ec52c3b | 141 | int rgb_readings [4]; |
jah128 | 15:66be5ec52c3b | 142 | led.set_base_led(1); |
jah128 | 15:66be5ec52c3b | 143 | enable_base_colour_sensor(); |
jah128 | 15:66be5ec52c3b | 144 | wait(0.03); |
jah128 | 15:66be5ec52c3b | 145 | read_base_colour_sensor_values( rgb_readings ); |
jah128 | 15:66be5ec52c3b | 146 | disable_base_colour_sensor(); |
jah128 | 15:66be5ec52c3b | 147 | led.set_base_led(0); |
jah128 | 15:66be5ec52c3b | 148 | 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 | 149 | float adjusted[4]; |
jah128 | 15:66be5ec52c3b | 150 | get_calibrated_colour(rgb_readings,adjusted); |
jah128 | 15:66be5ec52c3b | 151 | last_detected_colour = identify_colour_from_calibrated_colour_scores(adjusted); |
jah128 | 15:66be5ec52c3b | 152 | return last_detected_colour; |
jah128 | 15:66be5ec52c3b | 153 | } |
jah128 | 15:66be5ec52c3b | 154 | |
jah128 | 15:66be5ec52c3b | 155 | int Colour::get_detected_colour() |
jah128 | 15:66be5ec52c3b | 156 | { |
jah128 | 15:66be5ec52c3b | 157 | return last_detected_colour; |
jah128 | 0:d6269d17c8cf | 158 | } |
jah128 | 0:d6269d17c8cf | 159 | |
jah128 | 15:66be5ec52c3b | 160 | const char * Colour::get_colour_string(int colour_index) |
jah128 | 15:66be5ec52c3b | 161 | { |
jah128 | 15:66be5ec52c3b | 162 | switch(colour_index) { |
jah128 | 15:66be5ec52c3b | 163 | case 0: |
jah128 | 15:66be5ec52c3b | 164 | return "RED "; |
jah128 | 15:66be5ec52c3b | 165 | case 1: |
jah128 | 15:66be5ec52c3b | 166 | return "YELLOW "; |
jah128 | 15:66be5ec52c3b | 167 | case 2: |
jah128 | 15:66be5ec52c3b | 168 | return "GREEN "; |
jah128 | 15:66be5ec52c3b | 169 | case 3: |
jah128 | 15:66be5ec52c3b | 170 | return "CYAN "; |
jah128 | 15:66be5ec52c3b | 171 | case 4: |
jah128 | 15:66be5ec52c3b | 172 | return "BLUE "; |
jah128 | 15:66be5ec52c3b | 173 | case 5: |
jah128 | 15:66be5ec52c3b | 174 | return "MAGENTA"; |
jah128 | 15:66be5ec52c3b | 175 | case 6: |
jah128 | 15:66be5ec52c3b | 176 | return "WHITE "; |
jah128 | 15:66be5ec52c3b | 177 | case 7: |
jah128 | 15:66be5ec52c3b | 178 | return "BLACK "; |
jah128 | 15:66be5ec52c3b | 179 | } |
jah128 | 15:66be5ec52c3b | 180 | return "-------"; |
jah128 | 0:d6269d17c8cf | 181 | } |
jah128 | 0:d6269d17c8cf | 182 | |
jah128 | 15:66be5ec52c3b | 183 | void Colour::get_calibrated_colour(int * colour_array_in, float * colour_array_out) |
jah128 | 15:66be5ec52c3b | 184 | { |
jah128 | 15:66be5ec52c3b | 185 | int colour_temp = colour_array_in[0]; |
jah128 | 15:66be5ec52c3b | 186 | if(colour_temp < CS_C_BLACK) colour_temp = CS_C_BLACK; |
jah128 | 15:66be5ec52c3b | 187 | if(colour_temp > CS_C_WHITE) colour_temp = CS_C_WHITE; |
jah128 | 15:66be5ec52c3b | 188 | colour_array_out[0] = (colour_temp - CS_C_BLACK) / (float) (CS_C_WHITE - CS_C_BLACK); |
jah128 | 15:66be5ec52c3b | 189 | float black_level = 1.0 - colour_array_out[0]; |
jah128 | 15:66be5ec52c3b | 190 | 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 | 15:66be5ec52c3b | 191 | 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 | 15:66be5ec52c3b | 192 | 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 | 193 | // Normalise array |
jah128 | 15:66be5ec52c3b | 194 | float norm_factor = 3.0/(colour_array_out[1] + colour_array_out[2] + colour_array_out[3]); |
jah128 | 15:66be5ec52c3b | 195 | colour_array_out[1] *= norm_factor; |
jah128 | 15:66be5ec52c3b | 196 | colour_array_out[2] *= norm_factor; |
jah128 | 15:66be5ec52c3b | 197 | colour_array_out[3] *= norm_factor; |
jah128 | 15:66be5ec52c3b | 198 | // int sum_black = CS_R_BLACK + CS_G_BLACK + CS_ |
jah128 | 15:66be5ec52c3b | 199 | // colour_array_out[1] = |
jah128 | 15:66be5ec52c3b | 200 | } |
jah128 | 15:66be5ec52c3b | 201 | |
jah128 | 15:66be5ec52c3b | 202 | int Colour::identify_colour_from_calibrated_colour_scores(float * calibrate_colour_array_in) |
jah128 | 15:66be5ec52c3b | 203 | { |
jah128 | 15:66be5ec52c3b | 204 | float scores[8]; |
jah128 | 15:66be5ec52c3b | 205 | 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 | 206 | 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 | 207 | 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 | 208 | 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 | 209 | 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 | 210 | 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 | 211 | 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 | 212 | scores[6] = calibrate_colour_array_in[0] * calibrate_colour_array_in[0] * grey_factor * 4; |
jah128 | 15:66be5ec52c3b | 213 | scores[7] = (1-calibrate_colour_array_in[0]) * (1-calibrate_colour_array_in[0]) * grey_factor * 4; |
jah128 | 15:66be5ec52c3b | 214 | //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 | 215 | float max = 2; |
jah128 | 15:66be5ec52c3b | 216 | int max_index = 8; |
jah128 | 15:66be5ec52c3b | 217 | for(int i=0; i<8; i++) { |
jah128 | 15:66be5ec52c3b | 218 | if(scores[i] > max) { |
jah128 | 15:66be5ec52c3b | 219 | max=scores[i]; |
jah128 | 15:66be5ec52c3b | 220 | max_index=i; |
jah128 | 15:66be5ec52c3b | 221 | } |
jah128 | 15:66be5ec52c3b | 222 | } |
jah128 | 15:66be5ec52c3b | 223 | return max_index; |
jah128 | 15:66be5ec52c3b | 224 | } |
jah128 | 15:66be5ec52c3b | 225 | |
jah128 | 15:66be5ec52c3b | 226 | char Colour::IF_check_base_colour_sensor(void) |
jah128 | 15:66be5ec52c3b | 227 | { |
jah128 | 0:d6269d17c8cf | 228 | //Reads the device ID flag of the colour sensor [0xB2] |
jah128 | 0:d6269d17c8cf | 229 | //This should equal 0x44 for both TCS34721 (top) and TCS34725 (base) sensors |
jah128 | 0:d6269d17c8cf | 230 | //Return a 1 if successful or a 0 otherwise |
jah128 | 0:d6269d17c8cf | 231 | char return_value = 0; |
jah128 | 0:d6269d17c8cf | 232 | char data[1] = {0x00}; |
jah128 | 0:d6269d17c8cf | 233 | char command[1] = {0xB2}; |
jah128 | 0:d6269d17c8cf | 234 | primary_i2c.write(BASE_COLOUR_ADDRESS, command, 1, false); |
jah128 | 0:d6269d17c8cf | 235 | primary_i2c.read(BASE_COLOUR_ADDRESS, data, 1, false); |
jah128 | 0:d6269d17c8cf | 236 | if(data[0] == 0x44) return_value = 1; |
jah128 | 12:878c6e9d9e60 | 237 | else psi.debug("Invalid response from colour sensor:%X\n",data[0]); |
jah128 | 0:d6269d17c8cf | 238 | return return_value; |
jah128 | 0:d6269d17c8cf | 239 | } |
jah128 | 15:66be5ec52c3b | 240 | |
jah128 | 15:66be5ec52c3b | 241 | int Colour::IF_writeSingleRegister( char address, char data ) |
jah128 | 15:66be5ec52c3b | 242 | { |
jah128 | 15:66be5ec52c3b | 243 | char tx[2] = { address | 160, data }; //0d160 = 0b10100000 |
jah128 | 15:66be5ec52c3b | 244 | int ack = primary_i2c.write(BASE_COLOUR_ADDRESS, tx, 2, false); |
jah128 | 15:66be5ec52c3b | 245 | return ack; |
jah128 | 15:66be5ec52c3b | 246 | } |
jah128 | 15:66be5ec52c3b | 247 | |
jah128 | 15:66be5ec52c3b | 248 | int Colour::IF_writeMultipleRegisters( char address, char* data, int quantity ) |
jah128 | 15:66be5ec52c3b | 249 | { |
jah128 | 15:66be5ec52c3b | 250 | char tx[ quantity + 1 ]; |
jah128 | 15:66be5ec52c3b | 251 | tx[0] = address | 160; |
jah128 | 15:66be5ec52c3b | 252 | for ( int i = 1; i <= quantity; i++ ) { |
jah128 | 15:66be5ec52c3b | 253 | tx[ i ] = data[ i - 1 ]; |
jah128 | 15:66be5ec52c3b | 254 | } |
jah128 | 15:66be5ec52c3b | 255 | int ack = primary_i2c.write(BASE_COLOUR_ADDRESS, tx, quantity + 1, false); |
jah128 | 15:66be5ec52c3b | 256 | return ack; |
jah128 | 15:66be5ec52c3b | 257 | } |
jah128 | 15:66be5ec52c3b | 258 | |
jah128 | 15:66be5ec52c3b | 259 | char Colour::IF_readSingleRegister( char address ) |
jah128 | 15:66be5ec52c3b | 260 | { |
jah128 | 15:66be5ec52c3b | 261 | char output = 255; |
jah128 | 15:66be5ec52c3b | 262 | char command = address | 160; //0d160 = 0b10100000 |
jah128 | 15:66be5ec52c3b | 263 | primary_i2c.write( BASE_COLOUR_ADDRESS, &command, 1, true ); |
jah128 | 15:66be5ec52c3b | 264 | primary_i2c.read( BASE_COLOUR_ADDRESS, &output, 1 ); |
jah128 | 15:66be5ec52c3b | 265 | return output; |
jah128 | 15:66be5ec52c3b | 266 | } |
jah128 | 15:66be5ec52c3b | 267 | |
jah128 | 15:66be5ec52c3b | 268 | int Colour::IF_readMultipleRegisters( char address, char* output, int quantity ) |
jah128 | 15:66be5ec52c3b | 269 | { |
jah128 | 15:66be5ec52c3b | 270 | char command = address | 160; //0d160 = 0b10100000 |
jah128 | 15:66be5ec52c3b | 271 | primary_i2c.write(BASE_COLOUR_ADDRESS, &command, 1, true ); |
jah128 | 15:66be5ec52c3b | 272 | int ack = primary_i2c.read( BASE_COLOUR_ADDRESS, output, quantity ); |
jah128 | 15:66be5ec52c3b | 273 | return ack; |
jah128 | 15:66be5ec52c3b | 274 | } |