C++ Library for the PsiSwarm Robot - Version 0.8
Dependents: PsiSwarm_V8_Blank_CPP Autonomia_RndmWlk
Fork of PsiSwarmV7_CPP by
Diff: demo.cpp
- Revision:
- 0:d6269d17c8cf
- Child:
- 2:c6986ee3c7c5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/demo.cpp Thu Feb 04 21:48:54 2016 +0000 @@ -0,0 +1,1355 @@ +/* University of York Robotics Laboratory PsiSwarm Library: Demo Mode Source File + * + * File: demo.cpp + * + * (C) Dept. Electronics & Computer Science, University of York + * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis + * + * PsiSwarm Library Version: 0.4 + * + * Version 0.4 : Added PsiSwarmBasic menu + * Version 0.2 : Remove most the functionality from center-button push to allow all operations to be accessable from + * four directions alone. + * Added extra sensor information, added various testing demos + * + * February 2016 + * + */ + + +#include "psiswarm.h" + +// PID terms +#define LF_P_TERM 0.2 +#define LF_I_TERM 0 +#define LF_D_TERM 4 + +char top_menu = 0; +char sub_menu = 0; +char level = 0; +char started = 0; +char topline[17]; +char bottomline[17]; +char led_state[9]; +char all_led_state = 0; +char base_led_state = 0; +char brightness = 20; +char bl_brightness = 100; +char base_ir_index = 0; +char side_ir_index = 0; +signed short left_speed = 0; +signed short right_speed = 0; +char both_motor_mode = 0; +char last_switch_pressed; +Timeout demo_event; +char handling_event = 0; + +Timeout demo_timeout; +char demo_running = 0; +Timer demo_timer; +float time_out; +float speed; +char state; +char led_step = 0; +char spin_step = 0; +char stress_step = 0; + + +float lf_right; +float lf_left; +float lf_current_pos_of_line = 0.0; +float lf_previous_pos_of_line = 0.0; +float lf_derivative,lf_proportional,lf_integral = 0; +float lf_power; +float lf_speed = 0.4; + + + +void demo_mode() +{ + debug("- Starting Demo Mode\n"); + if(use_flash_basic == 1) top_menu = 7; + demo_on = 1; + display.set_backlight_brightness(bl_brightness * 0.01f); + display.clear_display(); + display.write_string("PSI SWARM SYSTEM"); + display.set_position(1,0); + display.write_string(" DEMO MODE"); + wait(0.5); + display.clear_display(); + display.write_string("Use cursor to"); + display.set_position(1,0); + display.write_string("navigate menus"); + char step = 0; + while(demo_on) { + if(demo_running == 0) { + switch(step) { + case 0: + mbed_led1 = 1; + mbed_led2 = 0; + break; + case 1: + mbed_led2 = 1; + mbed_led1 = 0; + break; + } + step++; + if(step==2)step=0; + } else { + mbed_led1 = 0; + mbed_led2 = 0; + mbed_led3 = 0; + mbed_led4 = 0; + } + wait(0.5); + } + debug("- Demo mode ended\n"); +} + +void demo_handle_switch_event(char switch_pressed) +{ + if(!handling_event) { + handling_event = 1; + last_switch_pressed = switch_pressed; + demo_event.attach_us(&demo_event_thread, 1000); + } +} + +void demo_event_thread() +{ + handling_event = 0; + if(started == 1) { + switch(last_switch_pressed) { + case 1: //Up pressed + switch(level) { + case 0: + if(top_menu != 6) { + level++; + sub_menu = 0; + } else { + demo_on = 0; + user_code_running = user_code_restore_mode; + } + break; + case 1: + switch(top_menu) { + case 7: // PsiBasic Menu + if(sub_menu == psi_basic_file_count) level = 0; + break; + case 0: //LED Menu + if(sub_menu < 9) { + if(led_state[sub_menu] == 0) led_state[sub_menu] = 3; + else led_state[sub_menu]--; + demo_update_leds(); + } + if(sub_menu == 9) { + if(all_led_state == 0) all_led_state = 3; + else all_led_state--; + for(int i=0; i<9; i++) { + led_state[i]=all_led_state; + } + demo_update_leds(); + } + if(sub_menu == 10) { + base_led_state = 1 - base_led_state; + demo_update_leds(); + } + if(sub_menu == 11) { + switch(brightness) { + case 100: + brightness = 50; + break; + case 2: + brightness = 1; + break; + case 5: + brightness = 2; + break; + case 10: + brightness = 5; + break; + case 20: + brightness = 10; + break; + case 50: + brightness = 20; + break; + } + demo_update_leds(); + } + if(sub_menu == 12) { + if(bl_brightness > 0) bl_brightness-=10; + display.set_backlight_brightness(bl_brightness * 0.01f); + } + if(sub_menu == 13) level = 0; + break; + case 1: // Sensors Menu + if(sub_menu == 4 || sub_menu == 5) { + if(base_ir_index == 0) base_ir_index = 4; + else base_ir_index --; + } + if(sub_menu > 5 && sub_menu < 9) { + if(side_ir_index == 0) side_ir_index = 7; + else side_ir_index --; + } + if(sub_menu == 11) level = 0; + break; + case 2: // Motor Menu + if(sub_menu == 0) { + left_speed += 5; + if(left_speed > 100) left_speed = 100; + set_left_motor_speed(left_speed / 100.0f); + } + if(sub_menu == 1) { + right_speed += 5; + if(right_speed > 100) right_speed = 100; + set_right_motor_speed(right_speed / 100.0f); + } + if(sub_menu == 2) { + if(both_motor_mode == 0) both_motor_mode=5; + else both_motor_mode--; + switch(both_motor_mode) { + case 0: + stop(); + break; + case 1: + brake(); + break; + case 2: + forward(0.5); + break; + case 3: + forward(1); + break; + case 4: + backward(0.5); + break; + case 5: + 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 == 0) { + if(demo_running == 0) { + start_line_demo(); + } else { + demo_running = 0; + display.set_backlight_brightness(bl_brightness * 0.01f); + stop(); + } + } + if(sub_menu == 1) { + if(demo_running == 0) { + start_obstacle_demo(); + } else { + demo_running = 0; + display.set_backlight_brightness(bl_brightness * 0.01f); + stop(); + } + } + if(sub_menu == 2) { + if(demo_running == 0) { + start_spinning_demo(); + } else { + demo_running = 0; + display.set_backlight_brightness(bl_brightness * 0.01f); + stop(); + } + } + if(sub_menu == 3) { + if(demo_running == 0) { + start_stress_demo(); + } else { + demo_running = 0; + display.set_backlight_brightness(bl_brightness * 0.01f); + stop(); + } + } + if(sub_menu == 4) level = 0; + break; + } + break; + } + break; + case 2: //Down pressed + switch(level) { + case 0: + if(top_menu != 6) { + level++; + sub_menu = 0; + } else { + demo_on = 0; + user_code_running = user_code_restore_mode; + } + break; + case 1: + switch(top_menu) { + case 7: // PsiBasic Menu + if(sub_menu == psi_basic_file_count) level = 0; + break; + case 0: //LED Menu + if(sub_menu < 9) { + led_state[sub_menu]++; + if(led_state[sub_menu] == 4) led_state[sub_menu] = 0; + demo_update_leds(); + } + if(sub_menu == 9) { + all_led_state++; + if(all_led_state == 4) all_led_state = 0; + for(int i=0; i<9; i++) { + led_state[i]=all_led_state; + } + demo_update_leds(); + } + if(sub_menu == 10) { + base_led_state = 1 - base_led_state; + demo_update_leds(); + } + if(sub_menu == 11) { + switch(brightness) { + case 1: + brightness = 2; + break; + case 2: + brightness = 5; + break; + case 5: + brightness = 10; + break; + case 10: + brightness = 20; + break; + case 20: + brightness = 50; + break; + case 50: + brightness = 100; + break; + } + demo_update_leds(); + } + + if(sub_menu == 12) { + if(bl_brightness < 100) bl_brightness+=10; + display.set_backlight_brightness(bl_brightness * 0.01f); + } + if(sub_menu == 13) level = 0; + + break; + case 1: // Sensors Menu + if(sub_menu == 4 || sub_menu == 5) { + if(base_ir_index == 4) base_ir_index = 0; + else base_ir_index ++; + } + if(sub_menu > 5 && sub_menu < 9) { + if(side_ir_index == 7) side_ir_index = 0; + else side_ir_index ++; + } + if(sub_menu == 11) level = 0; + break; + case 2: // Motor Menu + if(sub_menu == 0) { + left_speed -= 5; + if(left_speed < -100) left_speed = -100; + set_left_motor_speed(left_speed / 100.0f); + } + if(sub_menu == 1) { + right_speed -= 5; + if(right_speed < -100) right_speed = -100; + set_right_motor_speed(right_speed / 100.0f); + } + if(sub_menu == 2) { + both_motor_mode++; + if(both_motor_mode == 6) both_motor_mode=0; + switch(both_motor_mode) { + case 0: + stop(); + break; + case 1: + brake(); + break; + case 2: + forward(0.5); + break; + case 3: + forward(1); + break; + case 4: + backward(0.5); + break; + case 5: + 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 == 0) { + if(demo_running == 0) { + start_line_demo(); + } else { + demo_running = 0; + display.set_backlight_brightness(bl_brightness * 0.01f); + stop(); + } + } + if(sub_menu == 1) { + if(demo_running == 0) { + start_obstacle_demo(); + } else { + demo_running = 0; + display.set_backlight_brightness(bl_brightness * 0.01f); + stop(); + + } + } + if(sub_menu == 2) { + if(demo_running == 0) { + start_spinning_demo(); + } else { + demo_running = 0; + display.set_backlight_brightness(bl_brightness * 0.01f); + stop(); + } + } + if(sub_menu == 3) { + if(demo_running == 0) { + start_stress_demo(); + } else { + demo_running = 0; + display.set_backlight_brightness(bl_brightness * 0.01f); + stop(); + } + } + if(sub_menu == 4) level = 0; + break; + } + break; + } + break; + case 4: //Left pressed + switch(level) { + case 0: + if(top_menu == 0) { + top_menu = 6; + if(use_flash_basic == 1) top_menu = 7; + } + else top_menu --; + break; + case 1: + switch(top_menu) { + case 0: //LED Menu + if(sub_menu == 0) sub_menu = 13; + else sub_menu --; + break; + case 1: //Sensors Menu + if(sub_menu == 0) sub_menu = 11; + else sub_menu --; + break; + + case 2: //Motor Menu + if(sub_menu == 0) sub_menu = 3; + else sub_menu --; + break; + case 4: //Info Menu + if(sub_menu == 0) sub_menu = 6; + else sub_menu --; + break; + case 5: //Demo Menu + if(sub_menu == 0) sub_menu = 4; + else sub_menu --; + break; + case 7: //PsiBasic Menu + if(sub_menu == 0) sub_menu = psi_basic_file_count; + else sub_menu --; + } + break; + + } + break; + case 8: //Right pressed + switch(level) { + case 0: + top_menu ++; + if((top_menu - use_flash_basic) > 6) top_menu = 0; + break; + case 1: + switch(top_menu) { + case 0: //LED Menu + if(sub_menu == 13) sub_menu = 0; + else sub_menu ++; + break; + case 1: //Sensors Menu + if(sub_menu == 11) sub_menu = 0; + else sub_menu ++; + break; + case 2: //Motor Menu + if(sub_menu == 3) sub_menu = 0; + else sub_menu ++; + break; + case 4: //Info Menu + if(sub_menu == 6) sub_menu = 0; + else sub_menu ++; + break; + case 5: //Demo Menu + if(sub_menu == 4) sub_menu = 0; + else sub_menu ++; + break; + case 7: //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 != 6) { + 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: + stop(); + break; + case 1: + brake(); + break; + case 2: + forward(0.5); + break; + case 3: + forward(1); + break; + case 4: + backward(0.5); + break; + case 5: + 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 7: // PsiBasic Menu + if(sub_menu == psi_basic_file_count) level = 0; + break; + } + break; + } + break; + } + } else started = 1; + display.clear_display(); + switch(level) { + case 0: + //Top level menu + switch(top_menu) { + case 0: + strcpy(topline,"---TEST LEDS----"); + break; + case 1: + strcpy(topline,"--TEST SENSORS--"); + break; + case 2: + strcpy(topline,"--TEST MOTORS---"); + break; + case 3: + strcpy(topline,"---TEST RADIO---"); + break; + case 4: + strcpy(topline,"------INFO------"); + break; + case 5: + strcpy(topline,"---CODE DEMOS---"); + break; + case 6: + strcpy(topline,"------EXIT------"); + break; + case 7: + strcpy(topline,"---PSI BASIC----"); + break; + } + strcpy(bottomline,""); + break; + case 1: + //Sub level menu + switch(top_menu) { + case 7: + 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: + strcpy(topline,"----LED MENU----"); + char led_status[7]; + if(sub_menu<9) { + switch(led_state[sub_menu]) { + case 0: + strcpy(led_status,"OFF"); + break; + case 1: + strcpy(led_status,"RED"); + break; + case 2: + strcpy(led_status,"GREEN"); + break; + case 3: + strcpy(led_status,"YELLOW"); + break; + } + } + if(sub_menu < 8) sprintf(bottomline,"OUTER %d: %s",(sub_menu + 1),led_status); + if(sub_menu == 8) sprintf(bottomline,"CENTER: %s",led_status); + if(sub_menu == 9) { + switch(all_led_state) { + case 0: + strcpy(led_status,"OFF"); + break; + case 1: + strcpy(led_status,"RED"); + break; + case 2: + strcpy(led_status,"GREEN"); + break; + case 3: + strcpy(led_status,"YELLOW"); + break; + } + sprintf(bottomline,"SET ALL %s", led_status); + } + if(sub_menu == 10) { + if(base_led_state == 0) strcpy(led_status,"OFF"); + else strcpy(led_status,"ON"); + sprintf(bottomline,"BASE: %s",led_status); + } + if(sub_menu == 11) sprintf(bottomline,"CLED BNESS %d%%", brightness); + if(sub_menu == 12) sprintf(bottomline,"DISP BNESS %d%%", bl_brightness); + if(sub_menu == 13) strcpy(bottomline,"EXIT"); + break; + + case 1: + strcpy(topline,"--SENSORS MENU--"); + switch(sub_menu) { + case 0: { + float battery = get_battery_voltage (); + sprintf(bottomline,"BATTERY: %1.3fV",battery); + break; + } + case 1: { + float dc = get_dc_voltage (); + sprintf(bottomline,"DC: %1.3fV",dc); + break; + } + case 2: { + float current = get_current (); + sprintf(bottomline,"CURRENT: %1.3fA",current); + break; + } + case 3: { + float temperature = get_temperature(); + sprintf(bottomline,"TEMP: %3.2fC", temperature); + break; + } + case 4: + store_background_base_ir_values(); + sprintf(bottomline,"BIR%dB:%d",base_ir_index+1,get_background_base_ir_value(base_ir_index)); + break; + case 5: + store_illuminated_base_ir_values(); + sprintf(bottomline,"BIR%dR:%d",base_ir_index+1,get_illuminated_base_ir_value(base_ir_index)); + break; + case 6: + store_background_raw_ir_values(); + sprintf(bottomline,"SIR%dB:%d",side_ir_index+1,get_background_raw_ir_value(side_ir_index)); + break; + case 7: + store_illuminated_raw_ir_values(); + sprintf(bottomline,"SIR%dR:%d",side_ir_index+1,get_illuminated_raw_ir_value(side_ir_index)); + break; + case 8: + sprintf(bottomline,"SIR%dD:%3.1f",side_ir_index+1,0.0); + break; + case 9: + if(ultrasonic_distance_updated == 1) { + sprintf(bottomline,"USONIC:%3dcm",ultrasonic_distance); + } else sprintf(bottomline,"USONIC:---------"); + update_ultrasonic_measure(); + break; + case 10: + store_line_position(); + if(line_found == 1)sprintf(bottomline,"LINE:%1.3f",line_position); + else sprintf(bottomline,"LINE:---------"); + break; + case 11: + sprintf(bottomline,"EXIT"); + break; + } + break; + case 2: + strcpy(topline,"--MOTORS MENU---"); + switch(sub_menu) { + case 0: + sprintf(bottomline,"LEFT: %d%%", left_speed); + break; + case 1: + sprintf(bottomline,"RIGHT: %d%%", right_speed); + break; + case 2: + char both_mode_string[16]; + switch(both_motor_mode) { + case 0: + strcpy(both_mode_string,"OFF"); + break; + case 1: + strcpy(both_mode_string,"BRAKE"); + break; + case 2: + strcpy(both_mode_string,"+50%"); + break; + case 3: + strcpy(both_mode_string,"+100%"); + break; + case 4: + strcpy(both_mode_string,"-50%"); + break; + case 5: + strcpy(both_mode_string,"-100%"); + break; + } + sprintf(bottomline,"BOTH TO %s",both_mode_string); + break; + case 3: + sprintf(bottomline,"EXIT"); + break; + } + break; + case 3: + strcpy(topline,"---RADIO MENU---"); + switch(sub_menu) { + + case 0: + sprintf(bottomline,"EXIT"); + break; + } + break; + case 4: + strcpy(topline,"---INFO MENU----"); + switch(sub_menu) { + case 0: + sprintf(bottomline,"ROBOT ID: %d",robot_id); + break; + case 1: + sprintf(bottomline,"SOFTWARE: %1.2f",SOFTWARE_VERSION_CODE); + break; + case 2: + if(firmware_version > 0) sprintf(bottomline,"FIRMWARE: %1.2f",firmware_version); + else sprintf(bottomline,"FIRMWARE: ?????"); + break; + case 3: + sprintf(bottomline,"PROG:%s",program_name); + break; + case 4: + sprintf(bottomline,"AUTH:%s",author_name); + break; + case 5: + sprintf(bottomline,"VER:%s",version_name); + break; + case 6: + sprintf(bottomline,"EXIT"); + break; + } + break; + case 5: + strcpy(topline,"---CODE DEMOS---"); + switch(sub_menu) { + case 0: + sprintf(bottomline,"LINE FOLLOW"); + break; + case 1: + sprintf(bottomline,"OBST. AVOID"); + break; + case 2: + sprintf(bottomline,"COLOUR SPIN"); + break; + case 3: + sprintf(bottomline,"STRESS TEST"); + break; + case 4: + sprintf(bottomline,"EXIT"); + break; + } + break; + case 6: + strcpy(topline,""); + break; + } + break; + } + display.write_string(topline); + display.set_position(1,0); + display.write_string(bottomline); + if(top_menu == 1 && level == 1) { + demo_event.attach(&demo_event_thread, 0.25); + } +} + +void start_line_demo() +{ + display.set_backlight_brightness(0); + time_out = 0.25f; + demo_timer.start(); + state = 0; + speed = 0; + led_step = 0; + demo_running = 1; + demo_timeout.attach_us(&line_demo_cycle,1000); +} + +void start_obstacle_demo() +{ + display.set_backlight_brightness(0); + time_out = 0.25f; + demo_timer.start(); + state = 0; + speed = 0; + led_step = 0; + demo_running = 1; + demo_timeout.attach_us(&obstacle_demo_cycle,1000); +} + +void start_stress_demo() +{ + display.set_backlight_brightness(0.25); + display.write_string("STRESS TEST"); + display.set_position(1,0); + display.write_string("----25%----"); + time_out = 0.04f; + demo_timer.start(); + state = 0; + speed = 0; + stress_step = 0; + spin_step = 0; + demo_running = 1; + demo_timeout.attach_us(&stress_demo_cycle,1000); +} + +void start_spinning_demo() +{ + display.set_backlight_brightness(0); + time_out = 0.0f; + demo_timer.start(); + state = 0; + speed = 0; + led_step = 0; + spin_step = 0; + demo_running = 1; + demo_timeout.attach_us(&spinning_demo_cycle,1000); +} + +void line_demo_cycle() +{ + if(demo_timer.read() > time_out) { + store_line_position(); + if(line_found) { + time_out = 0.01f; + state = 0; + // Get the position of the line. + lf_current_pos_of_line = line_position; + lf_proportional = lf_current_pos_of_line; + + // Compute the derivative + lf_derivative = lf_current_pos_of_line - lf_previous_pos_of_line; + + // Compute the integral + lf_integral += lf_proportional; + + // Remember the last position. + lf_previous_pos_of_line = lf_current_pos_of_line; + + // Compute the power + lf_power = (lf_proportional * (LF_P_TERM) ) + (lf_integral*(LF_I_TERM)) + (lf_derivative*(LF_D_TERM)) ; + + // Compute new speeds + lf_right = lf_speed-lf_power; + lf_left = lf_speed+lf_power; + + // limit checks + if (lf_right < 0) + lf_right = 0; + else if (lf_right > 1.0f) + lf_right = 1.0f; + + if (lf_left < 0) + 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{ + //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 + set_left_motor_speed(lf_left); + set_right_motor_speed(lf_right); + + + demo_timer.reset(); + } + if(demo_running == 1)demo_timeout.attach_us(&line_demo_cycle,500); + else { + stop(); + display.set_backlight_brightness(bl_brightness * 0.01f); + } +} + +void stress_demo_cycle() +{ + if(demo_timer.read() > time_out) { + float pct = 0.25 + (0.25 * stress_step); + switch(state) { + case 0: + if(spin_step % 2 == 0) { + forward(pct); + set_leds(0xFF,0xFF); + } else { + backward(pct); + set_leds(0,0xFF); + } + spin_step ++; + if(spin_step > 199) { + state ++; + spin_step = 0; + } + break; + case 1: + if(stress_step < 3) stress_step ++; + float pct = 0.25 + (0.25 * stress_step); + display.set_backlight_brightness(pct); + display.set_position(1,0); + switch(stress_step) { + case 1: + display.write_string("----50%----"); + break; + case 2: + display.write_string("----75%----"); + break; + case 3: + display.write_string("---100%----"); + break; + } + state = 0; + break; + } + demo_timer.reset(); + } + if(demo_running == 1)demo_timeout.attach_us(&stress_demo_cycle,500); + else { + stop(); + display.set_backlight_brightness(bl_brightness * 0.01f); + } +} + +void spinning_demo_cycle() +{ + if(demo_timer.read() > time_out) { + switch(state) { + case 0: //Robot is stopped + set_leds(0,0xFF); + set_center_led(1,1); + speed = 0.1f; + brake(); + time_out = 0.5; + state = 1; + led_step = 0; + break; + case 1: //Motor is turning right, accelerating + time_out = 0.1; + set_center_led(2,1); + switch(led_step) { + case 0: + set_leds(0x01,0); + break; + case 1: + set_leds(0x02,0); + break; + case 2: + set_leds(0x04,0); + break; + case 3: + set_leds(0x08,0); + break; + case 4: + set_leds(0x10,0); + break; + case 5: + set_leds(0x20,0); + break; + case 6: + set_leds(0x40,0); + break; + case 7: + set_leds(0x80,0); + break; + } + led_step ++; + if(led_step == 8) led_step =0; + if(speed < 1) { + speed += 0.0125; + turn(speed); + } else { + state = 2; + spin_step = 0; + led_step =0; + } + break; + case 2: //Motor is turning right, full speed + set_center_led(3,1); + switch(led_step) { + case 0: + set_leds(0x33,0x33); + break; + case 1: + set_leds(0x66,0x66); + break; + case 2: + set_leds(0xCC,0xCC); + break; + case 3: + set_leds(0x99,0x99); + break; + } + led_step ++; + if(led_step == 4) led_step = 0; + spin_step ++; + if(spin_step == 40) { + state = 3; + led_step = 0; + } + break; + case 3: //Motor is turning right, decelerating + set_center_led(2,1); + switch(led_step) { + case 0: + set_leds(0x01,0); + break; + case 1: + set_leds(0x02,0); + break; + case 2: + set_leds(0x04,0); + break; + case 3: + set_leds(0x08,0); + break; + case 4: + set_leds(0x10,0); + break; + case 5: + set_leds(0x20,0); + break; + case 6: + set_leds(0x40,0); + break; + case 7: + set_leds(0x80,0); + break; + } + if(led_step == 0) led_step =8; + led_step --; + if(speed > 0.1) { + speed -= 0.025; + turn(speed); + } else { + state = 4; + spin_step = 0; + led_step =0; + } + break; + case 4: //Robot is stopped + set_leds(0,0xFF); + set_center_led(1,1); + speed = 0.1f; + brake(); + time_out = 0.5; + led_step =0; + state = 5; + break; + case 5: //Motor is turning left, accelerating + time_out = 0.1; + set_center_led(2,1); + switch(led_step) { + case 0: + set_leds(0x01,0); + break; + case 1: + set_leds(0x02,0); + break; + case 2: + set_leds(0x04,0); + break; + case 3: + set_leds(0x08,0); + break; + case 4: + set_leds(0x10,0); + break; + case 5: + set_leds(0x20,0); + break; + case 6: + set_leds(0x40,0); + break; + case 7: + set_leds(0x80,0); + break; + } + if(led_step == 0) led_step =8; + led_step --; + if(speed < 1) { + speed += 0.0125; + turn(-speed); + } else { + state = 6; + spin_step = 0; + led_step = 0; + } + break; + case 6: //Motor is turning left, full speed + set_center_led(3,1); + switch(led_step) { + case 0: + set_leds(0x33,0x33); + break; + case 1: + set_leds(0x66,0x66); + break; + case 2: + set_leds(0xCC,0xCC); + break; + case 3: + set_leds(0x99,0x99); + break; + } + if(led_step == 0) led_step = 4; + led_step --; + spin_step ++; + if(spin_step == 40) { + state = 7; + led_step = 0; + } + break; + case 7: //Motor is turning left, decelerating + set_center_led(2,1); + switch(led_step) { + case 0: + set_leds(0x01,0); + break; + case 1: + set_leds(0x02,0); + break; + case 2: + set_leds(0x04,0); + break; + case 3: + set_leds(0x08,0); + break; + case 4: + set_leds(0x10,0); + break; + case 5: + set_leds(0x20,0); + break; + case 6: + set_leds(0x40,0); + break; + case 7: + set_leds(0x80,0); + break; + } + led_step ++; + if(led_step == 8) led_step =0; + if(speed > 0.1) { + speed -= 0.025; + turn(-speed); + } else { + state = 0; + spin_step = 0; + led_step = 0; + } + break; + } + demo_timer.reset(); + } + if(demo_running == 1)demo_timeout.attach_us(&spinning_demo_cycle,500); + else { + stop(); + display.set_backlight_brightness(bl_brightness * 0.01f); + } +} + +void obstacle_demo_cycle() +{ + if(demo_timer.read() > time_out) { + switch(state) { + case 0: //Robot is stopped + set_leds(0,0xFF); + set_center_led(1,0.4); + speed = 0.3f; + forward(speed); + time_out = 0.05; + state = 1; + break; + case 1: { //Motor is moving forward + store_ir_values(); + int front_right = read_illuminated_raw_ir_value(0); + int front_left = read_illuminated_raw_ir_value(7); + if(front_left > 400 || front_right > 400) { + brake(); + time_out = 0.04; + if(front_left > front_right)state=2; + else state=3; + } else { + if(speed < 0.5) { + speed += 0.03; + forward(speed); + } + switch(led_step) { + case 0: + set_leds(0x01,0); + break; + case 1: + set_leds(0x38,0); + break; + case 2: + set_leds(0x6C,0); + break; + case 3: + set_leds(0xC6,0); + break; + case 4: + set_leds(0x83,0); + break; + } + set_center_led(2, 0.6); + led_step ++; + if(led_step == 5) led_step = 0; + } + break; + } + case 2: //Turn right + set_left_motor_speed(0.85); + set_right_motor_speed(-0.85); + time_out = 0.4; + state = 0; + set_leds(0x0E,0x0E); + set_center_led(3,0.5); + break; + case 3: //Turn left + set_left_motor_speed(-0.85); + set_right_motor_speed(0.85); + time_out = 0.4; + state = 0; + set_leds(0xE0,0xE0); + set_center_led(3,0.5); + break; + } + demo_timer.reset(); + } + if(demo_running == 1)demo_timeout.attach_us(&obstacle_demo_cycle,200); + else { + stop(); + display.set_backlight_brightness(bl_brightness * 0.01f); + } +} + +void demo_update_leds() +{ + char red = 0; + char green = 0; + for(int i=0; i<8; i++) { + if(led_state[i]==1 || led_state[i]==3) red+=(1<<i); + if(led_state[i]==2 || led_state[i]==3) green+=(1<<i); + } + set_leds(green,red); + float brightness_f = brightness / 100.0f; + set_center_led(led_state[8], brightness_f); + set_base_led(base_led_state); +} \ No newline at end of file