Psi Swarm Robot
Psi Swarm robot library version 0.9
Fork of
PsiSwarmV9
by 
James Hilder
Diff: demo.cpp
- Revision:
- 16:50686c07ad07
- Parent:
- 15:66be5ec52c3b
- Child:
- 17:bf614e28668f
--- a/demo.cpp Thu Jun 01 21:58:14 2017 +0000
+++ b/demo.cpp Thu Jun 01 23:02:32 2017 +0000
@@ -15,7 +15,7 @@
*
* PsiSwarm Library Version: 0.9
*
- * Version 0.9 : TODO:Added colour sensor functions
+ * Version 0.9 : Added colour sensor functions, colour sensor demo and self-test mode
* Version 0.8 : Rewritten as OO\C++
* Version 0.7 : Updated for new MBED API
* Version 0.5 : Added motor calibration menu
@@ -210,7 +210,7 @@
if(side_ir_index == 0) side_ir_index = 7;
else side_ir_index --;
}
- if(sub_menu == 11) level = 0;
+ if(sub_menu == 12) level = 0;
break;
case 3: // Motor Menu
if(sub_menu == 0) {
@@ -294,7 +294,16 @@
motors.stop();
}
}
- if(sub_menu == 4) level = 0;
+ if(sub_menu == 4) {
+ if(demo_running == 0) {
+ start_colour_demo();
+ } else {
+ demo_running = 0;
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ motors.stop();
+ }
+ }
+ if(sub_menu == 5) level = 0;
break;
}
break;
@@ -378,7 +387,7 @@
if(side_ir_index == 7) side_ir_index = 0;
else side_ir_index ++;
}
- if(sub_menu == 11) level = 0;
+ if(sub_menu == 12) level = 0;
break;
case 3: // Motor Menu
if(sub_menu == 0) {
@@ -463,7 +472,16 @@
motors.stop();
}
}
- if(sub_menu == 4) level = 0;
+ if(sub_menu == 4) {
+ if(demo_running == 0) {
+ start_colour_demo();
+ } else {
+ demo_running = 0;
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ motors.stop();
+ }
+ }
+ if(sub_menu == 5) level = 0;
break;
}
break;
@@ -486,7 +504,7 @@
else sub_menu --;
break;
case 2: //Sensors Menu
- if(sub_menu == 0) sub_menu = 11;
+ if(sub_menu == 0) sub_menu = 12;
else sub_menu --;
break;
@@ -499,7 +517,7 @@
else sub_menu --;
break;
case 6: //Demo Menu
- if(sub_menu == 0) sub_menu = 4;
+ if(sub_menu == 0) sub_menu = 5;
else sub_menu --;
break;
case 7: //Calibrate Menu
@@ -527,7 +545,7 @@
else sub_menu ++;
break;
case 2: //Sensors Menu
- if(sub_menu == 11) sub_menu = 0;
+ if(sub_menu == 12) sub_menu = 0;
else sub_menu ++;
break;
case 3: //Motor Menu
@@ -539,7 +557,7 @@
else sub_menu ++;
break;
case 6: //Demo Menu
- if(sub_menu == 4) sub_menu = 0;
+ if(sub_menu == 5) sub_menu = 0;
else sub_menu ++;
break;
case 7: //Calibrate Menu
@@ -703,6 +721,9 @@
else sprintf(bottomline,"LINE:---------");
break;
case 11:
+ sprintf(bottomline,"COLOUR: %s",colour.get_colour_string(colour.detect_colour_once()));
+ break;
+ case 12:
sprintf(bottomline,"EXIT");
break;
}
@@ -797,6 +818,9 @@
sprintf(bottomline,"STRESS TEST");
break;
case 4:
+ sprintf(bottomline,"COLOUR WALK");
+ break;
+ case 5:
sprintf(bottomline,"EXIT");
break;
}
@@ -821,7 +845,8 @@
display.write_string(topline);
display.set_position(1,0);
display.write_string(bottomline);
- if(top_menu == 1 && level == 1) {
+ // Periodically update when on sensors menu
+ if(top_menu == 2 && level == 1) {
demo_event.attach(this,&Demo::demo_event_thread, 0.25);
}
}
@@ -1018,6 +1043,19 @@
demo_timeout.attach_us(this,&Demo::obstacle_demo_cycle,1000);
}
+void Demo::start_colour_demo()
+{
+ display.set_backlight_brightness(0);
+ time_out = 0.25f;
+ demo_timer.start();
+ state = 0;
+ speed = 0;
+ led_step = 0;
+ demo_running = 1;
+ colour.start_colour_ticker(100);
+ demo_timeout.attach_us(this,&Demo::colour_demo_cycle,1000);
+}
+
void Demo::start_stress_demo()
{
display.set_backlight_brightness(0.25);
@@ -1413,8 +1451,74 @@
}
}
+void Demo::colour_demo_cycle()
+{
+ if(demo_timer.read() > time_out) {
+ int col = colour.get_detected_colour();
+ switch(col){
+ case 0: led.set_leds(0,0xFF);
+ led.set_center_led(1,1);
+ break;
+ case 1: led.set_leds(0xFF,0xFF);
+ led.set_center_led(3,1);
+ break;
+ case 2: led.set_leds(0xFF,0);
+ led.set_center_led(2,1);
+ break;
+ default: led.set_leds(0,0);
+ led.set_center_led(0,0);
+ break;
+ }
+ switch(state) {
+ case 0: //Robot is stopped
+ speed = 0.2f;
+ motors.forward(speed);
+ time_out = 0.05;
+ state = 1;
+ break;
+ case 1: { //Motor is moving forward
+ sensors.store_ir_values();
+ int front_right = sensors.read_illuminated_raw_ir_value(0);
+ int front_left = sensors.read_illuminated_raw_ir_value(7);
+ if(front_left > 400 || front_right > 400) {
+ motors.brake();
+ time_out = 0.04;
+ if(front_left > front_right)state=2;
+ else state=3;
+ } else {
+ if(speed < 0.5) {
+ speed += 0.03;
+ motors.forward(speed);
+ }
+ }
+ break;
+ }
+ case 2: //Turn right
+ motors.set_left_motor_speed(0.35);
+ motors.set_right_motor_speed(-0.35);
+ time_out = 0.5;
+ state = 0;
+ break;
+ case 3: //Turn left
+ motors.set_left_motor_speed(-0.35);
+ motors.set_right_motor_speed(0.35);
+ time_out = 0.5;
+ state = 0;
+ break;
+ }
+ demo_timer.reset();
+ }
+ if(demo_running == 1)demo_timeout.attach_us(this,&Demo::colour_demo_cycle,200);
+ else {
+ motors.stop();
+ display.set_backlight_brightness(bl_brightness * 0.01f);
+ }
+}
+
+
void Demo::obstacle_demo_cycle()
{
+
if(demo_timer.read() > time_out) {
switch(state) {
case 0: //Robot is stopped
@@ -1488,6 +1592,7 @@
}
}
+
void Demo::demo_update_leds()
{
char red = 0;