PsiSwarmV9-Fork - Psi Swarm robot library version 0.9

Users » jah128 » Code » PsiSwarmV9-Fork

Psi Swarm robot library version 0.9

Fork of PsiSwarmV9 by Psi Swarm Robot

demo.cpp@20:1bc6c6dc477b, 2018-05-14 (annotated)

Committer:: jah128
Date:: Mon May 14 15:35:38 2018 +0000
Revision:: 20:1bc6c6dc477b
Parent:: 19:3e3b03d80ea3

Updated?

Who changed what in which revision?

User	Revision	Line number	New contents of line
jah128	0:d6269d17c8cf	1	/* University of York Robotics Laboratory PsiSwarm Library: Demo Mode Source File
jah128	15:66be5ec52c3b	2	*
jah128	14:2f1ad77d281e	3	* Copyright 2017 University of York
jah128	6:b340a527add9	4	*
jah128	15:66be5ec52c3b	5	* Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
jah128	6:b340a527add9	6	* You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
jah128	6:b340a527add9	7	* Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
jah128	15:66be5ec52c3b	8	* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
jah128	6:b340a527add9	9	* See the License for the specific language governing permissions and limitations under the License.
jah128	0:d6269d17c8cf	10	*
jah128	0:d6269d17c8cf	11	* File: demo.cpp
jah128	0:d6269d17c8cf	12	*
jah128	0:d6269d17c8cf	13	* (C) Dept. Electronics & Computer Science, University of York
jah128	0:d6269d17c8cf	14	* James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
jah128	0:d6269d17c8cf	15	*
jah128	14:2f1ad77d281e	16	* PsiSwarm Library Version: 0.9
jah128	0:d6269d17c8cf	17	*
jah128	16:50686c07ad07	18	* Version 0.9 : Added colour sensor functions, colour sensor demo and self-test mode
jah128	8:6c92789d5f87	19	* Version 0.8 : Rewritten as OO\C++
jah128	5:3cdd1a37cdd7	20	* Version 0.7 : Updated for new MBED API
jah128	4:1c621cb8cf0d	21	* Version 0.5 : Added motor calibration menu
jah128	0:d6269d17c8cf	22	* Version 0.4 : Added PsiSwarmBasic menu
jah128	0:d6269d17c8cf	23	* Version 0.2 : Remove most the functionality from center-button push to allow all operations to be accessable from
jah128	0:d6269d17c8cf	24	* four directions alone.
jah128	0:d6269d17c8cf	25	* Added extra sensor information, added various testing demos
jah128	0:d6269d17c8cf	26	*
jah128	2:c6986ee3c7c5	27	*
jah128	14:2f1ad77d281e	28	* June 2017
jah128	0:d6269d17c8cf	29	*
jah128	0:d6269d17c8cf	30	*/
jah128	0:d6269d17c8cf	31
jah128	0:d6269d17c8cf	32
jah128	0:d6269d17c8cf	33	#include "psiswarm.h"
jah128	0:d6269d17c8cf	34
jah128	0:d6269d17c8cf	35	// PID terms
jah128	0:d6269d17c8cf	36	#define LF_P_TERM 0.2
jah128	0:d6269d17c8cf	37	#define LF_I_TERM 0
jah128	0:d6269d17c8cf	38	#define LF_D_TERM 4
jah128	0:d6269d17c8cf	39
jah128	15:66be5ec52c3b	40	char quick_test = 0;
jah128	0:d6269d17c8cf	41	char top_menu = 0;
jah128	0:d6269d17c8cf	42	char sub_menu = 0;
jah128	0:d6269d17c8cf	43	char level = 0;
jah128	0:d6269d17c8cf	44	char started = 0;
jah128	0:d6269d17c8cf	45	char topline[17];
jah128	0:d6269d17c8cf	46	char bottomline[17];
jah128	0:d6269d17c8cf	47	char led_state[9];
jah128	0:d6269d17c8cf	48	char all_led_state = 0;
jah128	0:d6269d17c8cf	49	char base_led_state = 0;
jah128	0:d6269d17c8cf	50	char brightness = 20;
jah128	0:d6269d17c8cf	51	char bl_brightness = 100;
jah128	0:d6269d17c8cf	52	char base_ir_index = 0;
jah128	0:d6269d17c8cf	53	char side_ir_index = 0;
jah128	0:d6269d17c8cf	54	signed short left_speed = 0;
jah128	0:d6269d17c8cf	55	signed short right_speed = 0;
jah128	0:d6269d17c8cf	56	char both_motor_mode = 0;
jah128	0:d6269d17c8cf	57	char last_switch_pressed;
jah128	0:d6269d17c8cf	58	Timeout demo_event;
jah128	0:d6269d17c8cf	59	char handling_event = 0;
jah128	0:d6269d17c8cf	60
jah128	0:d6269d17c8cf	61	Timeout demo_timeout;
jah128	0:d6269d17c8cf	62	char demo_running = 0;
jah128	0:d6269d17c8cf	63	Timer demo_timer;
jah128	0:d6269d17c8cf	64	float time_out;
jah128	0:d6269d17c8cf	65	float speed;
jah128	0:d6269d17c8cf	66	char state;
jah128	0:d6269d17c8cf	67	char led_step = 0;
jah128	0:d6269d17c8cf	68	char spin_step = 0;
jah128	0:d6269d17c8cf	69	char stress_step = 0;
jah128	0:d6269d17c8cf	70
jah128	0:d6269d17c8cf	71
jah128	0:d6269d17c8cf	72	float lf_right;
jah128	0:d6269d17c8cf	73	float lf_left;
jah128	0:d6269d17c8cf	74	float lf_current_pos_of_line = 0.0;
jah128	0:d6269d17c8cf	75	float lf_previous_pos_of_line = 0.0;
jah128	0:d6269d17c8cf	76	float lf_derivative,lf_proportional,lf_integral = 0;
jah128	0:d6269d17c8cf	77	float lf_power;
jah128	0:d6269d17c8cf	78	float lf_speed = 0.4;
jah128	0:d6269d17c8cf	79
jah128	0:d6269d17c8cf	80
jah128	0:d6269d17c8cf	81
jah128	11:312663037b8c	82	void Demo::start_demo_mode()
jah128	0:d6269d17c8cf	83	{
jah128	12:878c6e9d9e60	84	psi.debug("- Starting Demo Mode\n");
jah128	17:bf614e28668f	85	if(use_flash_basic == 1) top_menu = 8;
jah128	17:bf614e28668f	86	if((has_base_ir == 1 && base_ir_calibration_set != 1) \|\| (has_base_colour_sensor == 1 && base_colour_calibration_set != 1)) top_menu = 7;
jah128	0:d6269d17c8cf	87	demo_on = 1;
jah128	0:d6269d17c8cf	88	display.set_backlight_brightness(bl_brightness * 0.01f);
jah128	0:d6269d17c8cf	89	display.clear_display();
jah128	0:d6269d17c8cf	90	display.write_string("PSI SWARM SYSTEM");
jah128	0:d6269d17c8cf	91	display.set_position(1,0);
jah128	0:d6269d17c8cf	92	display.write_string(" DEMO MODE");
jah128	0:d6269d17c8cf	93	wait(0.5);
jah128	0:d6269d17c8cf	94	display.clear_display();
jah128	0:d6269d17c8cf	95	display.write_string("Use cursor to");
jah128	0:d6269d17c8cf	96	display.set_position(1,0);
jah128	0:d6269d17c8cf	97	display.write_string("navigate menus");
jah128	0:d6269d17c8cf	98	char step = 0;
jah128	0:d6269d17c8cf	99	while(demo_on) {
jah128	0:d6269d17c8cf	100	if(demo_running == 0) {
jah128	0:d6269d17c8cf	101	switch(step) {
jah128	0:d6269d17c8cf	102	case 0:
jah128	0:d6269d17c8cf	103	mbed_led1 = 1;
jah128	0:d6269d17c8cf	104	mbed_led2 = 0;
jah128	0:d6269d17c8cf	105	break;
jah128	0:d6269d17c8cf	106	case 1:
jah128	0:d6269d17c8cf	107	mbed_led2 = 1;
jah128	0:d6269d17c8cf	108	mbed_led1 = 0;
jah128	0:d6269d17c8cf	109	break;
jah128	0:d6269d17c8cf	110	}
jah128	0:d6269d17c8cf	111	step++;
jah128	0:d6269d17c8cf	112	if(step==2)step=0;
jah128	0:d6269d17c8cf	113	} else {
jah128	0:d6269d17c8cf	114	mbed_led1 = 0;
jah128	0:d6269d17c8cf	115	mbed_led2 = 0;
jah128	0:d6269d17c8cf	116	mbed_led3 = 0;
jah128	0:d6269d17c8cf	117	mbed_led4 = 0;
jah128	0:d6269d17c8cf	118	}
jah128	0:d6269d17c8cf	119	wait(0.5);
jah128	0:d6269d17c8cf	120	}
jah128	12:878c6e9d9e60	121	psi.debug("- Demo mode ended\n");
jah128	0:d6269d17c8cf	122	}
jah128	0:d6269d17c8cf	123
jah128	11:312663037b8c	124	void Demo::demo_handle_switch_event(char switch_pressed)
jah128	0:d6269d17c8cf	125	{
jah128	0:d6269d17c8cf	126	if(!handling_event) {
jah128	0:d6269d17c8cf	127	handling_event = 1;
jah128	0:d6269d17c8cf	128	last_switch_pressed = switch_pressed;
jah128	11:312663037b8c	129	demo_event.attach_us(this,&Demo::demo_event_thread, 1000);
jah128	0:d6269d17c8cf	130	}
jah128	0:d6269d17c8cf	131	}
jah128	0:d6269d17c8cf	132
jah128	11:312663037b8c	133	void Demo::demo_event_thread()
jah128	0:d6269d17c8cf	134	{
jah128	0:d6269d17c8cf	135	handling_event = 0;
jah128	0:d6269d17c8cf	136	if(started == 1) {
jah128	0:d6269d17c8cf	137	switch(last_switch_pressed) {
jah128	0:d6269d17c8cf	138	case 1: //Up pressed
jah128	0:d6269d17c8cf	139	switch(level) {
jah128	0:d6269d17c8cf	140	case 0:
jah128	15:66be5ec52c3b	141	if(top_menu != 9 && top_menu!= 0) {
jah128	0:d6269d17c8cf	142	level++;
jah128	0:d6269d17c8cf	143	sub_menu = 0;
jah128	0:d6269d17c8cf	144	} else {
jah128	15:66be5ec52c3b	145	if(top_menu == 0) {
jah128	15:66be5ec52c3b	146	toggle_quick_test();
jah128	15:66be5ec52c3b	147	} else {
jah128	15:66be5ec52c3b	148	demo_on = 0;
jah128	15:66be5ec52c3b	149	user_code_running = user_code_restore_mode;
jah128	15:66be5ec52c3b	150	}
jah128	0:d6269d17c8cf	151	}
jah128	0:d6269d17c8cf	152	break;
jah128	0:d6269d17c8cf	153	case 1:
jah128	0:d6269d17c8cf	154	switch(top_menu) {
jah128	15:66be5ec52c3b	155	case 8: // PsiBasic Menu
jah128	0:d6269d17c8cf	156	if(sub_menu == psi_basic_file_count) level = 0;
jah128	0:d6269d17c8cf	157	break;
jah128	17:bf614e28668f	158	case 1: //LED Menu
jah128	0:d6269d17c8cf	159	if(sub_menu < 9) {
jah128	0:d6269d17c8cf	160	if(led_state[sub_menu] == 0) led_state[sub_menu] = 3;
jah128	0:d6269d17c8cf	161	else led_state[sub_menu]--;
jah128	0:d6269d17c8cf	162	demo_update_leds();
jah128	0:d6269d17c8cf	163	}
jah128	0:d6269d17c8cf	164	if(sub_menu == 9) {
jah128	0:d6269d17c8cf	165	if(all_led_state == 0) all_led_state = 3;
jah128	0:d6269d17c8cf	166	else all_led_state--;
jah128	0:d6269d17c8cf	167	for(int i=0; i<9; i++) {
jah128	0:d6269d17c8cf	168	led_state[i]=all_led_state;
jah128	0:d6269d17c8cf	169	}
jah128	0:d6269d17c8cf	170	demo_update_leds();
jah128	0:d6269d17c8cf	171	}
jah128	0:d6269d17c8cf	172	if(sub_menu == 10) {
jah128	0:d6269d17c8cf	173	base_led_state = 1 - base_led_state;
jah128	0:d6269d17c8cf	174	demo_update_leds();
jah128	0:d6269d17c8cf	175	}
jah128	0:d6269d17c8cf	176	if(sub_menu == 11) {
jah128	0:d6269d17c8cf	177	switch(brightness) {
jah128	0:d6269d17c8cf	178	case 100:
jah128	0:d6269d17c8cf	179	brightness = 50;
jah128	0:d6269d17c8cf	180	break;
jah128	0:d6269d17c8cf	181	case 2:
jah128	0:d6269d17c8cf	182	brightness = 1;
jah128	0:d6269d17c8cf	183	break;
jah128	0:d6269d17c8cf	184	case 5:
jah128	0:d6269d17c8cf	185	brightness = 2;
jah128	0:d6269d17c8cf	186	break;
jah128	0:d6269d17c8cf	187	case 10:
jah128	0:d6269d17c8cf	188	brightness = 5;
jah128	0:d6269d17c8cf	189	break;
jah128	0:d6269d17c8cf	190	case 20:
jah128	0:d6269d17c8cf	191	brightness = 10;
jah128	0:d6269d17c8cf	192	break;
jah128	0:d6269d17c8cf	193	case 50:
jah128	0:d6269d17c8cf	194	brightness = 20;
jah128	0:d6269d17c8cf	195	break;
jah128	0:d6269d17c8cf	196	}
jah128	0:d6269d17c8cf	197	demo_update_leds();
jah128	0:d6269d17c8cf	198	}
jah128	0:d6269d17c8cf	199	if(sub_menu == 12) {
jah128	0:d6269d17c8cf	200	if(bl_brightness > 0) bl_brightness-=10;
jah128	0:d6269d17c8cf	201	display.set_backlight_brightness(bl_brightness * 0.01f);
jah128	0:d6269d17c8cf	202	}
jah128	0:d6269d17c8cf	203	if(sub_menu == 13) level = 0;
jah128	0:d6269d17c8cf	204	break;
jah128	15:66be5ec52c3b	205	case 2: // Sensors Menu
jah128	17:bf614e28668f	206	if(sub_menu == 4 \|\| sub_menu == 5 \|\| sub_menu==6) {
jah128	0:d6269d17c8cf	207	if(base_ir_index == 0) base_ir_index = 4;
jah128	0:d6269d17c8cf	208	else base_ir_index --;
jah128	0:d6269d17c8cf	209	}
jah128	17:bf614e28668f	210	if(sub_menu > 6 && sub_menu < 10) {
jah128	0:d6269d17c8cf	211	if(side_ir_index == 0) side_ir_index = 7;
jah128	0:d6269d17c8cf	212	else side_ir_index --;
jah128	0:d6269d17c8cf	213	}
jah128	17:bf614e28668f	214	if(sub_menu == 13) level = 0;
jah128	0:d6269d17c8cf	215	break;
jah128	15:66be5ec52c3b	216	case 3: // Motor Menu
jah128	0:d6269d17c8cf	217	if(sub_menu == 0) {
jah128	0:d6269d17c8cf	218	left_speed += 5;
jah128	0:d6269d17c8cf	219	if(left_speed > 100) left_speed = 100;
jah128	8:6c92789d5f87	220	motors.set_left_motor_speed(left_speed / 100.0f);
jah128	0:d6269d17c8cf	221	}
jah128	0:d6269d17c8cf	222	if(sub_menu == 1) {
jah128	0:d6269d17c8cf	223	right_speed += 5;
jah128	0:d6269d17c8cf	224	if(right_speed > 100) right_speed = 100;
jah128	8:6c92789d5f87	225	motors.set_right_motor_speed(right_speed / 100.0f);
jah128	0:d6269d17c8cf	226	}
jah128	0:d6269d17c8cf	227	if(sub_menu == 2) {
jah128	0:d6269d17c8cf	228	if(both_motor_mode == 0) both_motor_mode=5;
jah128	0:d6269d17c8cf	229	else both_motor_mode--;
jah128	0:d6269d17c8cf	230	switch(both_motor_mode) {
jah128	0:d6269d17c8cf	231	case 0:
jah128	8:6c92789d5f87	232	motors.stop();
jah128	0:d6269d17c8cf	233	break;
jah128	0:d6269d17c8cf	234	case 1:
jah128	8:6c92789d5f87	235	motors.brake();
jah128	0:d6269d17c8cf	236	break;
jah128	0:d6269d17c8cf	237	case 2:
jah128	8:6c92789d5f87	238	motors.forward(0.5);
jah128	0:d6269d17c8cf	239	break;
jah128	0:d6269d17c8cf	240	case 3:
jah128	8:6c92789d5f87	241	motors.forward(1);
jah128	0:d6269d17c8cf	242	break;
jah128	0:d6269d17c8cf	243	case 4:
jah128	8:6c92789d5f87	244	motors.backward(0.5);
jah128	0:d6269d17c8cf	245	break;
jah128	0:d6269d17c8cf	246	case 5:
jah128	8:6c92789d5f87	247	motors.backward(1.0);
jah128	0:d6269d17c8cf	248	break;
jah128	0:d6269d17c8cf	249	}
jah128	0:d6269d17c8cf	250	}
jah128	0:d6269d17c8cf	251	if(sub_menu == 3) {
jah128	0:d6269d17c8cf	252	level = 0;
jah128	0:d6269d17c8cf	253	}
jah128	0:d6269d17c8cf	254	break;
jah128	15:66be5ec52c3b	255	case 4: // Radio Menu
jah128	0:d6269d17c8cf	256	if(sub_menu == 0) level = 0;
jah128	0:d6269d17c8cf	257	break;
jah128	15:66be5ec52c3b	258	case 5: // Info Menu
jah128	0:d6269d17c8cf	259	if(sub_menu == 6) level = 0;
jah128	0:d6269d17c8cf	260	break;
jah128	17:bf614e28668f	261	case 7: // Calibration Menu
jah128	17:bf614e28668f	262	if(sub_menu == 0) sensors.calibrate_base_sensors();
jah128	18:9204f74069b4	263	if(sub_menu == 1) motors.calibrate_motors();
jah128	17:bf614e28668f	264	if(sub_menu == 2) level = 0;
jah128	17:bf614e28668f	265	break;
jah128	15:66be5ec52c3b	266	case 6: // Demo Menu
jah128	0:d6269d17c8cf	267	if(sub_menu == 0) {
jah128	0:d6269d17c8cf	268	if(demo_running == 0) {
jah128	0:d6269d17c8cf	269	start_line_demo();
jah128	0:d6269d17c8cf	270	} else {
jah128	0:d6269d17c8cf	271	demo_running = 0;
jah128	0:d6269d17c8cf	272	display.set_backlight_brightness(bl_brightness * 0.01f);
jah128	8:6c92789d5f87	273	motors.stop();
jah128	0:d6269d17c8cf	274	}
jah128	0:d6269d17c8cf	275	}
jah128	0:d6269d17c8cf	276	if(sub_menu == 1) {
jah128	0:d6269d17c8cf	277	if(demo_running == 0) {
jah128	0:d6269d17c8cf	278	start_obstacle_demo();
jah128	0:d6269d17c8cf	279	} else {
jah128	0:d6269d17c8cf	280	demo_running = 0;
jah128	0:d6269d17c8cf	281	display.set_backlight_brightness(bl_brightness * 0.01f);
jah128	8:6c92789d5f87	282	motors.stop();
jah128	0:d6269d17c8cf	283	}
jah128	0:d6269d17c8cf	284	}
jah128	0:d6269d17c8cf	285	if(sub_menu == 2) {
jah128	0:d6269d17c8cf	286	if(demo_running == 0) {
jah128	0:d6269d17c8cf	287	start_spinning_demo();
jah128	0:d6269d17c8cf	288	} else {
jah128	0:d6269d17c8cf	289	demo_running = 0;
jah128	0:d6269d17c8cf	290	display.set_backlight_brightness(bl_brightness * 0.01f);
jah128	8:6c92789d5f87	291	motors.stop();
jah128	0:d6269d17c8cf	292	}
jah128	0:d6269d17c8cf	293	}
jah128	0:d6269d17c8cf	294	if(sub_menu == 3) {
jah128	0:d6269d17c8cf	295	if(demo_running == 0) {
jah128	0:d6269d17c8cf	296	start_stress_demo();
jah128	0:d6269d17c8cf	297	} else {
jah128	0:d6269d17c8cf	298	demo_running = 0;
jah128	0:d6269d17c8cf	299	display.set_backlight_brightness(bl_brightness * 0.01f);
jah128	8:6c92789d5f87	300	motors.stop();
jah128	0:d6269d17c8cf	301	}
jah128	0:d6269d17c8cf	302	}
jah128	16:50686c07ad07	303	if(sub_menu == 4) {
jah128	16:50686c07ad07	304	if(demo_running == 0) {
jah128	16:50686c07ad07	305	start_colour_demo();
jah128	16:50686c07ad07	306	} else {
jah128	16:50686c07ad07	307	demo_running = 0;
jah128	16:50686c07ad07	308	display.set_backlight_brightness(bl_brightness * 0.01f);
jah128	16:50686c07ad07	309	motors.stop();
jah128	16:50686c07ad07	310	}
jah128	16:50686c07ad07	311	}
jah128	16:50686c07ad07	312	if(sub_menu == 5) level = 0;
jah128	0:d6269d17c8cf	313	break;
jah128	0:d6269d17c8cf	314	}
jah128	0:d6269d17c8cf	315	break;
jah128	0:d6269d17c8cf	316	}
jah128	0:d6269d17c8cf	317	break;
jah128	0:d6269d17c8cf	318	case 2: //Down pressed
jah128	0:d6269d17c8cf	319	switch(level) {
jah128	0:d6269d17c8cf	320	case 0:
jah128	15:66be5ec52c3b	321	if(top_menu != 9 && top_menu != 0) {
jah128	0:d6269d17c8cf	322	level++;
jah128	0:d6269d17c8cf	323	sub_menu = 0;
jah128	0:d6269d17c8cf	324	} else {
jah128	15:66be5ec52c3b	325	if(top_menu == 0) {
jah128	15:66be5ec52c3b	326	toggle_quick_test();
jah128	15:66be5ec52c3b	327	} else {
jah128	15:66be5ec52c3b	328	demo_on = 0;
jah128	15:66be5ec52c3b	329	user_code_running = user_code_restore_mode;
jah128	15:66be5ec52c3b	330	}
jah128	0:d6269d17c8cf	331	}
jah128	0:d6269d17c8cf	332	break;
jah128	0:d6269d17c8cf	333	case 1:
jah128	0:d6269d17c8cf	334	switch(top_menu) {
jah128	15:66be5ec52c3b	335	case 8: // PsiBasic Menu
jah128	0:d6269d17c8cf	336	if(sub_menu == psi_basic_file_count) level = 0;
jah128	0:d6269d17c8cf	337	break;
jah128	15:66be5ec52c3b	338	case 1: //LED Menu
jah128	0:d6269d17c8cf	339	if(sub_menu < 9) {
jah128	0:d6269d17c8cf	340	led_state[sub_menu]++;
jah128	0:d6269d17c8cf	341	if(led_state[sub_menu] == 4) led_state[sub_menu] = 0;
jah128	0:d6269d17c8cf	342	demo_update_leds();
jah128	0:d6269d17c8cf	343	}
jah128	0:d6269d17c8cf	344	if(sub_menu == 9) {
jah128	0:d6269d17c8cf	345	all_led_state++;
jah128	0:d6269d17c8cf	346	if(all_led_state == 4) all_led_state = 0;
jah128	0:d6269d17c8cf	347	for(int i=0; i<9; i++) {
jah128	0:d6269d17c8cf	348	led_state[i]=all_led_state;
jah128	0:d6269d17c8cf	349	}
jah128	0:d6269d17c8cf	350	demo_update_leds();
jah128	0:d6269d17c8cf	351	}
jah128	0:d6269d17c8cf	352	if(sub_menu == 10) {
jah128	0:d6269d17c8cf	353	base_led_state = 1 - base_led_state;
jah128	0:d6269d17c8cf	354	demo_update_leds();
jah128	0:d6269d17c8cf	355	}
jah128	0:d6269d17c8cf	356	if(sub_menu == 11) {
jah128	0:d6269d17c8cf	357	switch(brightness) {
jah128	0:d6269d17c8cf	358	case 1:
jah128	0:d6269d17c8cf	359	brightness = 2;
jah128	0:d6269d17c8cf	360	break;
jah128	0:d6269d17c8cf	361	case 2:
jah128	0:d6269d17c8cf	362	brightness = 5;
jah128	0:d6269d17c8cf	363	break;
jah128	0:d6269d17c8cf	364	case 5:
jah128	0:d6269d17c8cf	365	brightness = 10;
jah128	0:d6269d17c8cf	366	break;
jah128	0:d6269d17c8cf	367	case 10:
jah128	0:d6269d17c8cf	368	brightness = 20;
jah128	0:d6269d17c8cf	369	break;
jah128	0:d6269d17c8cf	370	case 20:
jah128	0:d6269d17c8cf	371	brightness = 50;
jah128	0:d6269d17c8cf	372	break;
jah128	0:d6269d17c8cf	373	case 50:
jah128	0:d6269d17c8cf	374	brightness = 100;
jah128	0:d6269d17c8cf	375	break;
jah128	0:d6269d17c8cf	376	}
jah128	0:d6269d17c8cf	377	demo_update_leds();
jah128	0:d6269d17c8cf	378	}
jah128	0:d6269d17c8cf	379
jah128	0:d6269d17c8cf	380	if(sub_menu == 12) {
jah128	0:d6269d17c8cf	381	if(bl_brightness < 100) bl_brightness+=10;
jah128	0:d6269d17c8cf	382	display.set_backlight_brightness(bl_brightness * 0.01f);
jah128	0:d6269d17c8cf	383	}
jah128	0:d6269d17c8cf	384	if(sub_menu == 13) level = 0;
jah128	0:d6269d17c8cf	385
jah128	0:d6269d17c8cf	386	break;
jah128	15:66be5ec52c3b	387	case 2: // Sensors Menu
jah128	17:bf614e28668f	388	if(sub_menu == 4 \|\| sub_menu == 5 \|\| sub_menu == 6) {
jah128	0:d6269d17c8cf	389	if(base_ir_index == 4) base_ir_index = 0;
jah128	0:d6269d17c8cf	390	else base_ir_index ++;
jah128	0:d6269d17c8cf	391	}
jah128	17:bf614e28668f	392	if(sub_menu > 6 && sub_menu < 10) {
jah128	0:d6269d17c8cf	393	if(side_ir_index == 7) side_ir_index = 0;
jah128	0:d6269d17c8cf	394	else side_ir_index ++;
jah128	0:d6269d17c8cf	395	}
jah128	17:bf614e28668f	396	if(sub_menu == 13) level = 0;
jah128	0:d6269d17c8cf	397	break;
jah128	15:66be5ec52c3b	398	case 3: // Motor Menu
jah128	0:d6269d17c8cf	399	if(sub_menu == 0) {
jah128	0:d6269d17c8cf	400	left_speed -= 5;
jah128	0:d6269d17c8cf	401	if(left_speed < -100) left_speed = -100;
jah128	8:6c92789d5f87	402	motors.set_left_motor_speed(left_speed / 100.0f);
jah128	0:d6269d17c8cf	403	}
jah128	0:d6269d17c8cf	404	if(sub_menu == 1) {
jah128	0:d6269d17c8cf	405	right_speed -= 5;
jah128	0:d6269d17c8cf	406	if(right_speed < -100) right_speed = -100;
jah128	8:6c92789d5f87	407	motors.set_right_motor_speed(right_speed / 100.0f);
jah128	0:d6269d17c8cf	408	}
jah128	0:d6269d17c8cf	409	if(sub_menu == 2) {
jah128	0:d6269d17c8cf	410	both_motor_mode++;
jah128	0:d6269d17c8cf	411	if(both_motor_mode == 6) both_motor_mode=0;
jah128	0:d6269d17c8cf	412	switch(both_motor_mode) {
jah128	0:d6269d17c8cf	413	case 0:
jah128	8:6c92789d5f87	414	motors.stop();
jah128	0:d6269d17c8cf	415	break;
jah128	0:d6269d17c8cf	416	case 1:
jah128	8:6c92789d5f87	417	motors.brake();
jah128	0:d6269d17c8cf	418	break;
jah128	0:d6269d17c8cf	419	case 2:
jah128	8:6c92789d5f87	420	motors.forward(0.5);
jah128	0:d6269d17c8cf	421	break;
jah128	0:d6269d17c8cf	422	case 3:
jah128	8:6c92789d5f87	423	motors.forward(1);
jah128	0:d6269d17c8cf	424	break;
jah128	0:d6269d17c8cf	425	case 4:
jah128	8:6c92789d5f87	426	motors.backward(0.5);
jah128	0:d6269d17c8cf	427	break;
jah128	0:d6269d17c8cf	428	case 5:
jah128	8:6c92789d5f87	429	motors.backward(1.0);
jah128	0:d6269d17c8cf	430	break;
jah128	0:d6269d17c8cf	431	}
jah128	0:d6269d17c8cf	432	}
jah128	0:d6269d17c8cf	433	if(sub_menu == 3) {
jah128	0:d6269d17c8cf	434	level = 0;
jah128	0:d6269d17c8cf	435	}
jah128	0:d6269d17c8cf	436	break;
jah128	15:66be5ec52c3b	437	case 4: // Radio Menu
jah128	0:d6269d17c8cf	438	if(sub_menu == 0) level = 0;
jah128	0:d6269d17c8cf	439	break;
jah128	15:66be5ec52c3b	440	case 5: // Info Menu
jah128	0:d6269d17c8cf	441	if(sub_menu == 6) level = 0;
jah128	0:d6269d17c8cf	442	break;
jah128	17:bf614e28668f	443	case 7: // Calibration Menu
jah128	17:bf614e28668f	444	if(sub_menu == 0) sensors.calibrate_base_sensors();
jah128	18:9204f74069b4	445	if(sub_menu == 1) motors.calibrate_motors();
jah128	17:bf614e28668f	446	if(sub_menu == 2) level = 0;
jah128	18:9204f74069b4	447	break;
jah128	15:66be5ec52c3b	448	case 6: // Demo Menu
jah128	0:d6269d17c8cf	449	if(sub_menu == 0) {
jah128	0:d6269d17c8cf	450	if(demo_running == 0) {
jah128	0:d6269d17c8cf	451	start_line_demo();
jah128	0:d6269d17c8cf	452	} else {
jah128	0:d6269d17c8cf	453	demo_running = 0;
jah128	0:d6269d17c8cf	454	display.set_backlight_brightness(bl_brightness * 0.01f);
jah128	8:6c92789d5f87	455	motors.stop();
jah128	0:d6269d17c8cf	456	}
jah128	0:d6269d17c8cf	457	}
jah128	0:d6269d17c8cf	458	if(sub_menu == 1) {
jah128	0:d6269d17c8cf	459	if(demo_running == 0) {
jah128	0:d6269d17c8cf	460	start_obstacle_demo();
jah128	0:d6269d17c8cf	461	} else {
jah128	0:d6269d17c8cf	462	demo_running = 0;
jah128	0:d6269d17c8cf	463	display.set_backlight_brightness(bl_brightness * 0.01f);
jah128	8:6c92789d5f87	464	motors.stop();
jah128	0:d6269d17c8cf	465
jah128	0:d6269d17c8cf	466	}
jah128	0:d6269d17c8cf	467	}
jah128	0:d6269d17c8cf	468	if(sub_menu == 2) {
jah128	0:d6269d17c8cf	469	if(demo_running == 0) {
jah128	0:d6269d17c8cf	470	start_spinning_demo();
jah128	0:d6269d17c8cf	471	} else {
jah128	0:d6269d17c8cf	472	demo_running = 0;
jah128	0:d6269d17c8cf	473	display.set_backlight_brightness(bl_brightness * 0.01f);
jah128	8:6c92789d5f87	474	motors.stop();
jah128	0:d6269d17c8cf	475	}
jah128	0:d6269d17c8cf	476	}
jah128	0:d6269d17c8cf	477	if(sub_menu == 3) {
jah128	0:d6269d17c8cf	478	if(demo_running == 0) {
jah128	0:d6269d17c8cf	479	start_stress_demo();
jah128	0:d6269d17c8cf	480	} else {
jah128	0:d6269d17c8cf	481	demo_running = 0;
jah128	0:d6269d17c8cf	482	display.set_backlight_brightness(bl_brightness * 0.01f);
jah128	8:6c92789d5f87	483	motors.stop();
jah128	0:d6269d17c8cf	484	}
jah128	0:d6269d17c8cf	485	}
jah128	16:50686c07ad07	486	if(sub_menu == 4) {
jah128	16:50686c07ad07	487	if(demo_running == 0) {
jah128	16:50686c07ad07	488	start_colour_demo();
jah128	16:50686c07ad07	489	} else {
jah128	16:50686c07ad07	490	demo_running = 0;
jah128	16:50686c07ad07	491	display.set_backlight_brightness(bl_brightness * 0.01f);
jah128	16:50686c07ad07	492	motors.stop();
jah128	16:50686c07ad07	493	}
jah128	16:50686c07ad07	494	}
jah128	16:50686c07ad07	495	if(sub_menu == 5) level = 0;
jah128	0:d6269d17c8cf	496	break;
jah128	0:d6269d17c8cf	497	}
jah128	0:d6269d17c8cf	498	break;
jah128	0:d6269d17c8cf	499	}
jah128	0:d6269d17c8cf	500	break;
jah128	0:d6269d17c8cf	501	case 4: //Left pressed
jah128	0:d6269d17c8cf	502	switch(level) {
jah128	0:d6269d17c8cf	503	case 0:
jah128	20:1bc6c6dc477b	504	stop_demo();
jah128	0:d6269d17c8cf	505	if(top_menu == 0) {
jah128	15:66be5ec52c3b	506	top_menu = 9;
jah128	15:66be5ec52c3b	507	} else {
jah128	17:bf614e28668f	508	top_menu --;
jah128	15:66be5ec52c3b	509	if(use_flash_basic == 0 && top_menu == 8) top_menu = 7;
jah128	19:3e3b03d80ea3	510	if(has_433_radio == 0 && top_menu == 4) top_menu = 3;
jah128	4:1c621cb8cf0d	511	}
jah128	0:d6269d17c8cf	512	break;
jah128	0:d6269d17c8cf	513	case 1:
jah128	0:d6269d17c8cf	514	switch(top_menu) {
jah128	15:66be5ec52c3b	515	case 1: //LED Menu
jah128	0:d6269d17c8cf	516	if(sub_menu == 0) sub_menu = 13;
jah128	0:d6269d17c8cf	517	else sub_menu --;
jah128	0:d6269d17c8cf	518	break;
jah128	15:66be5ec52c3b	519	case 2: //Sensors Menu
jah128	17:bf614e28668f	520	if(sub_menu == 0) sub_menu = 13;
jah128	0:d6269d17c8cf	521	else sub_menu --;
jah128	0:d6269d17c8cf	522	break;
jah128	0:d6269d17c8cf	523
jah128	15:66be5ec52c3b	524	case 3: //Motor Menu
jah128	0:d6269d17c8cf	525	if(sub_menu == 0) sub_menu = 3;
jah128	0:d6269d17c8cf	526	else sub_menu --;
jah128	0:d6269d17c8cf	527	break;
jah128	15:66be5ec52c3b	528	case 5: //Info Menu
jah128	0:d6269d17c8cf	529	if(sub_menu == 0) sub_menu = 6;
jah128	0:d6269d17c8cf	530	else sub_menu --;
jah128	0:d6269d17c8cf	531	break;
jah128	15:66be5ec52c3b	532	case 6: //Demo Menu
jah128	20:1bc6c6dc477b	533	stop_demo();
jah128	16:50686c07ad07	534	if(sub_menu == 0) sub_menu = 5;
jah128	0:d6269d17c8cf	535	else sub_menu --;
jah128	0:d6269d17c8cf	536	break;
jah128	15:66be5ec52c3b	537	case 7: //Calibrate Menu
jah128	4:1c621cb8cf0d	538	if(sub_menu == 0) sub_menu = 2;
jah128	4:1c621cb8cf0d	539	else sub_menu --;
jah128	4:1c621cb8cf0d	540	break;
jah128	15:66be5ec52c3b	541	case 8: //PsiBasic Menu
jah128	0:d6269d17c8cf	542	if(sub_menu == 0) sub_menu = psi_basic_file_count;
jah128	0:d6269d17c8cf	543	else sub_menu --;
jah128	0:d6269d17c8cf	544	}
jah128	0:d6269d17c8cf	545	break;
jah128	0:d6269d17c8cf	546
jah128	0:d6269d17c8cf	547	}
jah128	0:d6269d17c8cf	548	break;
jah128	0:d6269d17c8cf	549	case 8: //Right pressed
jah128	0:d6269d17c8cf	550	switch(level) {
jah128	0:d6269d17c8cf	551	case 0:
jah128	20:1bc6c6dc477b	552	stop_demo();
jah128	0:d6269d17c8cf	553	top_menu ++;
jah128	17:bf614e28668f	554	if(top_menu == 8 && use_flash_basic == 0) top_menu = 9;
jah128	19:3e3b03d80ea3	555	if(has_433_radio == 0 && top_menu == 4) top_menu = 5;
jah128	17:bf614e28668f	556	if((top_menu) > 9) top_menu = 0;
jah128	0:d6269d17c8cf	557	break;
jah128	0:d6269d17c8cf	558	case 1:
jah128	0:d6269d17c8cf	559	switch(top_menu) {
jah128	15:66be5ec52c3b	560	case 1: //LED Menu
jah128	0:d6269d17c8cf	561	if(sub_menu == 13) sub_menu = 0;
jah128	0:d6269d17c8cf	562	else sub_menu ++;
jah128	0:d6269d17c8cf	563	break;
jah128	15:66be5ec52c3b	564	case 2: //Sensors Menu
jah128	17:bf614e28668f	565	if(sub_menu == 13) sub_menu = 0;
jah128	0:d6269d17c8cf	566	else sub_menu ++;
jah128	0:d6269d17c8cf	567	break;
jah128	15:66be5ec52c3b	568	case 3: //Motor Menu
jah128	0:d6269d17c8cf	569	if(sub_menu == 3) sub_menu = 0;
jah128	0:d6269d17c8cf	570	else sub_menu ++;
jah128	0:d6269d17c8cf	571	break;
jah128	15:66be5ec52c3b	572	case 5: //Info Menu
jah128	0:d6269d17c8cf	573	if(sub_menu == 6) sub_menu = 0;
jah128	0:d6269d17c8cf	574	else sub_menu ++;
jah128	0:d6269d17c8cf	575	break;
jah128	15:66be5ec52c3b	576	case 6: //Demo Menu
jah128	20:1bc6c6dc477b	577	stop_demo();
jah128	16:50686c07ad07	578	if(sub_menu == 5) sub_menu = 0;
jah128	0:d6269d17c8cf	579	else sub_menu ++;
jah128	0:d6269d17c8cf	580	break;
jah128	15:66be5ec52c3b	581	case 7: //Calibrate Menu
jah128	4:1c621cb8cf0d	582	if(sub_menu == 2) sub_menu = 0;
jah128	4:1c621cb8cf0d	583	else sub_menu ++;
jah128	4:1c621cb8cf0d	584	break;
jah128	15:66be5ec52c3b	585	case 8: //PsiBasic Menu
jah128	0:d6269d17c8cf	586	if(sub_menu == psi_basic_file_count) sub_menu = 0;
jah128	0:d6269d17c8cf	587	else sub_menu ++;
jah128	0:d6269d17c8cf	588	}
jah128	0:d6269d17c8cf	589	break;
jah128	0:d6269d17c8cf	590	}
jah128	0:d6269d17c8cf	591	break;
jah128	0:d6269d17c8cf	592	}
jah128	0:d6269d17c8cf	593	} else started = 1;
jah128	20:1bc6c6dc477b	594	if(demo_running == 0) {
jah128	20:1bc6c6dc477b	595	display.clear_display();
jah128	20:1bc6c6dc477b	596	switch(level) {
jah128	20:1bc6c6dc477b	597	case 0:
jah128	20:1bc6c6dc477b	598	//Top level menu
jah128	20:1bc6c6dc477b	599	switch(top_menu) {
jah128	20:1bc6c6dc477b	600	case 0:
jah128	20:1bc6c6dc477b	601	strcpy(topline,"---QUICK TEST---");
jah128	20:1bc6c6dc477b	602	break;
jah128	20:1bc6c6dc477b	603	case 1:
jah128	20:1bc6c6dc477b	604	strcpy(topline,"---TEST LEDS----");
jah128	20:1bc6c6dc477b	605	break;
jah128	20:1bc6c6dc477b	606	case 2:
jah128	20:1bc6c6dc477b	607	strcpy(topline,"--TEST SENSORS--");
jah128	20:1bc6c6dc477b	608	break;
jah128	20:1bc6c6dc477b	609	case 3:
jah128	20:1bc6c6dc477b	610	strcpy(topline,"--TEST MOTORS---");
jah128	20:1bc6c6dc477b	611	break;
jah128	20:1bc6c6dc477b	612	case 4:
jah128	20:1bc6c6dc477b	613	strcpy(topline,"---TEST RADIO---");
jah128	20:1bc6c6dc477b	614	break;
jah128	20:1bc6c6dc477b	615	case 5:
jah128	20:1bc6c6dc477b	616	strcpy(topline,"------INFO------");
jah128	20:1bc6c6dc477b	617	break;
jah128	20:1bc6c6dc477b	618	case 6:
jah128	20:1bc6c6dc477b	619	strcpy(topline,"-----DEMOS------");
jah128	20:1bc6c6dc477b	620	break;
jah128	20:1bc6c6dc477b	621	case 7:
jah128	20:1bc6c6dc477b	622	strcpy(topline,"---CALIBRATION--");
jah128	20:1bc6c6dc477b	623	break;
jah128	20:1bc6c6dc477b	624	case 8:
jah128	20:1bc6c6dc477b	625	strcpy(topline,"---PSI BASIC----");
jah128	20:1bc6c6dc477b	626	break;
jah128	20:1bc6c6dc477b	627	case 9:
jah128	20:1bc6c6dc477b	628	strcpy(topline,"------EXIT------");
jah128	20:1bc6c6dc477b	629	break;
jah128	20:1bc6c6dc477b	630	}
jah128	20:1bc6c6dc477b	631	strcpy(bottomline,"");
jah128	20:1bc6c6dc477b	632	break;
jah128	20:1bc6c6dc477b	633	case 1:
jah128	20:1bc6c6dc477b	634	//Sub level menu
jah128	20:1bc6c6dc477b	635	switch(top_menu) {
jah128	20:1bc6c6dc477b	636	case 8:
jah128	20:1bc6c6dc477b	637	strcpy(topline,"-PSI BASIC MENU-");
jah128	20:1bc6c6dc477b	638	if(sub_menu == psi_basic_file_count) strcpy(bottomline,"EXIT");
jah128	20:1bc6c6dc477b	639	else strcpy(bottomline,basic_filenames.at(sub_menu).c_str());
jah128	20:1bc6c6dc477b	640	break;
jah128	20:1bc6c6dc477b	641	case 1:
jah128	20:1bc6c6dc477b	642	strcpy(topline,"----LED MENU----");
jah128	20:1bc6c6dc477b	643	char led_status[7];
jah128	20:1bc6c6dc477b	644	if(sub_menu<9) {
jah128	20:1bc6c6dc477b	645	switch(led_state[sub_menu]) {
jah128	20:1bc6c6dc477b	646	case 0:
jah128	20:1bc6c6dc477b	647	strcpy(led_status,"OFF");
jah128	20:1bc6c6dc477b	648	break;
jah128	20:1bc6c6dc477b	649	case 1:
jah128	20:1bc6c6dc477b	650	strcpy(led_status,"RED");
jah128	20:1bc6c6dc477b	651	break;
jah128	20:1bc6c6dc477b	652	case 2:
jah128	20:1bc6c6dc477b	653	strcpy(led_status,"GREEN");
jah128	20:1bc6c6dc477b	654	break;
jah128	20:1bc6c6dc477b	655	case 3:
jah128	20:1bc6c6dc477b	656	strcpy(led_status,"YELLOW");
jah128	20:1bc6c6dc477b	657	break;
jah128	20:1bc6c6dc477b	658	}
jah128	20:1bc6c6dc477b	659	}
jah128	20:1bc6c6dc477b	660	if(sub_menu < 8) sprintf(bottomline,"OUTER %d: %s",(sub_menu + 1),led_status);
jah128	20:1bc6c6dc477b	661	if(sub_menu == 8) sprintf(bottomline,"CENTER: %s",led_status);
jah128	20:1bc6c6dc477b	662	if(sub_menu == 9) {
jah128	20:1bc6c6dc477b	663	switch(all_led_state) {
jah128	20:1bc6c6dc477b	664	case 0:
jah128	20:1bc6c6dc477b	665	strcpy(led_status,"OFF");
jah128	20:1bc6c6dc477b	666	break;
jah128	20:1bc6c6dc477b	667	case 1:
jah128	20:1bc6c6dc477b	668	strcpy(led_status,"RED");
jah128	20:1bc6c6dc477b	669	break;
jah128	20:1bc6c6dc477b	670	case 2:
jah128	20:1bc6c6dc477b	671	strcpy(led_status,"GREEN");
jah128	20:1bc6c6dc477b	672	break;
jah128	20:1bc6c6dc477b	673	case 3:
jah128	20:1bc6c6dc477b	674	strcpy(led_status,"YELLOW");
jah128	20:1bc6c6dc477b	675	break;
jah128	20:1bc6c6dc477b	676	}
jah128	20:1bc6c6dc477b	677	sprintf(bottomline,"SET ALL %s", led_status);
jah128	20:1bc6c6dc477b	678	}
jah128	20:1bc6c6dc477b	679	if(sub_menu == 10) {
jah128	20:1bc6c6dc477b	680	if(base_led_state == 0) strcpy(led_status,"OFF");
jah128	20:1bc6c6dc477b	681	else strcpy(led_status,"ON");
jah128	20:1bc6c6dc477b	682	sprintf(bottomline,"BASE: %s",led_status);
jah128	20:1bc6c6dc477b	683	}
jah128	20:1bc6c6dc477b	684	if(sub_menu == 11) sprintf(bottomline,"CLED BNESS %d%%", brightness);
jah128	20:1bc6c6dc477b	685	if(sub_menu == 12) sprintf(bottomline,"DISP BNESS %d%%", bl_brightness);
jah128	20:1bc6c6dc477b	686	if(sub_menu == 13) strcpy(bottomline,"EXIT");
jah128	20:1bc6c6dc477b	687	break;
jah128	20:1bc6c6dc477b	688
jah128	20:1bc6c6dc477b	689	case 2:
jah128	20:1bc6c6dc477b	690	strcpy(topline,"--SENSORS MENU--");
jah128	20:1bc6c6dc477b	691	switch(sub_menu) {
jah128	20:1bc6c6dc477b	692	case 0: {
jah128	20:1bc6c6dc477b	693	float battery = sensors.get_battery_voltage ();
jah128	20:1bc6c6dc477b	694	sprintf(bottomline,"BATTERY: %1.3fV",battery);
jah128	0:d6269d17c8cf	695	break;
jah128	20:1bc6c6dc477b	696	}
jah128	20:1bc6c6dc477b	697	case 1: {
jah128	20:1bc6c6dc477b	698	float dc = sensors.get_dc_voltage ();
jah128	20:1bc6c6dc477b	699	sprintf(bottomline,"DC: %1.3fV",dc);
jah128	20:1bc6c6dc477b	700	break;
jah128	20:1bc6c6dc477b	701	}
jah128	20:1bc6c6dc477b	702	case 2: {
jah128	20:1bc6c6dc477b	703	float current = sensors.get_current ();
jah128	20:1bc6c6dc477b	704	sprintf(bottomline,"CURRENT: %1.3fA",current);
jah128	0:d6269d17c8cf	705	break;
jah128	20:1bc6c6dc477b	706	}
jah128	20:1bc6c6dc477b	707	case 3: {
jah128	20:1bc6c6dc477b	708	float temperature = sensors.get_temperature();
jah128	20:1bc6c6dc477b	709	sprintf(bottomline,"TEMP: %3.2fC", temperature);
jah128	0:d6269d17c8cf	710	break;
jah128	20:1bc6c6dc477b	711	}
jah128	20:1bc6c6dc477b	712	case 4:
jah128	20:1bc6c6dc477b	713	sensors.store_background_base_ir_values();
jah128	20:1bc6c6dc477b	714	sprintf(bottomline,"BIR%dB:%d",base_ir_index+1,sensors.get_background_base_ir_value(base_ir_index));
jah128	20:1bc6c6dc477b	715	break;
jah128	20:1bc6c6dc477b	716	case 5:
jah128	20:1bc6c6dc477b	717	sensors.store_illuminated_base_ir_values();
jah128	20:1bc6c6dc477b	718	sprintf(bottomline,"BIR%dR:%d",base_ir_index+1,sensors.get_illuminated_base_ir_value(base_ir_index));
jah128	20:1bc6c6dc477b	719	break;
jah128	20:1bc6c6dc477b	720	case 6:
jah128	20:1bc6c6dc477b	721	sensors.store_illuminated_base_ir_values();
jah128	20:1bc6c6dc477b	722	sprintf(bottomline,"BIC%dR:%1.3f",base_ir_index+1,sensors.get_calibrated_base_ir_value(base_ir_index));
jah128	0:d6269d17c8cf	723	break;
jah128	20:1bc6c6dc477b	724	case 7:
jah128	20:1bc6c6dc477b	725	sensors.store_background_raw_ir_values();
jah128	20:1bc6c6dc477b	726	sprintf(bottomline,"SIR%dB:%d",side_ir_index+1,sensors.get_background_raw_ir_value(side_ir_index));
jah128	20:1bc6c6dc477b	727	break;
jah128	20:1bc6c6dc477b	728	case 8:
jah128	20:1bc6c6dc477b	729	sensors.store_illuminated_raw_ir_values();
jah128	20:1bc6c6dc477b	730	sprintf(bottomline,"SIR%dR:%d",side_ir_index+1,sensors.get_illuminated_raw_ir_value(side_ir_index));
jah128	20:1bc6c6dc477b	731	break;
jah128	20:1bc6c6dc477b	732	case 9:
jah128	20:1bc6c6dc477b	733	sprintf(bottomline,"SIR%dD:%3.1f",side_ir_index+1,0.0);
jah128	0:d6269d17c8cf	734	break;
jah128	20:1bc6c6dc477b	735	case 10:
jah128	20:1bc6c6dc477b	736	if(ultrasonic_distance_updated == 1) {
jah128	20:1bc6c6dc477b	737	sprintf(bottomline,"USONIC:%3dcm",ultrasonic_distance);
jah128	20:1bc6c6dc477b	738	} else sprintf(bottomline,"USONIC:---------");
jah128	20:1bc6c6dc477b	739	sensors.update_ultrasonic_measure();
jah128	0:d6269d17c8cf	740	break;
jah128	20:1bc6c6dc477b	741	case 11:
jah128	20:1bc6c6dc477b	742	sensors.store_line_position();
jah128	20:1bc6c6dc477b	743	if(line_found == 1)sprintf(bottomline,"LINE:%1.3f",line_position);
jah128	20:1bc6c6dc477b	744	else sprintf(bottomline,"LINE:---------");
jah128	0:d6269d17c8cf	745	break;
jah128	20:1bc6c6dc477b	746
jah128	20:1bc6c6dc477b	747	case 12:
jah128	20:1bc6c6dc477b	748	sprintf(bottomline,"COLOUR: %s",colour.get_colour_string(colour.detect_colour_once()));
jah128	20:1bc6c6dc477b	749	break;
jah128	20:1bc6c6dc477b	750	case 13:
jah128	20:1bc6c6dc477b	751	sprintf(bottomline,"EXIT");
jah128	0:d6269d17c8cf	752	break;
jah128	0:d6269d17c8cf	753	}
jah128	20:1bc6c6dc477b	754	break;
jah128	20:1bc6c6dc477b	755	case 3:
jah128	20:1bc6c6dc477b	756	strcpy(topline,"--MOTORS MENU---");
jah128	20:1bc6c6dc477b	757	switch(sub_menu) {
jah128	20:1bc6c6dc477b	758	case 0:
jah128	20:1bc6c6dc477b	759	sprintf(bottomline,"LEFT: %d%%", left_speed);
jah128	20:1bc6c6dc477b	760	break;
jah128	20:1bc6c6dc477b	761	case 1:
jah128	20:1bc6c6dc477b	762	sprintf(bottomline,"RIGHT: %d%%", right_speed);
jah128	20:1bc6c6dc477b	763	break;
jah128	20:1bc6c6dc477b	764	case 2:
jah128	20:1bc6c6dc477b	765	char both_mode_string[16];
jah128	20:1bc6c6dc477b	766	switch(both_motor_mode) {
jah128	20:1bc6c6dc477b	767	case 0:
jah128	20:1bc6c6dc477b	768	strcpy(both_mode_string,"OFF");
jah128	20:1bc6c6dc477b	769	break;
jah128	20:1bc6c6dc477b	770	case 1:
jah128	20:1bc6c6dc477b	771	strcpy(both_mode_string,"BRAKE");
jah128	20:1bc6c6dc477b	772	break;
jah128	20:1bc6c6dc477b	773	case 2:
jah128	20:1bc6c6dc477b	774	strcpy(both_mode_string,"+50%");
jah128	20:1bc6c6dc477b	775	break;
jah128	20:1bc6c6dc477b	776	case 3:
jah128	20:1bc6c6dc477b	777	strcpy(both_mode_string,"+100%");
jah128	20:1bc6c6dc477b	778	break;
jah128	20:1bc6c6dc477b	779	case 4:
jah128	20:1bc6c6dc477b	780	strcpy(both_mode_string,"-50%");
jah128	20:1bc6c6dc477b	781	break;
jah128	20:1bc6c6dc477b	782	case 5:
jah128	20:1bc6c6dc477b	783	strcpy(both_mode_string,"-100%");
jah128	20:1bc6c6dc477b	784	break;
jah128	20:1bc6c6dc477b	785	}
jah128	20:1bc6c6dc477b	786	sprintf(bottomline,"BOTH TO %s",both_mode_string);
jah128	20:1bc6c6dc477b	787	break;
jah128	20:1bc6c6dc477b	788	case 3:
jah128	20:1bc6c6dc477b	789	sprintf(bottomline,"EXIT");
jah128	20:1bc6c6dc477b	790	break;
jah128	0:d6269d17c8cf	791	}
jah128	20:1bc6c6dc477b	792	break;
jah128	20:1bc6c6dc477b	793	case 4:
jah128	20:1bc6c6dc477b	794	strcpy(topline,"---RADIO MENU---");
jah128	20:1bc6c6dc477b	795	switch(sub_menu) {
jah128	18:9204f74069b4	796
jah128	20:1bc6c6dc477b	797	case 0:
jah128	20:1bc6c6dc477b	798	sprintf(bottomline,"EXIT");
jah128	20:1bc6c6dc477b	799	break;
jah128	20:1bc6c6dc477b	800	}
jah128	20:1bc6c6dc477b	801	break;
jah128	20:1bc6c6dc477b	802	case 5:
jah128	20:1bc6c6dc477b	803	strcpy(topline,"---INFO MENU----");
jah128	20:1bc6c6dc477b	804	switch(sub_menu) {
jah128	20:1bc6c6dc477b	805	case 0:
jah128	20:1bc6c6dc477b	806	sprintf(bottomline,"ROBOT ID: %d",robot_id);
jah128	20:1bc6c6dc477b	807	break;
jah128	20:1bc6c6dc477b	808	case 1:
jah128	20:1bc6c6dc477b	809	sprintf(bottomline,"SOFTWARE: %1.2f",SOFTWARE_VERSION_CODE);
jah128	20:1bc6c6dc477b	810	break;
jah128	20:1bc6c6dc477b	811	case 2:
jah128	20:1bc6c6dc477b	812	if(firmware_version > 0) sprintf(bottomline,"FIRMWARE: %1.2f",firmware_version);
jah128	20:1bc6c6dc477b	813	else sprintf(bottomline,"FIRMWARE: ?????");
jah128	20:1bc6c6dc477b	814	break;
jah128	20:1bc6c6dc477b	815	case 3:
jah128	20:1bc6c6dc477b	816	sprintf(bottomline,"PROG:%s",program_name);
jah128	20:1bc6c6dc477b	817	break;
jah128	20:1bc6c6dc477b	818	case 4:
jah128	20:1bc6c6dc477b	819	sprintf(bottomline,"AUTH:%s",author_name);
jah128	20:1bc6c6dc477b	820	break;
jah128	20:1bc6c6dc477b	821	case 5:
jah128	20:1bc6c6dc477b	822	sprintf(bottomline,"VER:%s",version_name);
jah128	20:1bc6c6dc477b	823	break;
jah128	20:1bc6c6dc477b	824	case 6:
jah128	20:1bc6c6dc477b	825	sprintf(bottomline,"EXIT");
jah128	20:1bc6c6dc477b	826	break;
jah128	20:1bc6c6dc477b	827	}
jah128	20:1bc6c6dc477b	828	break;
jah128	20:1bc6c6dc477b	829	case 6:
jah128	20:1bc6c6dc477b	830	strcpy(topline,"---DEMOS MENU---");
jah128	20:1bc6c6dc477b	831	switch(sub_menu) {
jah128	20:1bc6c6dc477b	832	case 0:
jah128	20:1bc6c6dc477b	833	sprintf(bottomline,"LINE FOLLOW");
jah128	20:1bc6c6dc477b	834	break;
jah128	20:1bc6c6dc477b	835	case 1:
jah128	20:1bc6c6dc477b	836	sprintf(bottomline,"OBST. AVOID");
jah128	20:1bc6c6dc477b	837	break;
jah128	20:1bc6c6dc477b	838	case 2:
jah128	20:1bc6c6dc477b	839	sprintf(bottomline,"COLOUR SPIN");
jah128	20:1bc6c6dc477b	840	break;
jah128	20:1bc6c6dc477b	841	case 3:
jah128	20:1bc6c6dc477b	842	sprintf(bottomline,"STRESS TEST");
jah128	20:1bc6c6dc477b	843	break;
jah128	20:1bc6c6dc477b	844	case 4:
jah128	20:1bc6c6dc477b	845	sprintf(bottomline,"COLOUR WALK");
jah128	20:1bc6c6dc477b	846	break;
jah128	20:1bc6c6dc477b	847	case 5:
jah128	20:1bc6c6dc477b	848	sprintf(bottomline,"EXIT");
jah128	20:1bc6c6dc477b	849	break;
jah128	20:1bc6c6dc477b	850	}
jah128	20:1bc6c6dc477b	851	break;
jah128	20:1bc6c6dc477b	852	case 7:
jah128	20:1bc6c6dc477b	853	strcpy(topline,"---CALIB. MENU--");
jah128	20:1bc6c6dc477b	854	switch(sub_menu) {
jah128	20:1bc6c6dc477b	855	case 0:
jah128	20:1bc6c6dc477b	856	sprintf(bottomline,"BASE SENSOR");
jah128	20:1bc6c6dc477b	857	break;
jah128	20:1bc6c6dc477b	858	case 1:
jah128	20:1bc6c6dc477b	859	sprintf(bottomline,"MOTOR");
jah128	20:1bc6c6dc477b	860	break;
jah128	20:1bc6c6dc477b	861	case 2:
jah128	20:1bc6c6dc477b	862	sprintf(bottomline,"EXIT");
jah128	20:1bc6c6dc477b	863	break;
jah128	20:1bc6c6dc477b	864	}
jah128	20:1bc6c6dc477b	865	break;
jah128	20:1bc6c6dc477b	866	}
jah128	20:1bc6c6dc477b	867	break;
jah128	20:1bc6c6dc477b	868	}
jah128	20:1bc6c6dc477b	869	display.write_string(topline);
jah128	20:1bc6c6dc477b	870	display.set_position(1,0);
jah128	20:1bc6c6dc477b	871	display.write_string(bottomline);
jah128	20:1bc6c6dc477b	872	// Periodically update when on sensors menu
jah128	20:1bc6c6dc477b	873	if(top_menu == 2 && level == 1) {
jah128	20:1bc6c6dc477b	874	demo_event.attach(this,&Demo::demo_event_thread, 0.25);
jah128	20:1bc6c6dc477b	875	}
jah128	0:d6269d17c8cf	876	}
jah128	0:d6269d17c8cf	877	}
jah128	0:d6269d17c8cf	878
jah128	15:66be5ec52c3b	879	char test_step = 0;
jah128	15:66be5ec52c3b	880	char test_substep = 0;
jah128	15:66be5ec52c3b	881	char test_warnings = 0;
jah128	15:66be5ec52c3b	882
jah128	15:66be5ec52c3b	883
jah128	15:66be5ec52c3b	884	void Demo::quick_test_cycle()
jah128	15:66be5ec52c3b	885	{
jah128	20:1bc6c6dc477b	886	char next_step [] = {4,5,8,3,6,4};
jah128	15:66be5ec52c3b	887	char test_message [17];
jah128	20:1bc6c6dc477b	888	int wait_period = SELF_TEST_PERIOD * 5;
jah128	18:9204f74069b4	889
jah128	15:66be5ec52c3b	890	if(test_substep == 0) {
jah128	15:66be5ec52c3b	891	display.clear_display();
jah128	20:1bc6c6dc477b	892	if(test_step == 5 && has_base_colour_sensor == 0)test_step ++;
jah128	15:66be5ec52c3b	893	switch(test_step) {
jah128	15:66be5ec52c3b	894	case 0:
jah128	15:66be5ec52c3b	895	display.write_string("01 - Power ");
jah128	20:1bc6c6dc477b	896	pc.printf("\nTest 01: Power tests [%f]\n",psi.get_uptime());
jah128	15:66be5ec52c3b	897	break;
jah128	15:66be5ec52c3b	898	case 1:
jah128	15:66be5ec52c3b	899	display.write_string("02 - Base IR ");
jah128	15:66be5ec52c3b	900	pc.printf("\nTest 02: Base infrared tests [%f]\n",psi.get_uptime());
jah128	15:66be5ec52c3b	901	break;
jah128	15:66be5ec52c3b	902	case 2:
jah128	15:66be5ec52c3b	903	display.write_string("03 - Side IR ");
jah128	15:66be5ec52c3b	904	pc.printf("\nTest 03: Side infrared tests [%f]\n",psi.get_uptime());
jah128	15:66be5ec52c3b	905	break;
jah128	15:66be5ec52c3b	906	case 3:
jah128	15:66be5ec52c3b	907	display.write_string("04 - LEDs ");
jah128	20:1bc6c6dc477b	908	pc.printf("\nTest 04: LED tests [%f]\n",psi.get_uptime());
jah128	20:1bc6c6dc477b	909	break;
jah128	20:1bc6c6dc477b	910	case 4:
jah128	20:1bc6c6dc477b	911	display.write_string("05 - Motors ");
jah128	20:1bc6c6dc477b	912	pc.printf("\nTest 05: Motor tests [%f]\n",psi.get_uptime());
jah128	20:1bc6c6dc477b	913	break;
jah128	20:1bc6c6dc477b	914	case 5:
jah128	20:1bc6c6dc477b	915	display.write_string("06 - Colour ");
jah128	20:1bc6c6dc477b	916	pc.printf("\nTest 06: Base colour sensor tests [%f]\n",psi.get_uptime());
jah128	15:66be5ec52c3b	917	break;
jah128	15:66be5ec52c3b	918	}
jah128	15:66be5ec52c3b	919	}
jah128	20:1bc6c6dc477b	920	char wait_time = 8;
jah128	20:1bc6c6dc477b	921	if(test_step == 5) {
jah128	20:1bc6c6dc477b	922	// Base colour sensor tests
jah128	20:1bc6c6dc477b	923	int dark_rgb_readings [4];
jah128	20:1bc6c6dc477b	924	int light_rgb_readings [4];
jah128	20:1bc6c6dc477b	925	colour.enable_base_colour_sensor();
jah128	20:1bc6c6dc477b	926	wait(0.03);
jah128	20:1bc6c6dc477b	927	colour.read_base_colour_sensor_values( dark_rgb_readings);
jah128	20:1bc6c6dc477b	928	led.set_base_led(1);
jah128	20:1bc6c6dc477b	929	wait(0.05);
jah128	20:1bc6c6dc477b	930	colour.read_base_colour_sensor_values( light_rgb_readings);
jah128	20:1bc6c6dc477b	931	led.set_base_led(0);
jah128	20:1bc6c6dc477b	932	colour.disable_base_colour_sensor();
jah128	20:1bc6c6dc477b	933	wait(0.02);
jah128	20:1bc6c6dc477b	934	float adjusted[4];
jah128	20:1bc6c6dc477b	935	colour.get_calibrated_colour(light_rgb_readings,adjusted);
jah128	20:1bc6c6dc477b	936	int last_detected_colour = colour.identify_colour_from_calibrated_colour_scores(adjusted);
jah128	20:1bc6c6dc477b	937	pc.printf("Sample %d Dark: C%04d-R%04d-G%04d-B%04d Light: C%04d-R%04d-G%04d-B%04d Calib: C%1.3f-R%1.3f-G%1.3f-B%1.3f Colour: %s\n",
jah128	20:1bc6c6dc477b	938	dark_rgb_readings[0],dark_rgb_readings[1],dark_rgb_readings[2],dark_rgb_readings[3],
jah128	20:1bc6c6dc477b	939	light_rgb_readings[0],light_rgb_readings[1],light_rgb_readings[2],light_rgb_readings[3],
jah128	20:1bc6c6dc477b	940	adjusted[0],adjusted[1],adjusted[2],adjusted[3],colour.get_colour_string(last_detected_colour) );
jah128	20:1bc6c6dc477b	941	switch(test_substep){
jah128	20:1bc6c6dc477b	942	case 0:sprintf(test_message,"D:%4d %4d %4d",dark_rgb_readings[1],dark_rgb_readings[2],dark_rgb_readings[3]);break;
jah128	20:1bc6c6dc477b	943	case 1:sprintf(test_message,"L:%4d %4d %4d",light_rgb_readings[1],light_rgb_readings[2],light_rgb_readings[3]);break;
jah128	20:1bc6c6dc477b	944	case 2:sprintf(test_message,"C:%1.2f %1.2f %1.2f",adjusted[1],adjusted[2],adjusted[3]);break;
jah128	20:1bc6c6dc477b	945	case 3:sprintf(test_message,"COLOUR: %s",colour.get_colour_string(last_detected_colour));break;
jah128	20:1bc6c6dc477b	946	}
jah128	20:1bc6c6dc477b	947	}
jah128	20:1bc6c6dc477b	948
jah128	20:1bc6c6dc477b	949	if(test_step == 4) {
jah128	20:1bc6c6dc477b	950	// Motor tests
jah128	20:1bc6c6dc477b	951	switch(test_substep){
jah128	20:1bc6c6dc477b	952	case 0:sprintf(test_message,"20%% Forward");
jah128	20:1bc6c6dc477b	953	pc.printf("20%% Forward\n");
jah128	20:1bc6c6dc477b	954	motors.forward(0.2);
jah128	20:1bc6c6dc477b	955	wait_time = 16;
jah128	20:1bc6c6dc477b	956	break;
jah128	20:1bc6c6dc477b	957	case 1:sprintf(test_message,"40%% Forward");
jah128	20:1bc6c6dc477b	958	pc.printf("40%% Forward\n");
jah128	20:1bc6c6dc477b	959	motors.forward(0.4);
jah128	20:1bc6c6dc477b	960	break;
jah128	20:1bc6c6dc477b	961	case 3:sprintf(test_message,"20%% Backward");
jah128	20:1bc6c6dc477b	962	pc.printf("20%% Backward\n");
jah128	20:1bc6c6dc477b	963	motors.backward(0.2);
jah128	20:1bc6c6dc477b	964	wait_time = 16;
jah128	20:1bc6c6dc477b	965	break;
jah128	20:1bc6c6dc477b	966	case 4:sprintf(test_message,"40%% Backward");
jah128	20:1bc6c6dc477b	967	pc.printf("40%% Backward\n");
jah128	20:1bc6c6dc477b	968	motors.backward(0.4);
jah128	20:1bc6c6dc477b	969	break;
jah128	20:1bc6c6dc477b	970	case 2:case 5:sprintf(test_message,"BRAKE");
jah128	20:1bc6c6dc477b	971	pc.printf("Brake motors\n");
jah128	20:1bc6c6dc477b	972	motors.brake();
jah128	20:1bc6c6dc477b	973	break;
jah128	20:1bc6c6dc477b	974	}
jah128	20:1bc6c6dc477b	975	wait_us(SELF_TEST_PERIOD * wait_time);
jah128	20:1bc6c6dc477b	976	motors.stop();
jah128	20:1bc6c6dc477b	977	wait_us(SELF_TEST_PERIOD);
jah128	20:1bc6c6dc477b	978	}
jah128	20:1bc6c6dc477b	979	if(test_step == 3) {
jah128	20:1bc6c6dc477b	980	//LED tests
jah128	20:1bc6c6dc477b	981	switch(test_substep){
jah128	20:1bc6c6dc477b	982	case 0:sprintf(test_message,"RED");
jah128	20:1bc6c6dc477b	983	led.set_leds(0x00,0xFF);
jah128	20:1bc6c6dc477b	984	led.set_center_led(1,1);
jah128	20:1bc6c6dc477b	985	break;
jah128	20:1bc6c6dc477b	986	case 1:sprintf(test_message,"GREEN");
jah128	20:1bc6c6dc477b	987	led.set_leds(0xFF,0x00);
jah128	20:1bc6c6dc477b	988	led.set_center_led(2,1);
jah128	20:1bc6c6dc477b	989	break;
jah128	20:1bc6c6dc477b	990	case 2:sprintf(test_message,"YELLOW");
jah128	20:1bc6c6dc477b	991	led.set_leds(0xFF,0xFF);
jah128	20:1bc6c6dc477b	992	led.set_center_led(3,1);
jah128	20:1bc6c6dc477b	993	break;
jah128	20:1bc6c6dc477b	994	}
jah128	20:1bc6c6dc477b	995	wait_us(SELF_TEST_PERIOD * 10);
jah128	20:1bc6c6dc477b	996	led.set_leds(0x00,0x00);
jah128	20:1bc6c6dc477b	997	led.set_center_led(0);
jah128	20:1bc6c6dc477b	998	wait_us(SELF_TEST_PERIOD);
jah128	20:1bc6c6dc477b	999	}
jah128	18:9204f74069b4	1000	if(test_step == 1) {
jah128	18:9204f74069b4	1001	//Base IR tests
jah128	18:9204f74069b4	1002	sensors.store_background_base_ir_values();
jah128	18:9204f74069b4	1003	wait(0.05);
jah128	18:9204f74069b4	1004	sensors.store_illuminated_base_ir_values();
jah128	18:9204f74069b4	1005	pc.printf("Sample %d 1: %04d-%04d-%1.2f 2: %04d-%04d-%1.2f 3: %04d-%04d-%1.2f 4: %04d-%04d-%1.2f 5: %04d-%04d-%1.2f\n", (test_substep+1),
jah128	18:9204f74069b4	1006	sensors.get_background_base_ir_value(0), sensors.get_illuminated_base_ir_value(0), sensors.get_calibrated_base_ir_value (0),
jah128	18:9204f74069b4	1007	sensors.get_background_base_ir_value(1), sensors.get_illuminated_base_ir_value(1),sensors.get_calibrated_base_ir_value (1),
jah128	18:9204f74069b4	1008	sensors.get_background_base_ir_value(2), sensors.get_illuminated_base_ir_value(2),sensors.get_calibrated_base_ir_value (2),
jah128	18:9204f74069b4	1009	sensors.get_background_base_ir_value(3), sensors.get_illuminated_base_ir_value(3),sensors.get_calibrated_base_ir_value (3),
jah128	18:9204f74069b4	1010	sensors.get_background_base_ir_value(4), sensors.get_illuminated_base_ir_value(4),sensors.get_calibrated_base_ir_value (4));
jah128	18:9204f74069b4	1011	sprintf(test_message,"%d:%4d-%4d-%1.2f",test_substep+1,sensors.get_background_base_ir_value(test_substep),sensors.get_illuminated_base_ir_value(test_substep),sensors.get_calibrated_base_ir_value(test_substep));
jah128	18:9204f74069b4	1012	wait_us(SELF_TEST_PERIOD);
jah128	18:9204f74069b4	1013	}
jah128	18:9204f74069b4	1014	if(test_step == 2) {
jah128	18:9204f74069b4	1015	//Side IR tests
jah128	18:9204f74069b4	1016	sensors.store_background_raw_ir_values();
jah128	18:9204f74069b4	1017	wait(0.05);
jah128	18:9204f74069b4	1018	sensors.store_illuminated_raw_ir_values();
jah128	18:9204f74069b4	1019	pc.printf("Sample %d 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d 6: %04d-%04d 7: %04d-%04d 8: %04d-%04d\n", (test_substep+1),
jah128	18:9204f74069b4	1020	sensors.get_background_raw_ir_value(0), sensors.get_illuminated_raw_ir_value(0),
jah128	18:9204f74069b4	1021	sensors.get_background_raw_ir_value(1), sensors.get_illuminated_raw_ir_value(1),
jah128	18:9204f74069b4	1022	sensors.get_background_raw_ir_value(2), sensors.get_illuminated_raw_ir_value(2),
jah128	18:9204f74069b4	1023	sensors.get_background_raw_ir_value(3), sensors.get_illuminated_raw_ir_value(3),
jah128	18:9204f74069b4	1024	sensors.get_background_raw_ir_value(4), sensors.get_illuminated_raw_ir_value(4),
jah128	18:9204f74069b4	1025	sensors.get_background_raw_ir_value(5), sensors.get_illuminated_raw_ir_value(5),
jah128	18:9204f74069b4	1026	sensors.get_background_raw_ir_value(6), sensors.get_illuminated_raw_ir_value(6),
jah128	18:9204f74069b4	1027	sensors.get_background_raw_ir_value(7), sensors.get_illuminated_raw_ir_value(7));
jah128	18:9204f74069b4	1028	sprintf(test_message,"%d:%4d-%4d",test_substep+1,sensors.get_background_raw_ir_value(test_substep),sensors.get_illuminated_raw_ir_value(test_substep));
jah128	18:9204f74069b4	1029	wait_us(SELF_TEST_PERIOD);
jah128	18:9204f74069b4	1030	}
jah128	15:66be5ec52c3b	1031	if(test_step == 0) {
jah128	15:66be5ec52c3b	1032	// Power self-test
jah128	15:66be5ec52c3b	1033	switch(test_substep) {
jah128	15:66be5ec52c3b	1034	case 0: { // Battery Voltage
jah128	15:66be5ec52c3b	1035	float battery_voltages [SAMPLE_SIZE];
jah128	15:66be5ec52c3b	1036	float mean_battery_voltage = 0;
jah128	15:66be5ec52c3b	1037	float sd_battery_voltage = 0;
jah128	15:66be5ec52c3b	1038	for(int i=0; i<SAMPLE_SIZE; i++) {
jah128	15:66be5ec52c3b	1039	battery_voltages[i]=sensors.get_battery_voltage ();
jah128	15:66be5ec52c3b	1040	mean_battery_voltage += battery_voltages[i];
jah128	15:66be5ec52c3b	1041	wait_us(SELF_TEST_PERIOD);
jah128	15:66be5ec52c3b	1042	}
jah128	15:66be5ec52c3b	1043	mean_battery_voltage /= SAMPLE_SIZE;
jah128	15:66be5ec52c3b	1044	for(int i=0; i<SAMPLE_SIZE; ++i) sd_battery_voltage += pow(battery_voltages[i] - mean_battery_voltage, 2);
jah128	15:66be5ec52c3b	1045	sd_battery_voltage = sqrt(sd_battery_voltage/SAMPLE_SIZE);
jah128	15:66be5ec52c3b	1046
jah128	15:66be5ec52c3b	1047	sprintf(test_message,"BATTERY: %1.3fV",mean_battery_voltage);
jah128	15:66be5ec52c3b	1048	pc.printf(" - Battery Voltage : %1.4fV [SD = % 1.4f]\n",mean_battery_voltage,sd_battery_voltage);
jah128	15:66be5ec52c3b	1049	if(mean_battery_voltage < 3.6) {
jah128	15:66be5ec52c3b	1050	pc.printf(" - WARNING : Battery voltage low\n");
jah128	15:66be5ec52c3b	1051	test_warnings++;
jah128	15:66be5ec52c3b	1052	}
jah128	15:66be5ec52c3b	1053	break;
jah128	15:66be5ec52c3b	1054	}
jah128	15:66be5ec52c3b	1055	case 1: {// DC Voltage
jah128	15:66be5ec52c3b	1056	float dc_voltages [SAMPLE_SIZE];
jah128	15:66be5ec52c3b	1057	float mean_dc_voltage = 0;
jah128	15:66be5ec52c3b	1058	float sd_dc_voltage = 0;
jah128	15:66be5ec52c3b	1059	for(int i=0; i<SAMPLE_SIZE; i++) {
jah128	15:66be5ec52c3b	1060	dc_voltages[i]=sensors.get_dc_voltage ();
jah128	15:66be5ec52c3b	1061	mean_dc_voltage += dc_voltages[i];
jah128	15:66be5ec52c3b	1062	wait_us(SELF_TEST_PERIOD);
jah128	15:66be5ec52c3b	1063	}
jah128	15:66be5ec52c3b	1064	mean_dc_voltage /= SAMPLE_SIZE;
jah128	15:66be5ec52c3b	1065	for(int i=0; i<SAMPLE_SIZE; ++i) sd_dc_voltage += pow(dc_voltages[i] - mean_dc_voltage, 2);
jah128	15:66be5ec52c3b	1066	sd_dc_voltage = sqrt(sd_dc_voltage/SAMPLE_SIZE);
jah128	15:66be5ec52c3b	1067
jah128	15:66be5ec52c3b	1068	sprintf(test_message,"DC : %1.3fV",mean_dc_voltage);
jah128	15:66be5ec52c3b	1069	pc.printf(" - DC Input Voltage : %1.4fV [SD = % 1.4f]\n",mean_dc_voltage,sd_dc_voltage);
jah128	15:66be5ec52c3b	1070	if(mean_dc_voltage < 0.5) {
jah128	15:66be5ec52c3b	1071	pc.printf(" - WARNING : DC voltage low - check charging resistor\n");
jah128	15:66be5ec52c3b	1072	test_warnings++;
jah128	15:66be5ec52c3b	1073	}
jah128	15:66be5ec52c3b	1074	break;
jah128	15:66be5ec52c3b	1075	}
jah128	15:66be5ec52c3b	1076
jah128	15:66be5ec52c3b	1077
jah128	15:66be5ec52c3b	1078	case 2: { // Current
jah128	15:66be5ec52c3b	1079	float currents [SAMPLE_SIZE];
jah128	15:66be5ec52c3b	1080	float mean_current = 0;
jah128	15:66be5ec52c3b	1081	float sd_current = 0;
jah128	15:66be5ec52c3b	1082	for(int i=0; i<SAMPLE_SIZE; i++) {
jah128	15:66be5ec52c3b	1083	currents[i]=sensors.get_current ();
jah128	15:66be5ec52c3b	1084	mean_current += currents[i];
jah128	15:66be5ec52c3b	1085	wait_us(SELF_TEST_PERIOD);
jah128	15:66be5ec52c3b	1086	}
jah128	15:66be5ec52c3b	1087	mean_current /= SAMPLE_SIZE;
jah128	15:66be5ec52c3b	1088	for(int i=0; i<SAMPLE_SIZE; ++i) sd_current += pow(currents[i] - mean_current, 2);
jah128	15:66be5ec52c3b	1089	sd_current = sqrt(sd_current/SAMPLE_SIZE);
jah128	15:66be5ec52c3b	1090
jah128	15:66be5ec52c3b	1091	sprintf(test_message,"CURRENT: %1.3fV",mean_current);
jah128	18:9204f74069b4	1092	pc.printf(" - Load Current : %1.4fA [SD = % 1.4f]\n",mean_current,sd_current);
jah128	15:66be5ec52c3b	1093	if(mean_current > 0.2) {
jah128	15:66be5ec52c3b	1094	pc.printf(" - WARNING : Higher than expected load current\n");
jah128	15:66be5ec52c3b	1095	test_warnings++;
jah128	15:66be5ec52c3b	1096	}
jah128	15:66be5ec52c3b	1097	break;
jah128	15:66be5ec52c3b	1098	}
jah128	18:9204f74069b4	1099
jah128	15:66be5ec52c3b	1100	case 3: {// System temperature
jah128	15:66be5ec52c3b	1101	float temps[SAMPLE_SIZE];
jah128	15:66be5ec52c3b	1102	float mean_temp = 0;
jah128	15:66be5ec52c3b	1103	float sd_temp = 0;
jah128	15:66be5ec52c3b	1104	for(int i=0; i<SAMPLE_SIZE; i++) {
jah128	15:66be5ec52c3b	1105	temps[i]=sensors.get_temperature();
jah128	15:66be5ec52c3b	1106	mean_temp += temps[i];
jah128	15:66be5ec52c3b	1107	wait_us(SELF_TEST_PERIOD);
jah128	15:66be5ec52c3b	1108	}
jah128	15:66be5ec52c3b	1109	mean_temp /= SAMPLE_SIZE;
jah128	15:66be5ec52c3b	1110	for(int i=0; i<SAMPLE_SIZE; ++i) sd_temp += pow(temps[i] - mean_temp, 2);
jah128	15:66be5ec52c3b	1111	sd_temp = sqrt(sd_temp/SAMPLE_SIZE);
jah128	15:66be5ec52c3b	1112
jah128	15:66be5ec52c3b	1113	sprintf(test_message,"TEMP : %3.2fC",mean_temp);
jah128	18:9204f74069b4	1114	pc.printf(" - System Temperature : %3.2fC [SD = % 1.4f]\n",mean_temp,sd_temp);
jah128	15:66be5ec52c3b	1115	if(mean_temp > 32) {
jah128	15:66be5ec52c3b	1116	pc.printf(" - WARNING : High system temperature detected\n");
jah128	15:66be5ec52c3b	1117	test_warnings++;
jah128	15:66be5ec52c3b	1118	}
jah128	15:66be5ec52c3b	1119	if(mean_temp < 10) {
jah128	15:66be5ec52c3b	1120	pc.printf(" - WARNING : Low system temperature detected\n");
jah128	15:66be5ec52c3b	1121	test_warnings++;
jah128	15:66be5ec52c3b	1122	}
jah128	15:66be5ec52c3b	1123	break;
jah128	15:66be5ec52c3b	1124	}
jah128	15:66be5ec52c3b	1125	}
jah128	15:66be5ec52c3b	1126	}
jah128	15:66be5ec52c3b	1127
jah128	15:66be5ec52c3b	1128	display.set_position(1,0);
jah128	15:66be5ec52c3b	1129	display.write_string(" ");
jah128	15:66be5ec52c3b	1130	display.set_position(1,0);
jah128	15:66be5ec52c3b	1131	display.write_string(test_message);
jah128	15:66be5ec52c3b	1132	test_substep++;
jah128	15:66be5ec52c3b	1133	if(test_substep >= next_step[test_step]) {
jah128	15:66be5ec52c3b	1134	test_substep = 0;
jah128	15:66be5ec52c3b	1135	test_step ++;
jah128	20:1bc6c6dc477b	1136	if(test_step >= 6) test_step = 0;
jah128	15:66be5ec52c3b	1137	}
jah128	15:66be5ec52c3b	1138
jah128	20:1bc6c6dc477b	1139	if(demo_running == 1) {
jah128	20:1bc6c6dc477b	1140	demo_timeout.attach_us(this,&Demo::quick_test_cycle, wait_period);
jah128	20:1bc6c6dc477b	1141	} else pc.printf("- Quick test terminated\n");
jah128	15:66be5ec52c3b	1142	}
jah128	15:66be5ec52c3b	1143
jah128	15:66be5ec52c3b	1144	void Demo::toggle_quick_test()
jah128	15:66be5ec52c3b	1145	{
jah128	15:66be5ec52c3b	1146	quick_test = 1 - quick_test;
jah128	15:66be5ec52c3b	1147	if(quick_test == 0) {
jah128	15:66be5ec52c3b	1148	pc.printf("________________________________________\n");
jah128	15:66be5ec52c3b	1149	pc.printf("Self test stopped at %f with %d warnings\n\n",psi.get_uptime(),test_warnings);
jah128	15:66be5ec52c3b	1150	display.clear_display();
jah128	15:66be5ec52c3b	1151	display.write_string("---QUICK TEST---");
jah128	20:1bc6c6dc477b	1152	stop_demo();
jah128	15:66be5ec52c3b	1153	} else {
jah128	15:66be5ec52c3b	1154	// Reset all LEDs, motors, display
jah128	15:66be5ec52c3b	1155	pc.printf("\n________________________________________\n");
jah128	15:66be5ec52c3b	1156	pc.printf("Self test started at %f\n\n",psi.get_uptime(),test_warnings);
jah128	15:66be5ec52c3b	1157	display.clear_display();
jah128	15:66be5ec52c3b	1158	test_step = 0;
jah128	15:66be5ec52c3b	1159	test_substep = 0;
jah128	15:66be5ec52c3b	1160	demo_running = 1;
jah128	15:66be5ec52c3b	1161	demo_timeout.attach_us(this,&Demo::quick_test_cycle,1000);
jah128	15:66be5ec52c3b	1162	}
jah128	15:66be5ec52c3b	1163	}
jah128	15:66be5ec52c3b	1164
jah128	20:1bc6c6dc477b	1165	void Demo::setup_demo()
jah128	0:d6269d17c8cf	1166	{
jah128	0:d6269d17c8cf	1167	display.set_backlight_brightness(0);
jah128	0:d6269d17c8cf	1168	time_out = 0.25f;
jah128	0:d6269d17c8cf	1169	demo_timer.start();
jah128	0:d6269d17c8cf	1170	state = 0;
jah128	0:d6269d17c8cf	1171	speed = 0;
jah128	0:d6269d17c8cf	1172	led_step = 0;
jah128	0:d6269d17c8cf	1173	demo_running = 1;
jah128	20:1bc6c6dc477b	1174	}
jah128	20:1bc6c6dc477b	1175
jah128	20:1bc6c6dc477b	1176	void Demo::start_line_demo()
jah128	20:1bc6c6dc477b	1177	{
jah128	20:1bc6c6dc477b	1178	setup_demo();
jah128	11:312663037b8c	1179	demo_timeout.attach_us(this,&Demo::line_demo_cycle,1000);
jah128	0:d6269d17c8cf	1180	}
jah128	0:d6269d17c8cf	1181
jah128	11:312663037b8c	1182	void Demo::start_obstacle_demo()
jah128	0:d6269d17c8cf	1183	{
jah128	20:1bc6c6dc477b	1184	setup_demo();
jah128	11:312663037b8c	1185	demo_timeout.attach_us(this,&Demo::obstacle_demo_cycle,1000);
jah128	0:d6269d17c8cf	1186	}
jah128	0:d6269d17c8cf	1187
jah128	16:50686c07ad07	1188	void Demo::start_colour_demo()
jah128	16:50686c07ad07	1189	{
jah128	20:1bc6c6dc477b	1190	setup_demo();
jah128	16:50686c07ad07	1191	colour.start_colour_ticker(100);
jah128	16:50686c07ad07	1192	demo_timeout.attach_us(this,&Demo::colour_demo_cycle,1000);
jah128	16:50686c07ad07	1193	}
jah128	16:50686c07ad07	1194
jah128	11:312663037b8c	1195	void Demo::start_stress_demo()
jah128	0:d6269d17c8cf	1196	{
jah128	0:d6269d17c8cf	1197	display.set_backlight_brightness(0.25);
jah128	0:d6269d17c8cf	1198	display.write_string("STRESS TEST");
jah128	0:d6269d17c8cf	1199	display.set_position(1,0);
jah128	0:d6269d17c8cf	1200	display.write_string("----25%----");
jah128	20:1bc6c6dc477b	1201	setup_demo();
jah128	0:d6269d17c8cf	1202	time_out = 0.04f;
jah128	11:312663037b8c	1203	demo_timeout.attach_us(this,&Demo::stress_demo_cycle,1000);
jah128	0:d6269d17c8cf	1204	}
jah128	0:d6269d17c8cf	1205
jah128	11:312663037b8c	1206	void Demo::start_spinning_demo()
jah128	0:d6269d17c8cf	1207	{
jah128	0:d6269d17c8cf	1208	display.set_backlight_brightness(0);
jah128	20:1bc6c6dc477b	1209	setup_demo();
jah128	0:d6269d17c8cf	1210	time_out = 0.0f;
jah128	11:312663037b8c	1211	demo_timeout.attach_us(this,&Demo::spinning_demo_cycle,1000);
jah128	0:d6269d17c8cf	1212	}
jah128	0:d6269d17c8cf	1213
jah128	11:312663037b8c	1214	void Demo::line_demo_cycle()
jah128	0:d6269d17c8cf	1215	{
jah128	0:d6269d17c8cf	1216	if(demo_timer.read() > time_out) {
jah128	10:e58323951c08	1217	sensors.store_line_position();
jah128	0:d6269d17c8cf	1218	if(line_found) {
jah128	0:d6269d17c8cf	1219	time_out = 0.01f;
jah128	0:d6269d17c8cf	1220	state = 0;
jah128	0:d6269d17c8cf	1221	// Get the position of the line.
jah128	0:d6269d17c8cf	1222	lf_current_pos_of_line = line_position;
jah128	0:d6269d17c8cf	1223	lf_proportional = lf_current_pos_of_line;
jah128	0:d6269d17c8cf	1224
jah128	0:d6269d17c8cf	1225	// Compute the derivative
jah128	0:d6269d17c8cf	1226	lf_derivative = lf_current_pos_of_line - lf_previous_pos_of_line;
jah128	0:d6269d17c8cf	1227
jah128	0:d6269d17c8cf	1228	// Compute the integral
jah128	0:d6269d17c8cf	1229	lf_integral += lf_proportional;
jah128	0:d6269d17c8cf	1230
jah128	0:d6269d17c8cf	1231	// Remember the last position.
jah128	0:d6269d17c8cf	1232	lf_previous_pos_of_line = lf_current_pos_of_line;
jah128	0:d6269d17c8cf	1233
jah128	0:d6269d17c8cf	1234	// Compute the power
jah128	0:d6269d17c8cf	1235	lf_power = (lf_proportional * (LF_P_TERM) ) + (lf_integral(LF_I_TERM)) + (lf_derivative(LF_D_TERM)) ;
jah128	0:d6269d17c8cf	1236
jah128	0:d6269d17c8cf	1237	// Compute new speeds
jah128	0:d6269d17c8cf	1238	lf_right = lf_speed-lf_power;
jah128	0:d6269d17c8cf	1239	lf_left = lf_speed+lf_power;
jah128	0:d6269d17c8cf	1240
jah128	0:d6269d17c8cf	1241	// limit checks
jah128	0:d6269d17c8cf	1242	if (lf_right < 0)
jah128	0:d6269d17c8cf	1243	lf_right = 0;
jah128	0:d6269d17c8cf	1244	else if (lf_right > 1.0f)
jah128	0:d6269d17c8cf	1245	lf_right = 1.0f;
jah128	0:d6269d17c8cf	1246
jah128	0:d6269d17c8cf	1247	if (lf_left < 0)
jah128	0:d6269d17c8cf	1248	lf_left = 0;
jah128	0:d6269d17c8cf	1249	else if (lf_left > 1.0f)
jah128	0:d6269d17c8cf	1250	lf_left = 1.0f;
jah128	15:66be5ec52c3b	1251	} else {
jah128	15:66be5ec52c3b	1252	//Cannot see line: hunt for it
jah128	15:66be5ec52c3b	1253	if(lf_left > lf_right) {
jah128	15:66be5ec52c3b	1254	//Currently turning left, keep turning left
jah128	15:66be5ec52c3b	1255	state ++;
jah128	15:66be5ec52c3b	1256	float d_step = state * 0.04;
jah128	15:66be5ec52c3b	1257	lf_left = 0.2 + d_step;
jah128	15:66be5ec52c3b	1258	lf_right = -0.2 - d_step;
jah128	15:66be5ec52c3b	1259	if(state > 20) {
jah128	15:66be5ec52c3b	1260	state = 0;
jah128	15:66be5ec52c3b	1261	lf_right = 0.2;
jah128	15:66be5ec52c3b	1262	lf_left = -0.2;
jah128	15:66be5ec52c3b	1263	time_out += 0.01f;
jah128	15:66be5ec52c3b	1264	if(time_out > 0.1f) demo_running = 0;
jah128	15:66be5ec52c3b	1265	}
jah128	15:66be5ec52c3b	1266	} else {
jah128	15:66be5ec52c3b	1267	//Currently turning right, keep turning right
jah128	15:66be5ec52c3b	1268	state ++;
jah128	15:66be5ec52c3b	1269	float d_step = state * 0.04;
jah128	15:66be5ec52c3b	1270	lf_left = -0.2 - d_step;
jah128	15:66be5ec52c3b	1271	lf_right = 0.2 + d_step;
jah128	15:66be5ec52c3b	1272	if(state > 20) {
jah128	15:66be5ec52c3b	1273	state = 0;
jah128	15:66be5ec52c3b	1274	lf_right = -0.2;
jah128	15:66be5ec52c3b	1275	lf_left = 0.2;
jah128	15:66be5ec52c3b	1276	time_out += 0.01f;
jah128	15:66be5ec52c3b	1277	if(time_out > 0.1f) demo_running = 0;
jah128	15:66be5ec52c3b	1278	}
jah128	0:d6269d17c8cf	1279	}
jah128	0:d6269d17c8cf	1280	}
jah128	0:d6269d17c8cf	1281	// set speed
jah128	8:6c92789d5f87	1282	motors.set_left_motor_speed(lf_left);
jah128	8:6c92789d5f87	1283	motors.set_right_motor_speed(lf_right);
jah128	0:d6269d17c8cf	1284
jah128	0:d6269d17c8cf	1285
jah128	0:d6269d17c8cf	1286	demo_timer.reset();
jah128	0:d6269d17c8cf	1287	}
jah128	11:312663037b8c	1288	if(demo_running == 1)demo_timeout.attach_us(this,&Demo::line_demo_cycle,500);
jah128	20:1bc6c6dc477b	1289	else stop_demo();
jah128	0:d6269d17c8cf	1290	}
jah128	0:d6269d17c8cf	1291
jah128	11:312663037b8c	1292	void Demo::stress_demo_cycle()
jah128	0:d6269d17c8cf	1293	{
jah128	0:d6269d17c8cf	1294	if(demo_timer.read() > time_out) {
jah128	0:d6269d17c8cf	1295	float pct = 0.25 + (0.25 * stress_step);
jah128	0:d6269d17c8cf	1296	switch(state) {
jah128	0:d6269d17c8cf	1297	case 0:
jah128	0:d6269d17c8cf	1298	if(spin_step % 2 == 0) {
jah128	8:6c92789d5f87	1299	motors.forward(pct);
jah128	9:dde9e21030eb	1300	led.set_leds(0xFF,0xFF);
jah128	0:d6269d17c8cf	1301	} else {
jah128	8:6c92789d5f87	1302	motors.backward(pct);
jah128	9:dde9e21030eb	1303	led.set_leds(0,0xFF);
jah128	0:d6269d17c8cf	1304	}
jah128	0:d6269d17c8cf	1305	spin_step ++;
jah128	0:d6269d17c8cf	1306	if(spin_step > 199) {
jah128	0:d6269d17c8cf	1307	state ++;
jah128	0:d6269d17c8cf	1308	spin_step = 0;
jah128	0:d6269d17c8cf	1309	}
jah128	0:d6269d17c8cf	1310	break;
jah128	0:d6269d17c8cf	1311	case 1:
jah128	0:d6269d17c8cf	1312	if(stress_step < 3) stress_step ++;
jah128	0:d6269d17c8cf	1313	float pct = 0.25 + (0.25 * stress_step);
jah128	0:d6269d17c8cf	1314	display.set_backlight_brightness(pct);
jah128	0:d6269d17c8cf	1315	display.set_position(1,0);
jah128	0:d6269d17c8cf	1316	switch(stress_step) {
jah128	0:d6269d17c8cf	1317	case 1:
jah128	0:d6269d17c8cf	1318	display.write_string("----50%----");
jah128	0:d6269d17c8cf	1319	break;
jah128	0:d6269d17c8cf	1320	case 2:
jah128	0:d6269d17c8cf	1321	display.write_string("----75%----");
jah128	0:d6269d17c8cf	1322	break;
jah128	0:d6269d17c8cf	1323	case 3:
jah128	0:d6269d17c8cf	1324	display.write_string("---100%----");
jah128	0:d6269d17c8cf	1325	break;
jah128	0:d6269d17c8cf	1326	}
jah128	0:d6269d17c8cf	1327	state = 0;
jah128	0:d6269d17c8cf	1328	break;
jah128	0:d6269d17c8cf	1329	}
jah128	0:d6269d17c8cf	1330	demo_timer.reset();
jah128	0:d6269d17c8cf	1331	}
jah128	11:312663037b8c	1332	if(demo_running == 1)demo_timeout.attach_us(this,&Demo::stress_demo_cycle,500);
jah128	20:1bc6c6dc477b	1333	else stop_demo();
jah128	20:1bc6c6dc477b	1334	}
jah128	20:1bc6c6dc477b	1335
jah128	20:1bc6c6dc477b	1336	void Demo::stop_demo()
jah128	20:1bc6c6dc477b	1337	{
jah128	20:1bc6c6dc477b	1338	demo_running = 0;
jah128	20:1bc6c6dc477b	1339	demo_timeout.detach();
jah128	20:1bc6c6dc477b	1340	motors.stop();
jah128	20:1bc6c6dc477b	1341	display.set_backlight_brightness(bl_brightness * 0.01f);
jah128	0:d6269d17c8cf	1342	}
jah128	0:d6269d17c8cf	1343
jah128	11:312663037b8c	1344	void Demo::spinning_demo_cycle()
jah128	0:d6269d17c8cf	1345	{
jah128	0:d6269d17c8cf	1346	if(demo_timer.read() > time_out) {
jah128	0:d6269d17c8cf	1347	switch(state) {
jah128	0:d6269d17c8cf	1348	case 0: //Robot is stopped
jah128	9:dde9e21030eb	1349	led.set_leds(0,0xFF);
jah128	9:dde9e21030eb	1350	led.set_center_led(1,1);
jah128	0:d6269d17c8cf	1351	speed = 0.1f;
jah128	8:6c92789d5f87	1352	motors.brake();
jah128	0:d6269d17c8cf	1353	time_out = 0.5;
jah128	0:d6269d17c8cf	1354	state = 1;
jah128	0:d6269d17c8cf	1355	led_step = 0;
jah128	0:d6269d17c8cf	1356	break;
jah128	0:d6269d17c8cf	1357	case 1: //Motor is turning right, accelerating
jah128	0:d6269d17c8cf	1358	time_out = 0.1;
jah128	9:dde9e21030eb	1359	led.set_center_led(2,1);
jah128	0:d6269d17c8cf	1360	switch(led_step) {
jah128	0:d6269d17c8cf	1361	case 0:
jah128	9:dde9e21030eb	1362	led.set_leds(0x01,0);
jah128	0:d6269d17c8cf	1363	break;
jah128	0:d6269d17c8cf	1364	case 1:
jah128	9:dde9e21030eb	1365	led.set_leds(0x02,0);
jah128	0:d6269d17c8cf	1366	break;
jah128	0:d6269d17c8cf	1367	case 2:
jah128	9:dde9e21030eb	1368	led.set_leds(0x04,0);
jah128	0:d6269d17c8cf	1369	break;
jah128	0:d6269d17c8cf	1370	case 3:
jah128	9:dde9e21030eb	1371	led.set_leds(0x08,0);
jah128	0:d6269d17c8cf	1372	break;
jah128	0:d6269d17c8cf	1373	case 4:
jah128	9:dde9e21030eb	1374	led.set_leds(0x10,0);
jah128	0:d6269d17c8cf	1375	break;
jah128	0:d6269d17c8cf	1376	case 5:
jah128	9:dde9e21030eb	1377	led.set_leds(0x20,0);
jah128	0:d6269d17c8cf	1378	break;
jah128	0:d6269d17c8cf	1379	case 6:
jah128	9:dde9e21030eb	1380	led.set_leds(0x40,0);
jah128	0:d6269d17c8cf	1381	break;
jah128	0:d6269d17c8cf	1382	case 7:
jah128	9:dde9e21030eb	1383	led.set_leds(0x80,0);
jah128	0:d6269d17c8cf	1384	break;
jah128	0:d6269d17c8cf	1385	}
jah128	0:d6269d17c8cf	1386	led_step ++;
jah128	0:d6269d17c8cf	1387	if(led_step == 8) led_step =0;
jah128	0:d6269d17c8cf	1388	if(speed < 1) {
jah128	0:d6269d17c8cf	1389	speed += 0.0125;
jah128	8:6c92789d5f87	1390	motors.turn(speed);
jah128	0:d6269d17c8cf	1391	} else {
jah128	0:d6269d17c8cf	1392	state = 2;
jah128	0:d6269d17c8cf	1393	spin_step = 0;
jah128	0:d6269d17c8cf	1394	led_step =0;
jah128	0:d6269d17c8cf	1395	}
jah128	0:d6269d17c8cf	1396	break;
jah128	0:d6269d17c8cf	1397	case 2: //Motor is turning right, full speed
jah128	9:dde9e21030eb	1398	led.set_center_led(3,1);
jah128	0:d6269d17c8cf	1399	switch(led_step) {
jah128	0:d6269d17c8cf	1400	case 0:
jah128	9:dde9e21030eb	1401	led.set_leds(0x33,0x33);
jah128	0:d6269d17c8cf	1402	break;
jah128	0:d6269d17c8cf	1403	case 1:
jah128	9:dde9e21030eb	1404	led.set_leds(0x66,0x66);
jah128	0:d6269d17c8cf	1405	break;
jah128	0:d6269d17c8cf	1406	case 2:
jah128	9:dde9e21030eb	1407	led.set_leds(0xCC,0xCC);
jah128	0:d6269d17c8cf	1408	break;
jah128	0:d6269d17c8cf	1409	case 3:
jah128	9:dde9e21030eb	1410	led.set_leds(0x99,0x99);
jah128	0:d6269d17c8cf	1411	break;
jah128	0:d6269d17c8cf	1412	}
jah128	0:d6269d17c8cf	1413	led_step ++;
jah128	0:d6269d17c8cf	1414	if(led_step == 4) led_step = 0;
jah128	0:d6269d17c8cf	1415	spin_step ++;
jah128	0:d6269d17c8cf	1416	if(spin_step == 40) {
jah128	0:d6269d17c8cf	1417	state = 3;
jah128	0:d6269d17c8cf	1418	led_step = 0;
jah128	0:d6269d17c8cf	1419	}
jah128	0:d6269d17c8cf	1420	break;
jah128	0:d6269d17c8cf	1421	case 3: //Motor is turning right, decelerating
jah128	9:dde9e21030eb	1422	led.set_center_led(2,1);
jah128	0:d6269d17c8cf	1423	switch(led_step) {
jah128	0:d6269d17c8cf	1424	case 0:
jah128	9:dde9e21030eb	1425	led.set_leds(0x01,0);
jah128	0:d6269d17c8cf	1426	break;
jah128	0:d6269d17c8cf	1427	case 1:
jah128	9:dde9e21030eb	1428	led.set_leds(0x02,0);
jah128	0:d6269d17c8cf	1429	break;
jah128	0:d6269d17c8cf	1430	case 2:
jah128	9:dde9e21030eb	1431	led.set_leds(0x04,0);
jah128	0:d6269d17c8cf	1432	break;
jah128	0:d6269d17c8cf	1433	case 3:
jah128	9:dde9e21030eb	1434	led.set_leds(0x08,0);
jah128	0:d6269d17c8cf	1435	break;
jah128	0:d6269d17c8cf	1436	case 4:
jah128	9:dde9e21030eb	1437	led.set_leds(0x10,0);
jah128	0:d6269d17c8cf	1438	break;
jah128	0:d6269d17c8cf	1439	case 5:
jah128	9:dde9e21030eb	1440	led.set_leds(0x20,0);
jah128	0:d6269d17c8cf	1441	break;
jah128	0:d6269d17c8cf	1442	case 6:
jah128	9:dde9e21030eb	1443	led.set_leds(0x40,0);
jah128	0:d6269d17c8cf	1444	break;
jah128	0:d6269d17c8cf	1445	case 7:
jah128	9:dde9e21030eb	1446	led.set_leds(0x80,0);
jah128	0:d6269d17c8cf	1447	break;
jah128	0:d6269d17c8cf	1448	}
jah128	0:d6269d17c8cf	1449	if(led_step == 0) led_step =8;
jah128	0:d6269d17c8cf	1450	led_step --;
jah128	0:d6269d17c8cf	1451	if(speed > 0.1) {
jah128	0:d6269d17c8cf	1452	speed -= 0.025;
jah128	8:6c92789d5f87	1453	motors.turn(speed);
jah128	0:d6269d17c8cf	1454	} else {
jah128	0:d6269d17c8cf	1455	state = 4;
jah128	0:d6269d17c8cf	1456	spin_step = 0;
jah128	0:d6269d17c8cf	1457	led_step =0;
jah128	0:d6269d17c8cf	1458	}
jah128	0:d6269d17c8cf	1459	break;
jah128	0:d6269d17c8cf	1460	case 4: //Robot is stopped
jah128	9:dde9e21030eb	1461	led.set_leds(0,0xFF);
jah128	9:dde9e21030eb	1462	led.set_center_led(1,1);
jah128	0:d6269d17c8cf	1463	speed = 0.1f;
jah128	8:6c92789d5f87	1464	motors.brake();
jah128	0:d6269d17c8cf	1465	time_out = 0.5;
jah128	0:d6269d17c8cf	1466	led_step =0;
jah128	0:d6269d17c8cf	1467	state = 5;
jah128	0:d6269d17c8cf	1468	break;
jah128	0:d6269d17c8cf	1469	case 5: //Motor is turning left, accelerating
jah128	0:d6269d17c8cf	1470	time_out = 0.1;
jah128	9:dde9e21030eb	1471	led.set_center_led(2,1);
jah128	0:d6269d17c8cf	1472	switch(led_step) {
jah128	0:d6269d17c8cf	1473	case 0:
jah128	9:dde9e21030eb	1474	led.set_leds(0x01,0);
jah128	0:d6269d17c8cf	1475	break;
jah128	0:d6269d17c8cf	1476	case 1:
jah128	9:dde9e21030eb	1477	led.set_leds(0x02,0);
jah128	0:d6269d17c8cf	1478	break;
jah128	0:d6269d17c8cf	1479	case 2:
jah128	9:dde9e21030eb	1480	led.set_leds(0x04,0);
jah128	0:d6269d17c8cf	1481	break;
jah128	0:d6269d17c8cf	1482	case 3:
jah128	9:dde9e21030eb	1483	led.set_leds(0x08,0);
jah128	0:d6269d17c8cf	1484	break;
jah128	0:d6269d17c8cf	1485	case 4:
jah128	9:dde9e21030eb	1486	led.set_leds(0x10,0);
jah128	0:d6269d17c8cf	1487	break;
jah128	0:d6269d17c8cf	1488	case 5:
jah128	9:dde9e21030eb	1489	led.set_leds(0x20,0);
jah128	0:d6269d17c8cf	1490	break;
jah128	0:d6269d17c8cf	1491	case 6:
jah128	9:dde9e21030eb	1492	led.set_leds(0x40,0);
jah128	0:d6269d17c8cf	1493	break;
jah128	0:d6269d17c8cf	1494	case 7:
jah128	9:dde9e21030eb	1495	led.set_leds(0x80,0);
jah128	0:d6269d17c8cf	1496	break;
jah128	0:d6269d17c8cf	1497	}
jah128	0:d6269d17c8cf	1498	if(led_step == 0) led_step =8;
jah128	0:d6269d17c8cf	1499	led_step --;
jah128	0:d6269d17c8cf	1500	if(speed < 1) {
jah128	0:d6269d17c8cf	1501	speed += 0.0125;
jah128	8:6c92789d5f87	1502	motors.turn(-speed);
jah128	0:d6269d17c8cf	1503	} else {
jah128	0:d6269d17c8cf	1504	state = 6;
jah128	0:d6269d17c8cf	1505	spin_step = 0;
jah128	0:d6269d17c8cf	1506	led_step = 0;
jah128	0:d6269d17c8cf	1507	}
jah128	0:d6269d17c8cf	1508	break;
jah128	0:d6269d17c8cf	1509	case 6: //Motor is turning left, full speed
jah128	9:dde9e21030eb	1510	led.set_center_led(3,1);
jah128	0:d6269d17c8cf	1511	switch(led_step) {
jah128	0:d6269d17c8cf	1512	case 0:
jah128	9:dde9e21030eb	1513	led.set_leds(0x33,0x33);
jah128	0:d6269d17c8cf	1514	break;
jah128	0:d6269d17c8cf	1515	case 1:
jah128	9:dde9e21030eb	1516	led.set_leds(0x66,0x66);
jah128	0:d6269d17c8cf	1517	break;
jah128	0:d6269d17c8cf	1518	case 2:
jah128	9:dde9e21030eb	1519	led.set_leds(0xCC,0xCC);
jah128	0:d6269d17c8cf	1520	break;
jah128	0:d6269d17c8cf	1521	case 3:
jah128	9:dde9e21030eb	1522	led.set_leds(0x99,0x99);
jah128	0:d6269d17c8cf	1523	break;
jah128	0:d6269d17c8cf	1524	}
jah128	0:d6269d17c8cf	1525	if(led_step == 0) led_step = 4;
jah128	0:d6269d17c8cf	1526	led_step --;
jah128	0:d6269d17c8cf	1527	spin_step ++;
jah128	0:d6269d17c8cf	1528	if(spin_step == 40) {
jah128	0:d6269d17c8cf	1529	state = 7;
jah128	0:d6269d17c8cf	1530	led_step = 0;
jah128	0:d6269d17c8cf	1531	}
jah128	0:d6269d17c8cf	1532	break;
jah128	0:d6269d17c8cf	1533	case 7: //Motor is turning left, decelerating
jah128	9:dde9e21030eb	1534	led.set_center_led(2,1);
jah128	0:d6269d17c8cf	1535	switch(led_step) {
jah128	0:d6269d17c8cf	1536	case 0:
jah128	9:dde9e21030eb	1537	led.set_leds(0x01,0);
jah128	0:d6269d17c8cf	1538	break;
jah128	0:d6269d17c8cf	1539	case 1:
jah128	9:dde9e21030eb	1540	led.set_leds(0x02,0);
jah128	0:d6269d17c8cf	1541	break;
jah128	0:d6269d17c8cf	1542	case 2:
jah128	9:dde9e21030eb	1543	led.set_leds(0x04,0);
jah128	0:d6269d17c8cf	1544	break;
jah128	0:d6269d17c8cf	1545	case 3:
jah128	9:dde9e21030eb	1546	led.set_leds(0x08,0);
jah128	0:d6269d17c8cf	1547	break;
jah128	0:d6269d17c8cf	1548	case 4:
jah128	9:dde9e21030eb	1549	led.set_leds(0x10,0);
jah128	0:d6269d17c8cf	1550	break;
jah128	0:d6269d17c8cf	1551	case 5:
jah128	9:dde9e21030eb	1552	led.set_leds(0x20,0);
jah128	0:d6269d17c8cf	1553	break;
jah128	0:d6269d17c8cf	1554	case 6:
jah128	9:dde9e21030eb	1555	led.set_leds(0x40,0);
jah128	0:d6269d17c8cf	1556	break;
jah128	0:d6269d17c8cf	1557	case 7:
jah128	9:dde9e21030eb	1558	led.set_leds(0x80,0);
jah128	0:d6269d17c8cf	1559	break;
jah128	0:d6269d17c8cf	1560	}
jah128	0:d6269d17c8cf	1561	led_step ++;
jah128	0:d6269d17c8cf	1562	if(led_step == 8) led_step =0;
jah128	0:d6269d17c8cf	1563	if(speed > 0.1) {
jah128	0:d6269d17c8cf	1564	speed -= 0.025;
jah128	8:6c92789d5f87	1565	motors.turn(-speed);
jah128	0:d6269d17c8cf	1566	} else {
jah128	0:d6269d17c8cf	1567	state = 0;
jah128	0:d6269d17c8cf	1568	spin_step = 0;
jah128	0:d6269d17c8cf	1569	led_step = 0;
jah128	0:d6269d17c8cf	1570	}
jah128	0:d6269d17c8cf	1571	break;
jah128	0:d6269d17c8cf	1572	}
jah128	0:d6269d17c8cf	1573	demo_timer.reset();
jah128	0:d6269d17c8cf	1574	}
jah128	11:312663037b8c	1575	if(demo_running == 1)demo_timeout.attach_us(this,&Demo::spinning_demo_cycle,500);
jah128	20:1bc6c6dc477b	1576	else stop_demo();
jah128	0:d6269d17c8cf	1577	}
jah128	0:d6269d17c8cf	1578
jah128	16:50686c07ad07	1579	void Demo::colour_demo_cycle()
jah128	16:50686c07ad07	1580	{
jah128	16:50686c07ad07	1581	if(demo_timer.read() > time_out) {
jah128	16:50686c07ad07	1582	int col = colour.get_detected_colour();
jah128	18:9204f74069b4	1583	switch(col) {
jah128	18:9204f74069b4	1584	case 0:
jah128	18:9204f74069b4	1585	led.set_leds(0,0xFF);
jah128	18:9204f74069b4	1586	led.set_center_led(1,1);
jah128	18:9204f74069b4	1587	break;
jah128	18:9204f74069b4	1588	case 1:
jah128	18:9204f74069b4	1589	led.set_leds(0xFF,0xFF);
jah128	18:9204f74069b4	1590	led.set_center_led(3,1);
jah128	18:9204f74069b4	1591	break;
jah128	18:9204f74069b4	1592	case 2:
jah128	18:9204f74069b4	1593	led.set_leds(0xFF,0);
jah128	18:9204f74069b4	1594	led.set_center_led(2,1);
jah128	18:9204f74069b4	1595	break;
jah128	18:9204f74069b4	1596	default:
jah128	18:9204f74069b4	1597	led.set_leds(0,0);
jah128	18:9204f74069b4	1598	led.set_center_led(0,0);
jah128	18:9204f74069b4	1599	break;
jah128	16:50686c07ad07	1600	}
jah128	16:50686c07ad07	1601	switch(state) {
jah128	16:50686c07ad07	1602	case 0: //Robot is stopped
jah128	16:50686c07ad07	1603	speed = 0.2f;
jah128	16:50686c07ad07	1604	motors.forward(speed);
jah128	16:50686c07ad07	1605	time_out = 0.05;
jah128	16:50686c07ad07	1606	state = 1;
jah128	16:50686c07ad07	1607	break;
jah128	16:50686c07ad07	1608	case 1: { //Motor is moving forward
jah128	16:50686c07ad07	1609	sensors.store_ir_values();
jah128	16:50686c07ad07	1610	int front_right = sensors.read_illuminated_raw_ir_value(0);
jah128	16:50686c07ad07	1611	int front_left = sensors.read_illuminated_raw_ir_value(7);
jah128	16:50686c07ad07	1612	if(front_left > 400 \|\| front_right > 400) {
jah128	16:50686c07ad07	1613	motors.brake();
jah128	16:50686c07ad07	1614	time_out = 0.04;
jah128	16:50686c07ad07	1615	if(front_left > front_right)state=2;
jah128	16:50686c07ad07	1616	else state=3;
jah128	16:50686c07ad07	1617	} else {
jah128	16:50686c07ad07	1618	if(speed < 0.5) {
jah128	16:50686c07ad07	1619	speed += 0.03;
jah128	16:50686c07ad07	1620	motors.forward(speed);
jah128	16:50686c07ad07	1621	}
jah128	16:50686c07ad07	1622	}
jah128	16:50686c07ad07	1623	break;
jah128	16:50686c07ad07	1624	}
jah128	16:50686c07ad07	1625	case 2: //Turn right
jah128	16:50686c07ad07	1626	motors.set_left_motor_speed(0.35);
jah128	16:50686c07ad07	1627	motors.set_right_motor_speed(-0.35);
jah128	16:50686c07ad07	1628	time_out = 0.5;
jah128	16:50686c07ad07	1629	state = 0;
jah128	16:50686c07ad07	1630	break;
jah128	16:50686c07ad07	1631	case 3: //Turn left
jah128	16:50686c07ad07	1632	motors.set_left_motor_speed(-0.35);
jah128	16:50686c07ad07	1633	motors.set_right_motor_speed(0.35);
jah128	16:50686c07ad07	1634	time_out = 0.5;
jah128	16:50686c07ad07	1635	state = 0;
jah128	16:50686c07ad07	1636	break;
jah128	16:50686c07ad07	1637	}
jah128	16:50686c07ad07	1638	demo_timer.reset();
jah128	16:50686c07ad07	1639	}
jah128	16:50686c07ad07	1640	if(demo_running == 1)demo_timeout.attach_us(this,&Demo::colour_demo_cycle,200);
jah128	20:1bc6c6dc477b	1641	else stop_demo();
jah128	16:50686c07ad07	1642	}
jah128	16:50686c07ad07	1643
jah128	16:50686c07ad07	1644
jah128	11:312663037b8c	1645	void Demo::obstacle_demo_cycle()
jah128	0:d6269d17c8cf	1646	{
jah128	18:9204f74069b4	1647
jah128	0:d6269d17c8cf	1648	if(demo_timer.read() > time_out) {
jah128	0:d6269d17c8cf	1649	switch(state) {
jah128	0:d6269d17c8cf	1650	case 0: //Robot is stopped
jah128	9:dde9e21030eb	1651	led.set_leds(0,0xFF);
jah128	9:dde9e21030eb	1652	led.set_center_led(1,0.4);
jah128	0:d6269d17c8cf	1653	speed = 0.3f;
jah128	8:6c92789d5f87	1654	motors.forward(speed);
jah128	0:d6269d17c8cf	1655	time_out = 0.05;
jah128	0:d6269d17c8cf	1656	state = 1;
jah128	0:d6269d17c8cf	1657	break;
jah128	0:d6269d17c8cf	1658	case 1: { //Motor is moving forward
jah128	10:e58323951c08	1659	sensors.store_ir_values();
jah128	10:e58323951c08	1660	int front_right = sensors.read_illuminated_raw_ir_value(0);
jah128	10:e58323951c08	1661	int front_left = sensors.read_illuminated_raw_ir_value(7);
jah128	0:d6269d17c8cf	1662	if(front_left > 400 \|\| front_right > 400) {
jah128	8:6c92789d5f87	1663	motors.brake();
jah128	0:d6269d17c8cf	1664	time_out = 0.04;
jah128	0:d6269d17c8cf	1665	if(front_left > front_right)state=2;
jah128	0:d6269d17c8cf	1666	else state=3;
jah128	0:d6269d17c8cf	1667	} else {
jah128	0:d6269d17c8cf	1668	if(speed < 0.5) {
jah128	0:d6269d17c8cf	1669	speed += 0.03;
jah128	8:6c92789d5f87	1670	motors.forward(speed);
jah128	0:d6269d17c8cf	1671	}
jah128	0:d6269d17c8cf	1672	switch(led_step) {
jah128	0:d6269d17c8cf	1673	case 0:
jah128	9:dde9e21030eb	1674	led.set_leds(0x01,0);
jah128	0:d6269d17c8cf	1675	break;
jah128	0:d6269d17c8cf	1676	case 1:
jah128	9:dde9e21030eb	1677	led.set_leds(0x38,0);
jah128	0:d6269d17c8cf	1678	break;
jah128	0:d6269d17c8cf	1679	case 2:
jah128	9:dde9e21030eb	1680	led.set_leds(0x6C,0);
jah128	0:d6269d17c8cf	1681	break;
jah128	0:d6269d17c8cf	1682	case 3:
jah128	9:dde9e21030eb	1683	led.set_leds(0xC6,0);
jah128	0:d6269d17c8cf	1684	break;
jah128	0:d6269d17c8cf	1685	case 4:
jah128	9:dde9e21030eb	1686	led.set_leds(0x83,0);
jah128	0:d6269d17c8cf	1687	break;
jah128	0:d6269d17c8cf	1688	}
jah128	9:dde9e21030eb	1689	led.set_center_led(2, 0.6);
jah128	0:d6269d17c8cf	1690	led_step ++;
jah128	0:d6269d17c8cf	1691	if(led_step == 5) led_step = 0;
jah128	0:d6269d17c8cf	1692	}
jah128	0:d6269d17c8cf	1693	break;
jah128	0:d6269d17c8cf	1694	}
jah128	0:d6269d17c8cf	1695	case 2: //Turn right
jah128	8:6c92789d5f87	1696	motors.set_left_motor_speed(0.85);
jah128	8:6c92789d5f87	1697	motors.set_right_motor_speed(-0.85);
jah128	0:d6269d17c8cf	1698	time_out = 0.4;
jah128	0:d6269d17c8cf	1699	state = 0;
jah128	9:dde9e21030eb	1700	led.set_leds(0x0E,0x0E);
jah128	9:dde9e21030eb	1701	led.set_center_led(3,0.5);
jah128	0:d6269d17c8cf	1702	break;
jah128	0:d6269d17c8cf	1703	case 3: //Turn left
jah128	8:6c92789d5f87	1704	motors.set_left_motor_speed(-0.85);
jah128	8:6c92789d5f87	1705	motors.set_right_motor_speed(0.85);
jah128	0:d6269d17c8cf	1706	time_out = 0.4;
jah128	0:d6269d17c8cf	1707	state = 0;
jah128	9:dde9e21030eb	1708	led.set_leds(0xE0,0xE0);
jah128	9:dde9e21030eb	1709	led.set_center_led(3,0.5);
jah128	0:d6269d17c8cf	1710	break;
jah128	0:d6269d17c8cf	1711	}
jah128	0:d6269d17c8cf	1712	demo_timer.reset();
jah128	0:d6269d17c8cf	1713	}
jah128	11:312663037b8c	1714	if(demo_running == 1)demo_timeout.attach_us(this,&Demo::obstacle_demo_cycle,200);
jah128	20:1bc6c6dc477b	1715	else stop_demo();
jah128	0:d6269d17c8cf	1716	}
jah128	0:d6269d17c8cf	1717
jah128	16:50686c07ad07	1718
jah128	11:312663037b8c	1719	void Demo::demo_update_leds()
jah128	0:d6269d17c8cf	1720	{
jah128	0:d6269d17c8cf	1721	char red = 0;
jah128	0:d6269d17c8cf	1722	char green = 0;
jah128	0:d6269d17c8cf	1723	for(int i=0; i<8; i++) {
jah128	0:d6269d17c8cf	1724	if(led_state[i]==1 \|\| led_state[i]==3) red+=(1<<i);
jah128	0:d6269d17c8cf	1725	if(led_state[i]==2 \|\| led_state[i]==3) green+=(1<<i);
jah128	0:d6269d17c8cf	1726	}
jah128	9:dde9e21030eb	1727	led.set_leds(green,red);
jah128	0:d6269d17c8cf	1728	float brightness_f = brightness / 100.0f;
jah128	9:dde9e21030eb	1729	led.set_center_led(led_state[8], brightness_f);
jah128	9:dde9e21030eb	1730	led.set_base_led(base_led_state);
jah128	0:d6269d17c8cf	1731	}

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	14 May 2018
Imports:	0
Forks:	0
Commits:	21
Dependents:	0
Dependencies:	0
Followers:	1

demo.cpp@20:1bc6c6dc477b, 2018-05-14 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning