Psi Swarm robot library version 0.9
Fork of PsiSwarmV9 by
Diff: colour.cpp
- Revision:
- 17:bf614e28668f
- Parent:
- 16:50686c07ad07
- Child:
- 18:9204f74069b4
--- a/colour.cpp Thu Jun 01 23:02:32 2017 +0000 +++ b/colour.cpp Sun Jun 04 13:11:09 2017 +0000 @@ -26,10 +26,23 @@ int last_detected_colour = -1; +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; + +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){ + cs_c_black = c_black; + cs_r_black = r_black; + cs_g_black = g_black; + cs_b_black = b_black; + cs_c_white = c_white; + cs_r_white = r_white; + cs_g_white = g_white; + cs_b_white = b_white; +} + void Colour::colour_sensor_init() { - colour.set_base_colour_sensor_integration_time(3); - colour.set_base_colour_sensor_gain(4); + colour.set_base_colour_sensor_integration_time(BASE_COLOUR_SENSOR_INTEGRATION_TIME); + colour.set_base_colour_sensor_gain(BASE_COLOUR_SENSOR_GAIN); } void Colour::read_base_colour_sensor_values(int * store_array) @@ -187,19 +200,19 @@ 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); + 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); + 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_ + // int sum_black = cs_r_black + cs_g_black + CS_ // colour_array_out[1] = }