James Hilder
Psi Swarm robot library version 0.9
Fork of
PsiSwarmV8_CPP
by 
Psi Swarm Robot
Revision 15:66be5ec52c3b, committed 2017-06-01
- Comitter:
- jah128
- Date:
- Thu Jun 01 21:58:14 2017 +0000
- Parent:
- 14:2f1ad77d281e
- Commit message:
- Added colour detection code
Changed in this revision
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;
+}
diff -r 2f1ad77d281e -r 66be5ec52c3b colour.h
--- a/colour.h Tue May 30 21:03:18 2017 +0000
+++ b/colour.h Thu Jun 01 21:58:14 2017 +0000
@@ -24,6 +24,11 @@
#ifndef COLOUR_H
#define COLOUR_H
+#define ENABLE 0x00
+#define ATIME 0x01
+#define CONTROL 0x0F
+#define CDATA 0x14
+
/**
* The Colour class contains the functions for reading the base-mounted and top-mounted I2C colour sensors (optional).
*/
@@ -46,12 +51,64 @@
*/
void enable_base_colour_sensor(void);
+ /** Disable (power-down) the base colour sensor
+ */
+ void disable_base_colour_sensor(void);
+
/** Read the values from the base colour sensor
*
* @param Pointer to 3 x int array for r-g-b values
*/
void read_base_colour_sensor_values(int * store_array);
+
+ /** Function enables colour sensor, takes a reading, returns a single int (range -1 to 8)
+ *
+ * @return Output of identify_colour_from_calibrated_colour_scores - int range -1 to 8
+ */
+ int detect_colour_once();
+
+ /** Returns the most recent identified colour from detect_colour_once or detect_colour_ticker (range -1 to 8)
+ *
+ * @return Output of identify_colour_from_calibrated_colour_scores - int range -1 to 8
+ */
+ int get_detected_colour();
+
+ /** Attempts to identify a colour from a given array of calibrated colour values
+ *
+ * @param calibrate_colour_array_in : Pointer to the calibrate colour array [output of get_calibrated_colour]
+ * @return int defining detected colour: 0=RED 1=YELLOW 2=GREEN 3=CYAN 4=BLUE 5=MAGENTA 6=WHITE 7=BLACK 8=UNSURE
+ */
+ int identify_colour_from_calibrated_colour_scores(float * calibrate_colour_array_in);
+
+ /** Translate an input array of raw CT,R,G,B values into calibrated, normalised values
+ *
+ * @param colour_array_in : 4-element int input array [output of read_base_colour_sensor_values()]
+ * @param colour_array_out : Target 4-element float array of normalised values [0=CT, range 0-1 1=R 2=G 3=B, Sum R+G+B=3.0]
+ */
+ void get_calibrated_colour(int * colour_array_in, float * colour_array_out);
+
+ /** Returns a string form of the output of identify_colour_from_calibrated_colour_scores()
+ *
+ * @param colour_index : Output of identify_colour_from_calibrated_colour_scores()
+ * @return 7-character String (eg 0="RED ")
+ */
+ const char * get_colour_string(int colour_index);
+
+ /** Starts a polling ticker that cyclically checks to see if a colour can be detected
+ *
+ * @param period_ms : The approximate cycle period in milliseconds
+ */
+ void start_colour_ticker(int period_ms);
+
+
+
+ void IF_colour_ticker_start();
+ void IF_colour_ticker_main();
char IF_check_base_colour_sensor(void);
-
+ int IF_writeSingleRegister( char address, char data );
+ int IF_writeMultipleRegisters( char address, char* data, int quantity );
+ char IF_readSingleRegister( char address );
+ int IF_readMultipleRegisters( char address, char* output, int quantity );
+ float IF_roundTowardsZero( const float value );
};
#endif
\ No newline at end of file
diff -r 2f1ad77d281e -r 66be5ec52c3b demo.cpp
--- a/demo.cpp Tue May 30 21:03:18 2017 +0000
+++ b/demo.cpp Thu Jun 01 21:58:14 2017 +0000
@@ -1,11 +1,11 @@
/* University of York Robotics Laboratory PsiSwarm Library: Demo Mode 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: demo.cpp
@@ -37,6 +37,7 @@
#define LF_I_TERM 0
#define LF_D_TERM 4
+char quick_test = 0;
char top_menu = 0;
char sub_menu = 0;
char level = 0;
@@ -136,20 +137,24 @@
case 1: //Up pressed
switch(level) {
case 0:
- if(top_menu != 8) {
+ if(top_menu != 9 && top_menu!= 0) {
level++;
sub_menu = 0;
} else {
- demo_on = 0;
- user_code_running = user_code_restore_mode;
+ if(top_menu == 0) {
+ toggle_quick_test();
+ } else {
+ demo_on = 0;
+ user_code_running = user_code_restore_mode;
+ }
}
break;
case 1:
switch(top_menu) {
- case 7: // PsiBasic Menu
+ case 8: // PsiBasic Menu
if(sub_menu == psi_basic_file_count) level = 0;
break;
- case 0: //LED Menu
+ case 7: //LED Menu
if(sub_menu < 9) {
if(led_state[sub_menu] == 0) led_state[sub_menu] = 3;
else led_state[sub_menu]--;
@@ -196,7 +201,7 @@
}
if(sub_menu == 13) level = 0;
break;
- case 1: // Sensors Menu
+ case 2: // Sensors Menu
if(sub_menu == 4 || sub_menu == 5) {
if(base_ir_index == 0) base_ir_index = 4;
else base_ir_index --;
@@ -207,7 +212,7 @@
}
if(sub_menu == 11) level = 0;
break;
- case 2: // Motor Menu
+ case 3: // Motor Menu
if(sub_menu == 0) {
left_speed += 5;
if(left_speed > 100) left_speed = 100;
@@ -246,13 +251,13 @@
level = 0;
}
break;
- case 3: // Radio Menu
+ case 4: // Radio Menu
if(sub_menu == 0) level = 0;
break;
- case 4: // Info Menu
+ case 5: // Info Menu
if(sub_menu == 6) level = 0;
break;
- case 5: // Demo Menu
+ case 6: // Demo Menu
if(sub_menu == 0) {
if(demo_running == 0) {
start_line_demo();
@@ -298,20 +303,24 @@
case 2: //Down pressed
switch(level) {
case 0:
- if(top_menu != 8) {
+ if(top_menu != 9 && top_menu != 0) {
level++;
sub_menu = 0;
} else {
- demo_on = 0;
- user_code_running = user_code_restore_mode;
+ if(top_menu == 0) {
+ toggle_quick_test();
+ } else {
+ demo_on = 0;
+ user_code_running = user_code_restore_mode;
+ }
}
break;
case 1:
switch(top_menu) {
- case 7: // PsiBasic Menu
+ case 8: // PsiBasic Menu
if(sub_menu == psi_basic_file_count) level = 0;
break;
- case 0: //LED Menu
+ case 1: //LED Menu
if(sub_menu < 9) {
led_state[sub_menu]++;
if(led_state[sub_menu] == 4) led_state[sub_menu] = 0;
@@ -360,7 +369,7 @@
if(sub_menu == 13) level = 0;
break;
- case 1: // Sensors Menu
+ case 2: // Sensors Menu
if(sub_menu == 4 || sub_menu == 5) {
if(base_ir_index == 4) base_ir_index = 0;
else base_ir_index ++;
@@ -371,7 +380,7 @@
}
if(sub_menu == 11) level = 0;
break;
- case 2: // Motor Menu
+ case 3: // Motor Menu
if(sub_menu == 0) {
left_speed -= 5;
if(left_speed < -100) left_speed = -100;
@@ -410,13 +419,13 @@
level = 0;
}
break;
- case 3: // Radio Menu
+ case 4: // Radio Menu
if(sub_menu == 0) level = 0;
break;
- case 4: // Info Menu
+ case 5: // Info Menu
if(sub_menu == 6) level = 0;
break;
- case 5: // Demo Menu
+ case 6: // Demo Menu
if(sub_menu == 0) {
if(demo_running == 0) {
start_line_demo();
@@ -464,41 +473,40 @@
switch(level) {
case 0:
if(top_menu == 0) {
- top_menu = 8;
- }
- else {
- if(use_flash_basic == 0 && top_menu == 7) top_menu = 6;
+ top_menu = 9;
+ } else {
+ if(use_flash_basic == 0 && top_menu == 8) top_menu = 7;
top_menu --;
}
break;
case 1:
switch(top_menu) {
- case 0: //LED Menu
+ case 1: //LED Menu
if(sub_menu == 0) sub_menu = 13;
else sub_menu --;
break;
- case 1: //Sensors Menu
+ case 2: //Sensors Menu
if(sub_menu == 0) sub_menu = 11;
else sub_menu --;
break;
- case 2: //Motor Menu
+ case 3: //Motor Menu
if(sub_menu == 0) sub_menu = 3;
else sub_menu --;
break;
- case 4: //Info Menu
+ case 5: //Info Menu
if(sub_menu == 0) sub_menu = 6;
else sub_menu --;
break;
- case 5: //Demo Menu
+ case 6: //Demo Menu
if(sub_menu == 0) sub_menu = 4;
else sub_menu --;
break;
- case 6: //Calibrate Menu
+ case 7: //Calibrate Menu
if(sub_menu == 0) sub_menu = 2;
else sub_menu --;
break;
- case 7: //PsiBasic Menu
+ case 8: //PsiBasic Menu
if(sub_menu == 0) sub_menu = psi_basic_file_count;
else sub_menu --;
}
@@ -514,108 +522,37 @@
break;
case 1:
switch(top_menu) {
- case 0: //LED Menu
+ case 1: //LED Menu
if(sub_menu == 13) sub_menu = 0;
else sub_menu ++;
break;
- case 1: //Sensors Menu
+ case 2: //Sensors Menu
if(sub_menu == 11) sub_menu = 0;
else sub_menu ++;
break;
- case 2: //Motor Menu
+ case 3: //Motor Menu
if(sub_menu == 3) sub_menu = 0;
else sub_menu ++;
break;
- case 4: //Info Menu
+ case 5: //Info Menu
if(sub_menu == 6) sub_menu = 0;
else sub_menu ++;
break;
- case 5: //Demo Menu
+ case 6: //Demo Menu
if(sub_menu == 4) sub_menu = 0;
else sub_menu ++;
break;
- case 6: //Calibrate Menu
+ case 7: //Calibrate Menu
if(sub_menu == 2) sub_menu = 0;
else sub_menu ++;
break;
- case 7: //PsiBasic Menu
+ case 8: //PsiBasic Menu
if(sub_menu == psi_basic_file_count) sub_menu = 0;
else sub_menu ++;
}
break;
}
break;
- case 16: //Select pressed
- switch(level) {
- case 0:
- if(top_menu != 8) {
- level++;
- sub_menu = 0;
- } else {
- demo_on = 0;
- user_code_running = user_code_restore_mode;
- }
- break;
- case 1:
- switch(top_menu) {
- case 0: //LED Menu
- if(sub_menu == 9) {
- for(int i=0; i<9; i++) {
- led_state[i]=all_led_state;
- }
- demo_update_leds();
- }
- if(sub_menu == 13) level = 0;
- break;
- case 1: // Sensors Menu
- if(sub_menu == 11) level = 0;
- break;
- case 2: //Motor Menu
- if(sub_menu == 2) {
- switch(both_motor_mode) {
- case 0:
- motors.stop();
- break;
- case 1:
- motors.brake();
- break;
- case 2:
- motors.forward(0.5);
- break;
- case 3:
- motors.forward(1);
- break;
- case 4:
- motors.backward(0.5);
- break;
- case 5:
- motors.backward(1.0);
- break;
- }
- }
- if(sub_menu == 3) {
- level = 0;
- }
- break;
- case 3: // Radio Menu
- if(sub_menu == 0) level = 0;
- break;
- case 4: // Info Menu
- if(sub_menu == 6) level = 0;
- break;
- case 5: // Demo Menu
- if(sub_menu == 4) level = 0;
- break;
- case 6: // Calibrate Menu
- if(sub_menu == 2) level = 0;
- break;
- case 7: // PsiBasic Menu
- if(sub_menu == psi_basic_file_count) level = 0;
- break;
- }
- break;
- }
- break;
}
} else started = 1;
display.clear_display();
@@ -624,30 +561,33 @@
//Top level menu
switch(top_menu) {
case 0:
+ strcpy(topline,"---QUICK TEST---");
+ break;
+ case 1:
strcpy(topline,"---TEST LEDS----");
break;
- case 1:
+ case 2:
strcpy(topline,"--TEST SENSORS--");
break;
- case 2:
+ case 3:
strcpy(topline,"--TEST MOTORS---");
break;
- case 3:
- strcpy(topline,"---TEST RADIO---");
- break;
case 4:
- strcpy(topline,"------INFO------");
+ strcpy(topline,"---TEST RADIO---");
break;
case 5:
- strcpy(topline,"---CODE DEMOS---");
+ strcpy(topline,"------INFO------");
break;
case 6:
- strcpy(topline,"-MOTOR CALIBRATE");
+ strcpy(topline,"---CODE DEMOS---");
break;
case 7:
+ strcpy(topline,"-MOTOR CALIBRATE");
+ break;
+ case 8:
strcpy(topline,"---PSI BASIC----");
break;
- case 8:
+ case 9:
strcpy(topline,"------EXIT------");
break;
}
@@ -656,12 +596,12 @@
case 1:
//Sub level menu
switch(top_menu) {
- case 7:
+ case 8:
strcpy(topline,"-PSI BASIC MENU-");
if(sub_menu == psi_basic_file_count) strcpy(bottomline,"EXIT");
else strcpy(bottomline,basic_filenames.at(sub_menu).c_str());
break;
- case 0:
+ case 1:
strcpy(topline,"----LED MENU----");
char led_status[7];
if(sub_menu<9) {
@@ -709,7 +649,7 @@
if(sub_menu == 13) strcpy(bottomline,"EXIT");
break;
- case 1:
+ case 2:
strcpy(topline,"--SENSORS MENU--");
switch(sub_menu) {
case 0: {
@@ -767,7 +707,7 @@
break;
}
break;
- case 2:
+ case 3:
strcpy(topline,"--MOTORS MENU---");
switch(sub_menu) {
case 0:
@@ -805,7 +745,7 @@
break;
}
break;
- case 3:
+ case 4:
strcpy(topline,"---RADIO MENU---");
switch(sub_menu) {
@@ -814,7 +754,7 @@
break;
}
break;
- case 4:
+ case 5:
strcpy(topline,"---INFO MENU----");
switch(sub_menu) {
case 0:
@@ -841,7 +781,7 @@
break;
}
break;
- case 5:
+ case 6:
strcpy(topline,"---CODE DEMOS---");
switch(sub_menu) {
case 0:
@@ -861,7 +801,7 @@
break;
}
break;
- case 6:
+ case 7:
strcpy(topline,"-MOTOR CALIBRATE");
switch(sub_menu) {
case 0:
@@ -869,7 +809,7 @@
break;
case 1:
sprintf(bottomline,"ENTER VALUE");
- break;
+ break;
case 2:
sprintf(bottomline,"EXIT");
break;
@@ -886,6 +826,174 @@
}
}
+char test_step = 0;
+char test_substep = 0;
+char test_warnings = 0;
+
+
+void Demo::quick_test_cycle()
+{
+ char next_step [] = {4,5,8,3};
+ char test_message [17];
+ int wait_period = SELF_TEST_PERIOD * 10;
+
+ if(test_substep == 0) {
+ display.clear_display();
+ switch(test_step) {
+ case 0:
+ display.write_string("01 - Power ");
+ pc.printf("\nTest 01: Power quick tests [%f]\n",psi.get_uptime());
+ break;
+ case 1:
+ display.write_string("02 - Base IR ");
+ pc.printf("\nTest 02: Base infrared tests [%f]\n",psi.get_uptime());
+ break;
+ case 2:
+ display.write_string("03 - Side IR ");
+ pc.printf("\nTest 03: Side infrared tests [%f]\n",psi.get_uptime());
+ break;
+ case 3:
+ display.write_string("04 - LEDs ");
+ pc.printf("\nTest 04: LED quick tests [%f]\n",psi.get_uptime());
+ break;
+ }
+ }
+ if(test_step == 0) {
+ // Power self-test
+ switch(test_substep) {
+ case 0: { // Battery Voltage
+ float battery_voltages [SAMPLE_SIZE];
+ float mean_battery_voltage = 0;
+ float sd_battery_voltage = 0;
+ for(int i=0; i<SAMPLE_SIZE; i++) {
+ battery_voltages[i]=sensors.get_battery_voltage ();
+ mean_battery_voltage += battery_voltages[i];
+ wait_us(SELF_TEST_PERIOD);
+ }
+ mean_battery_voltage /= SAMPLE_SIZE;
+ for(int i=0; i<SAMPLE_SIZE; ++i) sd_battery_voltage += pow(battery_voltages[i] - mean_battery_voltage, 2);
+ sd_battery_voltage = sqrt(sd_battery_voltage/SAMPLE_SIZE);
+
+ sprintf(test_message,"BATTERY: %1.3fV",mean_battery_voltage);
+ pc.printf(" - Battery Voltage : %1.4fV [SD = % 1.4f]\n",mean_battery_voltage,sd_battery_voltage);
+ if(mean_battery_voltage < 3.6) {
+ pc.printf(" - WARNING : Battery voltage low\n");
+ test_warnings++;
+ }
+ break;
+ }
+ case 1: {// DC Voltage
+ float dc_voltages [SAMPLE_SIZE];
+ float mean_dc_voltage = 0;
+ float sd_dc_voltage = 0;
+ for(int i=0; i<SAMPLE_SIZE; i++) {
+ dc_voltages[i]=sensors.get_dc_voltage ();
+ mean_dc_voltage += dc_voltages[i];
+ wait_us(SELF_TEST_PERIOD);
+ }
+ mean_dc_voltage /= SAMPLE_SIZE;
+ for(int i=0; i<SAMPLE_SIZE; ++i) sd_dc_voltage += pow(dc_voltages[i] - mean_dc_voltage, 2);
+ sd_dc_voltage = sqrt(sd_dc_voltage/SAMPLE_SIZE);
+
+ sprintf(test_message,"DC : %1.3fV",mean_dc_voltage);
+ pc.printf(" - DC Input Voltage : %1.4fV [SD = % 1.4f]\n",mean_dc_voltage,sd_dc_voltage);
+ if(mean_dc_voltage < 0.5) {
+ pc.printf(" - WARNING : DC voltage low - check charging resistor\n");
+ test_warnings++;
+ }
+ break;
+ }
+
+
+ case 2: { // Current
+ float currents [SAMPLE_SIZE];
+ float mean_current = 0;
+ float sd_current = 0;
+ for(int i=0; i<SAMPLE_SIZE; i++) {
+ currents[i]=sensors.get_current ();
+ mean_current += currents[i];
+ wait_us(SELF_TEST_PERIOD);
+ }
+ mean_current /= SAMPLE_SIZE;
+ for(int i=0; i<SAMPLE_SIZE; ++i) sd_current += pow(currents[i] - mean_current, 2);
+ sd_current = sqrt(sd_current/SAMPLE_SIZE);
+
+ sprintf(test_message,"CURRENT: %1.3fV",mean_current);
+ pc.printf(" - Load Current : %1.4fA [SD = % 1.4f]\n",mean_current,sd_current);
+ if(mean_current > 0.2) {
+ pc.printf(" - WARNING : Higher than expected load current\n");
+ test_warnings++;
+ }
+ break;
+ }
+
+ case 3: {// System temperature
+ float temps[SAMPLE_SIZE];
+ float mean_temp = 0;
+ float sd_temp = 0;
+ for(int i=0; i<SAMPLE_SIZE; i++) {
+ temps[i]=sensors.get_temperature();
+ mean_temp += temps[i];
+ wait_us(SELF_TEST_PERIOD);
+ }
+ mean_temp /= SAMPLE_SIZE;
+ for(int i=0; i<SAMPLE_SIZE; ++i) sd_temp += pow(temps[i] - mean_temp, 2);
+ sd_temp = sqrt(sd_temp/SAMPLE_SIZE);
+
+ sprintf(test_message,"TEMP : %3.2fC",mean_temp);
+ pc.printf(" - System Temperature : %3.2fC [SD = % 1.4f]\n",mean_temp,sd_temp);
+ if(mean_temp > 32) {
+ pc.printf(" - WARNING : High system temperature detected\n");
+ test_warnings++;
+ }
+ if(mean_temp < 10) {
+ pc.printf(" - WARNING : Low system temperature detected\n");
+ test_warnings++;
+ }
+ break;
+ }
+ }
+ }
+
+ display.set_position(1,0);
+ display.write_string(" ");
+ display.set_position(1,0);
+ display.write_string(test_message);
+ test_substep++;
+ if(test_substep >= next_step[test_step]) {
+ test_substep = 0;
+ test_step ++;
+ if(test_step == 4) test_step = 0;
+ }
+
+ demo_timeout.attach_us(this,&Demo::quick_test_cycle, wait_period);
+}
+
+void Demo::toggle_quick_test()
+{
+ quick_test = 1 - quick_test;
+ if(quick_test == 0) {
+ pc.printf("________________________________________\n");
+ pc.printf("Self test stopped at %f with %d warnings\n\n",psi.get_uptime(),test_warnings);
+ demo_running = 0;
+ demo_timeout.detach();
+ display.set_backlight_brightness(1);
+ display.clear_display();
+ display.write_string("---QUICK TEST---");
+
+
+ } else {
+ // Reset all LEDs, motors, display
+ pc.printf("\n________________________________________\n");
+ pc.printf("Self test started at %f\n\n",psi.get_uptime(),test_warnings);
+ display.clear_display();
+ test_step = 0;
+ test_substep = 0;
+ demo_running = 1;
+ demo_timeout.attach_us(this,&Demo::quick_test_cycle,1000);
+ }
+}
+
void Demo::start_line_demo()
{
display.set_backlight_brightness(0);
@@ -976,35 +1084,35 @@
lf_left = 0;
else if (lf_left > 1.0f)
lf_left = 1.0f;
- }else{
- //Cannot see line: hunt for it
- if(lf_left > lf_right){
- //Currently turning left, keep turning left
- state ++;
- float d_step = state * 0.04;
- lf_left = 0.2 + d_step;
- lf_right = -0.2 - d_step;
- if(state > 20){
- state = 0;
- lf_right = 0.2;
- lf_left = -0.2;
- time_out += 0.01f;
- if(time_out > 0.1f) demo_running = 0;
+ } else {
+ //Cannot see line: hunt for it
+ if(lf_left > lf_right) {
+ //Currently turning left, keep turning left
+ state ++;
+ float d_step = state * 0.04;
+ lf_left = 0.2 + d_step;
+ lf_right = -0.2 - d_step;
+ if(state > 20) {
+ state = 0;
+ lf_right = 0.2;
+ lf_left = -0.2;
+ time_out += 0.01f;
+ if(time_out > 0.1f) demo_running = 0;
+ }
+ } else {
+ //Currently turning right, keep turning right
+ state ++;
+ float d_step = state * 0.04;
+ lf_left = -0.2 - d_step;
+ lf_right = 0.2 + d_step;
+ if(state > 20) {
+ state = 0;
+ lf_right = -0.2;
+ lf_left = 0.2;
+ time_out += 0.01f;
+ if(time_out > 0.1f) demo_running = 0;
+ }
}
- }else{
- //Currently turning right, keep turning right
- state ++;
- float d_step = state * 0.04;
- lf_left = -0.2 - d_step;
- lf_right = 0.2 + d_step;
- if(state > 20){
- state = 0;
- lf_right = -0.2;
- lf_left = 0.2;
- time_out += 0.01f;
- if(time_out > 0.1f) demo_running = 0;
- }
- }
}
// set speed
motors.set_left_motor_speed(lf_left);
diff -r 2f1ad77d281e -r 66be5ec52c3b demo.h
--- a/demo.h Tue May 30 21:03:18 2017 +0000
+++ b/demo.h Thu Jun 01 21:58:14 2017 +0000
@@ -53,5 +53,7 @@
void obstacle_demo_cycle(void);
void spinning_demo_cycle(void);
void stress_demo_cycle(void);
+ void quick_test_cycle(void);
+ void toggle_quick_test(void);
};
#endif
\ No newline at end of file
diff -r 2f1ad77d281e -r 66be5ec52c3b display.cpp
--- a/display.cpp Tue May 30 21:03:18 2017 +0000
+++ b/display.cpp Thu Jun 01 21:58:14 2017 +0000
@@ -64,9 +64,7 @@
wait(0.01);
return c;
}
-
-void Display::init_display(char mode){
- //Set initial states: display on, cursor off
+void Display::init_display_start(){
display_on = 1;
set_backlight_brightness(1);
cursor_on = 0;
@@ -95,14 +93,16 @@
write_string(psis,16);
set_position(1,0);
write_string(psis,16);
- wait(0.25);
+}
+
+void Display::init_display_end(char mode){
clear_display();
if(mode == 0) {
set_position(0,0);
write_string(" YORK ROBOTICS");
set_position(1,0);
write_string(" LABORATORY");
- init_timeout.attach(this,&Display::post_init,0.3);}
+ init_timeout.attach(this,&Display::post_init,0.25);}
else {
set_position(0,0);
write_string("Hold button to");
@@ -111,6 +111,30 @@
}
}
+void Display::show_switch_state(char switch_state){
+ switch(switch_state){
+ /// Switch_state = 1 if up is pressed, 2 if down is pressed, 4 if left is pressed, 8 if right is pressed and 16 if the center button is pressed
+ case 0: write_string("REL "); break;
+ case 1: write_string("UP "); break;
+ case 2: write_string("DOWN "); break;
+ case 4: write_string("LEFT "); break;
+ case 5: write_string("UP-L "); break;
+ case 6: write_string("DN-L "); break;
+ case 8: write_string("RIGHT "); break;
+ case 9: write_string("UP-R "); break;
+ case 10: write_string("DN-R "); break;
+ case 16: write_string("PRESS"); break;
+ case 17: write_string("UP *"); break;
+ case 18: write_string("DOWN *"); break;
+ case 20: write_string("LEFT *"); break;
+ case 21: write_string("UP-L *"); break;
+ case 22: write_string("DN-L *"); break;
+ case 24: write_string("RIGHT*"); break;
+ case 25: write_string("UP-R *"); break;
+ case 26: write_string("DN-R *"); break;
+ }
+}
+
void Display::post_init(){
clear_display();
home();
@@ -120,7 +144,7 @@
sprintf(line,"VERSION %1.2f", SOFTWARE_VERSION_CODE );
set_position(1,0);
write_string(line);
- init_timeout.attach(this,&Display::post_post_init,0.3);
+ init_timeout.attach(this,&Display::post_post_init,0.25);
}
void Display::post_post_init(){
diff -r 2f1ad77d281e -r 66be5ec52c3b display.h
--- a/display.h Tue May 30 21:03:18 2017 +0000
+++ b/display.h Thu Jun 01 21:58:14 2017 +0000
@@ -116,6 +116,11 @@
*/
void set_backlight_brightness(float brightness);
+ /** Display the switch state at the current cursor position
+ * @param switch_state - The value of the cursor switch (range 0 to 31)
+ */
+ void show_switch_state(char switch_state);
+
// Special function for when debug messages are sent to display
void debug_page(char * message, char length);
@@ -135,8 +140,11 @@
// Send a 1-byte control message to the display
int i2c_message(char byte);
- // Default initialisation sequence for the display
- void init_display(char mode);
+ // Default initialisation sequence for the display (start of boot)
+ void init_display_start();
+
+ // Default initialisation sequence for the display (end of boot)
+ void init_display_end(char mode);
int disp_putc(int c);
diff -r 2f1ad77d281e -r 66be5ec52c3b psiswarm.cpp
--- a/psiswarm.cpp Tue May 30 21:03:18 2017 +0000
+++ b/psiswarm.cpp Thu Jun 01 21:58:14 2017 +0000
@@ -1,6 +1,6 @@
/* University of York Robotics Laboratory PsiSwarm Library: PsiSwarm C++ Core Source File
*
- * Copyright 2016 University of York
+ * 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.
* You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
@@ -13,9 +13,9 @@
* (C) Dept. Electronics & Computer Science, University of York
* James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
*
- * PsiSwarm Library Version: 0.8
+ * PsiSwarm Library Version: 0.9
*
- * October 2016
+ * May 2017
*
*
*/
@@ -175,6 +175,8 @@
primary_i2c.frequency(400000);
serial.setup_serial_interfaces();
debug("PsiSwarm Robot Library %1.2f\n\n",SOFTWARE_VERSION_CODE);
+ debug("- Set up display\n");
+ display.init_display_start();
debug("- Setting up serial interface\n");
debug("- Reading firmware: ");
if(eprom.read_firmware() == 1) {
@@ -183,13 +185,13 @@
if(use_motor_calibration){
if(!motor_calibration_set){
if(firmware_version < 1.1){
- debug("- WARNING: This firmware is incompatible with motor calibration");
- debug("- WARNING: Please update the firmware to use this feature.");
+ debug("- WARNING: This firmware is incompatible with motor calibration\n");
+ debug("- WARNING: Please update the firmware to use this feature.\n");
use_motor_calibration = 0;
}
else {
- debug("- WARNING: Motor calibration values have not been stored in firmware");
- debug("- WARNING: Run calibration routine to use this feature.");
+ debug("- WARNING: Motor calibration values have not been stored in firmware\n");
+ debug("- WARNING: Run calibration routine to use this feature.\n");
use_motor_calibration = 0;
}
}
@@ -244,15 +246,16 @@
}
char demo_on = 0;
if(ENABLE_DEMO == 1 && switchstate > 0) demo_on=1;
- display.init_display(demo_on);
+ display.init_display_end(demo_on);
event_handler.attach_us(this,&Psiswarm::IF_handle_events, 1000);
if(demo_on > 0) {
debug("- Demo mode button is pressed\n");
- wait(1.0);
+ wait(0.6);
demo_on = i2c_setup.IF_get_switch_state();
if(demo_on > 0) demo.start_demo_mode();
- display.init_display(0);
+ display.init_display_end(0);
}
+ debug("___________________________________________________");
}
void Psiswarm::IF_update_minutes()
diff -r 2f1ad77d281e -r 66be5ec52c3b psiswarm.h
--- a/psiswarm.h Tue May 30 21:03:18 2017 +0000
+++ b/psiswarm.h Thu Jun 01 21:58:14 2017 +0000
@@ -1,6 +1,6 @@
/* University of York Robotics Laboratory PsiSwarm Library: PsiSwarm C++ Core Header File
*
- * Copyright 2016 University of York
+ * 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.
* You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
@@ -13,9 +13,9 @@
* (C) Dept. Electronics & Computer Science, University of York
* James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
*
- * PsiSwarm Library Version: 0.8
+ * PsiSwarm Library Version: 0.9
*
- * October 2016
+ * May 2017
*
*
*/
@@ -23,7 +23,7 @@
#ifndef PSISWARM_H
#define PSISWARM_H
-#define SOFTWARE_VERSION_CODE 0.80
+#define SOFTWARE_VERSION_CODE 0.90
#define PIC_ADDRESS 0x30
#define LCD_ADDRESS 0x7C
@@ -85,7 +85,7 @@
* Psiswarm psi;
* char * program_name = "Example";
* char * author_name = "Name";
- * char * version_name = "0.8";
+ * char * version_name = "0.9";
* void handle_switch_event(char switch_state){}
* void handle_user_serial_message(char * message, char length, char interface) {}
* int main(){
@@ -154,6 +154,7 @@
extern Demo demo;
extern Animations animations;
extern Basic basic;
+extern Colour colour;
extern char * program_name;
extern char * author_name;
extern char * version_name;
diff -r 2f1ad77d281e -r 66be5ec52c3b settings.h --- a/settings.h Tue May 30 21:03:18 2017 +0000 +++ b/settings.h Thu Jun 01 21:58:14 2017 +0000 @@ -96,5 +96,10 @@ /* DEBUG_OUTPUT_STREAM [1=PC\USB 2=BlueSmirf 4=Display]: Specify which output stream(s) should be used by default for debug messages, if enabled*/ #define DEBUG_OUTPUT_STREAM 1 +/* SELF_TEST_PERIOD [recommended = 10000]: Step time (in uS) for the quick self-test in the demo mode*/ +#define SELF_TEST_PERIOD 10000 + +/* SAMPLE_SIZE [recommended = 10]: Number of samples to take for the quick self-test in the demo mode*/ +#define SAMPLE_SIZE 10 #endif