Psi Swarm robot library version 0.9
Fork of PsiSwarmV9 by
Diff: colour.cpp
- Revision:
- 15:66be5ec52c3b
- Parent:
- 14:2f1ad77d281e
- Child:
- 16:50686c07ad07
diff -r 2f1ad77d281e -r 66be5ec52c3b colour.cpp --- a/colour.cpp Tue May 30 21:03:18 2017 +0000 +++ b/colour.cpp Thu Jun 01 21:58:14 2017 +0000 @@ -1,11 +1,11 @@ /* University of York Robotics Laboratory PsiSwarm Library: Colour Sensors Source File - * + * * Copyright 2017 University of York * - * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. + * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and limitations under the License. * * File: colour.cpp @@ -24,24 +24,207 @@ // Top colour sensor (if fitted) is a TCS34721 #include "psiswarm.h" +int CS_C_BLACK = 63; +int CS_C_WHITE = 802; +int CS_R_BLACK = 22; +int CS_R_WHITE = 244; +int CS_G_BLACK = 24; +int CS_G_WHITE = 297; +int CS_B_BLACK = 16; +int CS_B_WHITE = 232; +int last_detected_colour = -1; -void Colour::read_base_colour_sensor_values(int * store_array){ - +void Colour::read_base_colour_sensor_values(int * store_array) +{ + char buffer[8] = { 0 }; + IF_readMultipleRegisters( CDATA, buffer, 8 ); + store_array[0] = (int)buffer[1] << 8 | (int)buffer[0]; + store_array[1] = (int)buffer[3] << 8 | (int)buffer[2]; + store_array[2] = (int)buffer[5] << 8 | (int)buffer[4]; + store_array[3] = (int)buffer[7] << 8 | (int)buffer[6]; +} + +void Colour::set_base_colour_sensor_gain(char gain) +{ + char control; + int ack = 0; + switch (gain) { + case 1: + control = 0; + break; + case 4: + control = 1; + break; + case 16: + control = 2; + break; + case 60: + control = 3; + break; + default: + ack = 2; // 2 used to indicate invalid entry + break; + } + if ( ack != 2 ) { + ack = IF_writeSingleRegister( CONTROL, control ); + } } -void Colour::set_base_colour_sensor_gain(char gain){ - +void Colour::set_base_colour_sensor_integration_time(char int_time) +{ + char atime = 256 - IF_roundTowardsZero( int_time / 2.4 ); // rounding ensures nearest value of atime is used + int ack = IF_writeSingleRegister( ATIME, atime ); +} + +float Colour::IF_roundTowardsZero( const float value ) +{ + float result = 0; + if ( ( value >= 0 && ( value - (int)value ) < 0.5 ) || ( value < 0 && ( abs(value) - (int)abs(value) ) >= 0.5 ) ) { + result = floor(value); + } else { + result = ceil(value); + } + return result; +} + +void Colour::enable_base_colour_sensor(void) +{ + char enable_old = IF_readSingleRegister( ENABLE ); + char enable_new = enable_old | 3; // sets PON (bit 0) and AEN (bit 1) to 1 + int ack = IF_writeSingleRegister( ENABLE, enable_new ); +} + + +void Colour::disable_base_colour_sensor(void) +{ + char enable_old = IF_readSingleRegister( ENABLE ); + char enable_new = enable_old & 252; // sets PON (bit 0) and AEN (bit 1) to 0 + int ack = IF_writeSingleRegister( ENABLE, enable_new ); +} + +Timeout colour_ticker; +int colour_ticker_period; +int colour_ticker_on = 0; + +void Colour::start_colour_ticker(int period_ms) +{ + colour_ticker_on = 1; + colour_ticker_period = period_ms; + colour_ticker.attach_us(this,&Colour::IF_colour_ticker_start, 100); +} + +void Colour::IF_colour_ticker_start() +{ + led.set_base_led(1); + enable_base_colour_sensor(); + colour_ticker.attach_us(this,&Colour::IF_colour_ticker_main, 25000); } -void Colour::set_base_colour_sensor_integration_time(char int_time){ - +void Colour::IF_colour_ticker_main() +{ + int rgb_readings [4]; + read_base_colour_sensor_values( rgb_readings ); + disable_base_colour_sensor(); + led.set_base_led(0); + 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) { + float adjusted[4]; + get_calibrated_colour(rgb_readings,adjusted); + last_detected_colour = identify_colour_from_calibrated_colour_scores(adjusted); + } + if(colour_ticker_on == 1)colour_ticker.attach_us(this,&Colour::IF_colour_ticker_start, colour_ticker_period * 1000); +} + + + +int Colour::detect_colour_once() +{ + int rgb_readings [4]; + led.set_base_led(1); + enable_base_colour_sensor(); + wait(0.03); + read_base_colour_sensor_values( rgb_readings ); + disable_base_colour_sensor(); + led.set_base_led(0); + 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; + float adjusted[4]; + get_calibrated_colour(rgb_readings,adjusted); + last_detected_colour = identify_colour_from_calibrated_colour_scores(adjusted); + return last_detected_colour; +} + +int Colour::get_detected_colour() +{ + return last_detected_colour; } -void Colour::enable_base_colour_sensor(void){ - +const char * Colour::get_colour_string(int colour_index) +{ + switch(colour_index) { + case 0: + return "RED "; + case 1: + return "YELLOW "; + case 2: + return "GREEN "; + case 3: + return "CYAN "; + case 4: + return "BLUE "; + case 5: + return "MAGENTA"; + case 6: + return "WHITE "; + case 7: + return "BLACK "; + } + return "-------"; } -char Colour::IF_check_base_colour_sensor(void){ +void Colour::get_calibrated_colour(int * colour_array_in, float * colour_array_out) +{ + int colour_temp = colour_array_in[0]; + if(colour_temp < CS_C_BLACK) colour_temp = CS_C_BLACK; + if(colour_temp > CS_C_WHITE) colour_temp = CS_C_WHITE; + colour_array_out[0] = (colour_temp - CS_C_BLACK) / (float) (CS_C_WHITE - CS_C_BLACK); + float black_level = 1.0 - colour_array_out[0]; + 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); + 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); + 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); + // Normalise array + float norm_factor = 3.0/(colour_array_out[1] + colour_array_out[2] + colour_array_out[3]); + colour_array_out[1] *= norm_factor; + colour_array_out[2] *= norm_factor; + colour_array_out[3] *= norm_factor; + // int sum_black = CS_R_BLACK + CS_G_BLACK + CS_ + // colour_array_out[1] = +} + +int Colour::identify_colour_from_calibrated_colour_scores(float * calibrate_colour_array_in) +{ + float scores[8]; + 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])); + 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); + 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])); + 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); + 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])); + 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); + 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)))); + scores[6] = calibrate_colour_array_in[0] * calibrate_colour_array_in[0] * grey_factor * 4; + scores[7] = (1-calibrate_colour_array_in[0]) * (1-calibrate_colour_array_in[0]) * grey_factor * 4; + //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); + float max = 2; + int max_index = 8; + for(int i=0; i<8; i++) { + if(scores[i] > max) { + max=scores[i]; + max_index=i; + } + } + return max_index; +} + +char Colour::IF_check_base_colour_sensor(void) +{ //Reads the device ID flag of the colour sensor [0xB2] //This should equal 0x44 for both TCS34721 (top) and TCS34725 (base) sensors //Return a 1 if successful or a 0 otherwise @@ -54,3 +237,38 @@ else psi.debug("Invalid response from colour sensor:%X\n",data[0]); return return_value; } + +int Colour::IF_writeSingleRegister( char address, char data ) +{ + char tx[2] = { address | 160, data }; //0d160 = 0b10100000 + int ack = primary_i2c.write(BASE_COLOUR_ADDRESS, tx, 2, false); + return ack; +} + +int Colour::IF_writeMultipleRegisters( char address, char* data, int quantity ) +{ + char tx[ quantity + 1 ]; + tx[0] = address | 160; + for ( int i = 1; i <= quantity; i++ ) { + tx[ i ] = data[ i - 1 ]; + } + int ack = primary_i2c.write(BASE_COLOUR_ADDRESS, tx, quantity + 1, false); + return ack; +} + +char Colour::IF_readSingleRegister( char address ) +{ + char output = 255; + char command = address | 160; //0d160 = 0b10100000 + primary_i2c.write( BASE_COLOUR_ADDRESS, &command, 1, true ); + primary_i2c.read( BASE_COLOUR_ADDRESS, &output, 1 ); + return output; +} + +int Colour::IF_readMultipleRegisters( char address, char* output, int quantity ) +{ + char command = address | 160; //0d160 = 0b10100000 + primary_i2c.write(BASE_COLOUR_ADDRESS, &command, 1, true ); + int ack = primary_i2c.read( BASE_COLOUR_ADDRESS, output, quantity ); + return ack; +}