PsiSwarm Library  0.8
demo.cpp
1 /* University of York Robotics Laboratory PsiSwarm Library: Demo Mode Source File
2  *
3  * Copyright 2016 University of York
4  *
5  * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
7  * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
8  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
9  * See the License for the specific language governing permissions and limitations under the License.
10  *
11  * File: demo.cpp
12  *
13  * (C) Dept. Electronics & Computer Science, University of York
14  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
15  *
16  * PsiSwarm Library Version: 0.8
17  *
18  * Version 0.8 : Rewritten as OO\C++
19  * Version 0.7 : Updated for new MBED API
20  * Version 0.5 : Added motor calibration menu
21  * Version 0.4 : Added PsiSwarmBasic menu
22  * Version 0.2 : Remove most the functionality from center-button push to allow all operations to be accessable from
23  * four directions alone.
24  * Added extra sensor information, added various testing demos
25  *
26  *
27  * October 2016
28  *
29  */
30 
31 
32 #include "psiswarm.h"
33 
34 // PID terms
35 #define LF_P_TERM 0.2
36 #define LF_I_TERM 0
37 #define LF_D_TERM 4
38 
39 char top_menu = 0;
40 char sub_menu = 0;
41 char level = 0;
42 char started = 0;
43 char topline[17];
44 char bottomline[17];
45 char led_state[9];
46 char all_led_state = 0;
47 char base_led_state = 0;
48 char brightness = 20;
49 char bl_brightness = 100;
50 char base_ir_index = 0;
51 char side_ir_index = 0;
52 signed short left_speed = 0;
53 signed short right_speed = 0;
54 char both_motor_mode = 0;
55 char last_switch_pressed;
56 Timeout demo_event;
57 char handling_event = 0;
58 
59 Timeout demo_timeout;
60 char demo_running = 0;
61 Timer demo_timer;
62 float time_out;
63 float speed;
64 char state;
65 char led_step = 0;
66 char spin_step = 0;
67 char stress_step = 0;
68 
69 
70 float lf_right;
71 float lf_left;
72 float lf_current_pos_of_line = 0.0;
73 float lf_previous_pos_of_line = 0.0;
74 float lf_derivative,lf_proportional,lf_integral = 0;
75 float lf_power;
76 float lf_speed = 0.4;
77 
78 
79 
80 void Demo::start_demo_mode()
81 {
82  psi.debug("- Starting Demo Mode\n");
83  if(use_flash_basic == 1) top_menu = 7;
84  demo_on = 1;
85  display.set_backlight_brightness(bl_brightness * 0.01f);
86  display.clear_display();
87  display.write_string("PSI SWARM SYSTEM");
88  display.set_position(1,0);
89  display.write_string(" DEMO MODE");
90  wait(0.5);
91  display.clear_display();
92  display.write_string("Use cursor to");
93  display.set_position(1,0);
94  display.write_string("navigate menus");
95  char step = 0;
96  while(demo_on) {
97  if(demo_running == 0) {
98  switch(step) {
99  case 0:
100  mbed_led1 = 1;
101  mbed_led2 = 0;
102  break;
103  case 1:
104  mbed_led2 = 1;
105  mbed_led1 = 0;
106  break;
107  }
108  step++;
109  if(step==2)step=0;
110  } else {
111  mbed_led1 = 0;
112  mbed_led2 = 0;
113  mbed_led3 = 0;
114  mbed_led4 = 0;
115  }
116  wait(0.5);
117  }
118  psi.debug("- Demo mode ended\n");
119 }
120 
121 void Demo::demo_handle_switch_event(char switch_pressed)
122 {
123  if(!handling_event) {
124  handling_event = 1;
125  last_switch_pressed = switch_pressed;
126  demo_event.attach_us(this,&Demo::demo_event_thread, 1000);
127  }
128 }
129 
130 void Demo::demo_event_thread()
131 {
132  handling_event = 0;
133  if(started == 1) {
134  switch(last_switch_pressed) {
135  case 1: //Up pressed
136  switch(level) {
137  case 0:
138  if(top_menu != 8) {
139  level++;
140  sub_menu = 0;
141  } else {
142  demo_on = 0;
143  user_code_running = user_code_restore_mode;
144  }
145  break;
146  case 1:
147  switch(top_menu) {
148  case 7: // PsiBasic Menu
149  if(sub_menu == psi_basic_file_count) level = 0;
150  break;
151  case 0: //LED Menu
152  if(sub_menu < 9) {
153  if(led_state[sub_menu] == 0) led_state[sub_menu] = 3;
154  else led_state[sub_menu]--;
155  demo_update_leds();
156  }
157  if(sub_menu == 9) {
158  if(all_led_state == 0) all_led_state = 3;
159  else all_led_state--;
160  for(int i=0; i<9; i++) {
161  led_state[i]=all_led_state;
162  }
163  demo_update_leds();
164  }
165  if(sub_menu == 10) {
166  base_led_state = 1 - base_led_state;
167  demo_update_leds();
168  }
169  if(sub_menu == 11) {
170  switch(brightness) {
171  case 100:
172  brightness = 50;
173  break;
174  case 2:
175  brightness = 1;
176  break;
177  case 5:
178  brightness = 2;
179  break;
180  case 10:
181  brightness = 5;
182  break;
183  case 20:
184  brightness = 10;
185  break;
186  case 50:
187  brightness = 20;
188  break;
189  }
190  demo_update_leds();
191  }
192  if(sub_menu == 12) {
193  if(bl_brightness > 0) bl_brightness-=10;
194  display.set_backlight_brightness(bl_brightness * 0.01f);
195  }
196  if(sub_menu == 13) level = 0;
197  break;
198  case 1: // Sensors Menu
199  if(sub_menu == 4 || sub_menu == 5) {
200  if(base_ir_index == 0) base_ir_index = 4;
201  else base_ir_index --;
202  }
203  if(sub_menu > 5 && sub_menu < 9) {
204  if(side_ir_index == 0) side_ir_index = 7;
205  else side_ir_index --;
206  }
207  if(sub_menu == 11) level = 0;
208  break;
209  case 2: // Motor Menu
210  if(sub_menu == 0) {
211  left_speed += 5;
212  if(left_speed > 100) left_speed = 100;
213  motors.set_left_motor_speed(left_speed / 100.0f);
214  }
215  if(sub_menu == 1) {
216  right_speed += 5;
217  if(right_speed > 100) right_speed = 100;
218  motors.set_right_motor_speed(right_speed / 100.0f);
219  }
220  if(sub_menu == 2) {
221  if(both_motor_mode == 0) both_motor_mode=5;
222  else both_motor_mode--;
223  switch(both_motor_mode) {
224  case 0:
225  motors.stop();
226  break;
227  case 1:
228  motors.brake();
229  break;
230  case 2:
231  motors.forward(0.5);
232  break;
233  case 3:
234  motors.forward(1);
235  break;
236  case 4:
237  motors.backward(0.5);
238  break;
239  case 5:
240  motors.backward(1.0);
241  break;
242  }
243  }
244  if(sub_menu == 3) {
245  level = 0;
246  }
247  break;
248  case 3: // Radio Menu
249  if(sub_menu == 0) level = 0;
250  break;
251  case 4: // Info Menu
252  if(sub_menu == 6) level = 0;
253  break;
254  case 5: // Demo Menu
255  if(sub_menu == 0) {
256  if(demo_running == 0) {
257  start_line_demo();
258  } else {
259  demo_running = 0;
260  display.set_backlight_brightness(bl_brightness * 0.01f);
261  motors.stop();
262  }
263  }
264  if(sub_menu == 1) {
265  if(demo_running == 0) {
266  start_obstacle_demo();
267  } else {
268  demo_running = 0;
269  display.set_backlight_brightness(bl_brightness * 0.01f);
270  motors.stop();
271  }
272  }
273  if(sub_menu == 2) {
274  if(demo_running == 0) {
275  start_spinning_demo();
276  } else {
277  demo_running = 0;
278  display.set_backlight_brightness(bl_brightness * 0.01f);
279  motors.stop();
280  }
281  }
282  if(sub_menu == 3) {
283  if(demo_running == 0) {
284  start_stress_demo();
285  } else {
286  demo_running = 0;
287  display.set_backlight_brightness(bl_brightness * 0.01f);
288  motors.stop();
289  }
290  }
291  if(sub_menu == 4) level = 0;
292  break;
293  }
294  break;
295  }
296  break;
297  case 2: //Down pressed
298  switch(level) {
299  case 0:
300  if(top_menu != 8) {
301  level++;
302  sub_menu = 0;
303  } else {
304  demo_on = 0;
305  user_code_running = user_code_restore_mode;
306  }
307  break;
308  case 1:
309  switch(top_menu) {
310  case 7: // PsiBasic Menu
311  if(sub_menu == psi_basic_file_count) level = 0;
312  break;
313  case 0: //LED Menu
314  if(sub_menu < 9) {
315  led_state[sub_menu]++;
316  if(led_state[sub_menu] == 4) led_state[sub_menu] = 0;
317  demo_update_leds();
318  }
319  if(sub_menu == 9) {
320  all_led_state++;
321  if(all_led_state == 4) all_led_state = 0;
322  for(int i=0; i<9; i++) {
323  led_state[i]=all_led_state;
324  }
325  demo_update_leds();
326  }
327  if(sub_menu == 10) {
328  base_led_state = 1 - base_led_state;
329  demo_update_leds();
330  }
331  if(sub_menu == 11) {
332  switch(brightness) {
333  case 1:
334  brightness = 2;
335  break;
336  case 2:
337  brightness = 5;
338  break;
339  case 5:
340  brightness = 10;
341  break;
342  case 10:
343  brightness = 20;
344  break;
345  case 20:
346  brightness = 50;
347  break;
348  case 50:
349  brightness = 100;
350  break;
351  }
352  demo_update_leds();
353  }
354 
355  if(sub_menu == 12) {
356  if(bl_brightness < 100) bl_brightness+=10;
357  display.set_backlight_brightness(bl_brightness * 0.01f);
358  }
359  if(sub_menu == 13) level = 0;
360 
361  break;
362  case 1: // Sensors Menu
363  if(sub_menu == 4 || sub_menu == 5) {
364  if(base_ir_index == 4) base_ir_index = 0;
365  else base_ir_index ++;
366  }
367  if(sub_menu > 5 && sub_menu < 9) {
368  if(side_ir_index == 7) side_ir_index = 0;
369  else side_ir_index ++;
370  }
371  if(sub_menu == 11) level = 0;
372  break;
373  case 2: // Motor Menu
374  if(sub_menu == 0) {
375  left_speed -= 5;
376  if(left_speed < -100) left_speed = -100;
377  motors.set_left_motor_speed(left_speed / 100.0f);
378  }
379  if(sub_menu == 1) {
380  right_speed -= 5;
381  if(right_speed < -100) right_speed = -100;
382  motors.set_right_motor_speed(right_speed / 100.0f);
383  }
384  if(sub_menu == 2) {
385  both_motor_mode++;
386  if(both_motor_mode == 6) both_motor_mode=0;
387  switch(both_motor_mode) {
388  case 0:
389  motors.stop();
390  break;
391  case 1:
392  motors.brake();
393  break;
394  case 2:
395  motors.forward(0.5);
396  break;
397  case 3:
398  motors.forward(1);
399  break;
400  case 4:
401  motors.backward(0.5);
402  break;
403  case 5:
404  motors.backward(1.0);
405  break;
406  }
407  }
408  if(sub_menu == 3) {
409  level = 0;
410  }
411  break;
412  case 3: // Radio Menu
413  if(sub_menu == 0) level = 0;
414  break;
415  case 4: // Info Menu
416  if(sub_menu == 6) level = 0;
417  break;
418  case 5: // Demo Menu
419  if(sub_menu == 0) {
420  if(demo_running == 0) {
421  start_line_demo();
422  } else {
423  demo_running = 0;
424  display.set_backlight_brightness(bl_brightness * 0.01f);
425  motors.stop();
426  }
427  }
428  if(sub_menu == 1) {
429  if(demo_running == 0) {
430  start_obstacle_demo();
431  } else {
432  demo_running = 0;
433  display.set_backlight_brightness(bl_brightness * 0.01f);
434  motors.stop();
435 
436  }
437  }
438  if(sub_menu == 2) {
439  if(demo_running == 0) {
440  start_spinning_demo();
441  } else {
442  demo_running = 0;
443  display.set_backlight_brightness(bl_brightness * 0.01f);
444  motors.stop();
445  }
446  }
447  if(sub_menu == 3) {
448  if(demo_running == 0) {
449  start_stress_demo();
450  } else {
451  demo_running = 0;
452  display.set_backlight_brightness(bl_brightness * 0.01f);
453  motors.stop();
454  }
455  }
456  if(sub_menu == 4) level = 0;
457  break;
458  }
459  break;
460  }
461  break;
462  case 4: //Left pressed
463  switch(level) {
464  case 0:
465  if(top_menu == 0) {
466  top_menu = 8;
467  }
468  else {
469  if(use_flash_basic == 0 && top_menu == 7) top_menu = 6;
470  top_menu --;
471  }
472  break;
473  case 1:
474  switch(top_menu) {
475  case 0: //LED Menu
476  if(sub_menu == 0) sub_menu = 13;
477  else sub_menu --;
478  break;
479  case 1: //Sensors Menu
480  if(sub_menu == 0) sub_menu = 11;
481  else sub_menu --;
482  break;
483 
484  case 2: //Motor Menu
485  if(sub_menu == 0) sub_menu = 3;
486  else sub_menu --;
487  break;
488  case 4: //Info Menu
489  if(sub_menu == 0) sub_menu = 6;
490  else sub_menu --;
491  break;
492  case 5: //Demo Menu
493  if(sub_menu == 0) sub_menu = 4;
494  else sub_menu --;
495  break;
496  case 6: //Calibrate Menu
497  if(sub_menu == 0) sub_menu = 2;
498  else sub_menu --;
499  break;
500  case 7: //PsiBasic Menu
501  if(sub_menu == 0) sub_menu = psi_basic_file_count;
502  else sub_menu --;
503  }
504  break;
505 
506  }
507  break;
508  case 8: //Right pressed
509  switch(level) {
510  case 0:
511  top_menu ++;
512  if((top_menu - use_flash_basic) > 7) top_menu = 0;
513  break;
514  case 1:
515  switch(top_menu) {
516  case 0: //LED Menu
517  if(sub_menu == 13) sub_menu = 0;
518  else sub_menu ++;
519  break;
520  case 1: //Sensors Menu
521  if(sub_menu == 11) sub_menu = 0;
522  else sub_menu ++;
523  break;
524  case 2: //Motor Menu
525  if(sub_menu == 3) sub_menu = 0;
526  else sub_menu ++;
527  break;
528  case 4: //Info Menu
529  if(sub_menu == 6) sub_menu = 0;
530  else sub_menu ++;
531  break;
532  case 5: //Demo Menu
533  if(sub_menu == 4) sub_menu = 0;
534  else sub_menu ++;
535  break;
536  case 6: //Calibrate Menu
537  if(sub_menu == 2) sub_menu = 0;
538  else sub_menu ++;
539  break;
540  case 7: //PsiBasic Menu
541  if(sub_menu == psi_basic_file_count) sub_menu = 0;
542  else sub_menu ++;
543  }
544  break;
545  }
546  break;
547  case 16: //Select pressed
548  switch(level) {
549  case 0:
550  if(top_menu != 8) {
551  level++;
552  sub_menu = 0;
553  } else {
554  demo_on = 0;
555  user_code_running = user_code_restore_mode;
556  }
557  break;
558  case 1:
559  switch(top_menu) {
560  case 0: //LED Menu
561  if(sub_menu == 9) {
562  for(int i=0; i<9; i++) {
563  led_state[i]=all_led_state;
564  }
565  demo_update_leds();
566  }
567  if(sub_menu == 13) level = 0;
568  break;
569  case 1: // Sensors Menu
570  if(sub_menu == 11) level = 0;
571  break;
572  case 2: //Motor Menu
573  if(sub_menu == 2) {
574  switch(both_motor_mode) {
575  case 0:
576  motors.stop();
577  break;
578  case 1:
579  motors.brake();
580  break;
581  case 2:
582  motors.forward(0.5);
583  break;
584  case 3:
585  motors.forward(1);
586  break;
587  case 4:
588  motors.backward(0.5);
589  break;
590  case 5:
591  motors.backward(1.0);
592  break;
593  }
594  }
595  if(sub_menu == 3) {
596  level = 0;
597  }
598  break;
599  case 3: // Radio Menu
600  if(sub_menu == 0) level = 0;
601  break;
602  case 4: // Info Menu
603  if(sub_menu == 6) level = 0;
604  break;
605  case 5: // Demo Menu
606  if(sub_menu == 4) level = 0;
607  break;
608  case 6: // Calibrate Menu
609  if(sub_menu == 2) level = 0;
610  break;
611  case 7: // PsiBasic Menu
612  if(sub_menu == psi_basic_file_count) level = 0;
613  break;
614  }
615  break;
616  }
617  break;
618  }
619  } else started = 1;
620  display.clear_display();
621  switch(level) {
622  case 0:
623  //Top level menu
624  switch(top_menu) {
625  case 0:
626  strcpy(topline,"---TEST LEDS----");
627  break;
628  case 1:
629  strcpy(topline,"--TEST SENSORS--");
630  break;
631  case 2:
632  strcpy(topline,"--TEST MOTORS---");
633  break;
634  case 3:
635  strcpy(topline,"---TEST RADIO---");
636  break;
637  case 4:
638  strcpy(topline,"------INFO------");
639  break;
640  case 5:
641  strcpy(topline,"---CODE DEMOS---");
642  break;
643  case 6:
644  strcpy(topline,"-MOTOR CALIBRATE");
645  break;
646  case 7:
647  strcpy(topline,"---PSI BASIC----");
648  break;
649  case 8:
650  strcpy(topline,"------EXIT------");
651  break;
652  }
653  strcpy(bottomline,"");
654  break;
655  case 1:
656  //Sub level menu
657  switch(top_menu) {
658  case 7:
659  strcpy(topline,"-PSI BASIC MENU-");
660  if(sub_menu == psi_basic_file_count) strcpy(bottomline,"EXIT");
661  else strcpy(bottomline,basic_filenames.at(sub_menu).c_str());
662  break;
663  case 0:
664  strcpy(topline,"----LED MENU----");
665  char led_status[7];
666  if(sub_menu<9) {
667  switch(led_state[sub_menu]) {
668  case 0:
669  strcpy(led_status,"OFF");
670  break;
671  case 1:
672  strcpy(led_status,"RED");
673  break;
674  case 2:
675  strcpy(led_status,"GREEN");
676  break;
677  case 3:
678  strcpy(led_status,"YELLOW");
679  break;
680  }
681  }
682  if(sub_menu < 8) sprintf(bottomline,"OUTER %d: %s",(sub_menu + 1),led_status);
683  if(sub_menu == 8) sprintf(bottomline,"CENTER: %s",led_status);
684  if(sub_menu == 9) {
685  switch(all_led_state) {
686  case 0:
687  strcpy(led_status,"OFF");
688  break;
689  case 1:
690  strcpy(led_status,"RED");
691  break;
692  case 2:
693  strcpy(led_status,"GREEN");
694  break;
695  case 3:
696  strcpy(led_status,"YELLOW");
697  break;
698  }
699  sprintf(bottomline,"SET ALL %s", led_status);
700  }
701  if(sub_menu == 10) {
702  if(base_led_state == 0) strcpy(led_status,"OFF");
703  else strcpy(led_status,"ON");
704  sprintf(bottomline,"BASE: %s",led_status);
705  }
706  if(sub_menu == 11) sprintf(bottomline,"CLED BNESS %d%%", brightness);
707  if(sub_menu == 12) sprintf(bottomline,"DISP BNESS %d%%", bl_brightness);
708  if(sub_menu == 13) strcpy(bottomline,"EXIT");
709  break;
710 
711  case 1:
712  strcpy(topline,"--SENSORS MENU--");
713  switch(sub_menu) {
714  case 0: {
715  float battery = sensors.get_battery_voltage ();
716  sprintf(bottomline,"BATTERY: %1.3fV",battery);
717  break;
718  }
719  case 1: {
720  float dc = sensors.get_dc_voltage ();
721  sprintf(bottomline,"DC: %1.3fV",dc);
722  break;
723  }
724  case 2: {
725  float current = sensors.get_current ();
726  sprintf(bottomline,"CURRENT: %1.3fA",current);
727  break;
728  }
729  case 3: {
730  float temperature = sensors.get_temperature();
731  sprintf(bottomline,"TEMP: %3.2fC", temperature);
732  break;
733  }
734  case 4:
735  sensors.store_background_base_ir_values();
736  sprintf(bottomline,"BIR%dB:%d",base_ir_index+1,sensors.get_background_base_ir_value(base_ir_index));
737  break;
738  case 5:
739  sensors.store_illuminated_base_ir_values();
740  sprintf(bottomline,"BIR%dR:%d",base_ir_index+1,sensors.get_illuminated_base_ir_value(base_ir_index));
741  break;
742  case 6:
743  sensors.store_background_raw_ir_values();
744  sprintf(bottomline,"SIR%dB:%d",side_ir_index+1,sensors.get_background_raw_ir_value(side_ir_index));
745  break;
746  case 7:
747  sensors.store_illuminated_raw_ir_values();
748  sprintf(bottomline,"SIR%dR:%d",side_ir_index+1,sensors.get_illuminated_raw_ir_value(side_ir_index));
749  break;
750  case 8:
751  sprintf(bottomline,"SIR%dD:%3.1f",side_ir_index+1,0.0);
752  break;
753  case 9:
754  if(ultrasonic_distance_updated == 1) {
755  sprintf(bottomline,"USONIC:%3dcm",ultrasonic_distance);
756  } else sprintf(bottomline,"USONIC:---------");
757  sensors.update_ultrasonic_measure();
758  break;
759  case 10:
760  sensors.store_line_position();
761  if(line_found == 1)sprintf(bottomline,"LINE:%1.3f",line_position);
762  else sprintf(bottomline,"LINE:---------");
763  break;
764  case 11:
765  sprintf(bottomline,"EXIT");
766  break;
767  }
768  break;
769  case 2:
770  strcpy(topline,"--MOTORS MENU---");
771  switch(sub_menu) {
772  case 0:
773  sprintf(bottomline,"LEFT: %d%%", left_speed);
774  break;
775  case 1:
776  sprintf(bottomline,"RIGHT: %d%%", right_speed);
777  break;
778  case 2:
779  char both_mode_string[16];
780  switch(both_motor_mode) {
781  case 0:
782  strcpy(both_mode_string,"OFF");
783  break;
784  case 1:
785  strcpy(both_mode_string,"BRAKE");
786  break;
787  case 2:
788  strcpy(both_mode_string,"+50%");
789  break;
790  case 3:
791  strcpy(both_mode_string,"+100%");
792  break;
793  case 4:
794  strcpy(both_mode_string,"-50%");
795  break;
796  case 5:
797  strcpy(both_mode_string,"-100%");
798  break;
799  }
800  sprintf(bottomline,"BOTH TO %s",both_mode_string);
801  break;
802  case 3:
803  sprintf(bottomline,"EXIT");
804  break;
805  }
806  break;
807  case 3:
808  strcpy(topline,"---RADIO MENU---");
809  switch(sub_menu) {
810 
811  case 0:
812  sprintf(bottomline,"EXIT");
813  break;
814  }
815  break;
816  case 4:
817  strcpy(topline,"---INFO MENU----");
818  switch(sub_menu) {
819  case 0:
820  sprintf(bottomline,"ROBOT ID: %d",robot_id);
821  break;
822  case 1:
823  sprintf(bottomline,"SOFTWARE: %1.2f",SOFTWARE_VERSION_CODE);
824  break;
825  case 2:
826  if(firmware_version > 0) sprintf(bottomline,"FIRMWARE: %1.2f",firmware_version);
827  else sprintf(bottomline,"FIRMWARE: ?????");
828  break;
829  case 3:
830  sprintf(bottomline,"PROG:%s",program_name);
831  break;
832  case 4:
833  sprintf(bottomline,"AUTH:%s",author_name);
834  break;
835  case 5:
836  sprintf(bottomline,"VER:%s",version_name);
837  break;
838  case 6:
839  sprintf(bottomline,"EXIT");
840  break;
841  }
842  break;
843  case 5:
844  strcpy(topline,"---CODE DEMOS---");
845  switch(sub_menu) {
846  case 0:
847  sprintf(bottomline,"LINE FOLLOW");
848  break;
849  case 1:
850  sprintf(bottomline,"OBST. AVOID");
851  break;
852  case 2:
853  sprintf(bottomline,"COLOUR SPIN");
854  break;
855  case 3:
856  sprintf(bottomline,"STRESS TEST");
857  break;
858  case 4:
859  sprintf(bottomline,"EXIT");
860  break;
861  }
862  break;
863  case 6:
864  strcpy(topline,"-MOTOR CALIBRATE");
865  switch(sub_menu) {
866  case 0:
867  sprintf(bottomline,"RUN A3 TEST");
868  break;
869  case 1:
870  sprintf(bottomline,"ENTER VALUE");
871  break;
872  case 2:
873  sprintf(bottomline,"EXIT");
874  break;
875  }
876  break;
877  }
878  break;
879  }
880  display.write_string(topline);
881  display.set_position(1,0);
882  display.write_string(bottomline);
883  if(top_menu == 1 && level == 1) {
884  demo_event.attach(this,&Demo::demo_event_thread, 0.25);
885  }
886 }
887 
888 void Demo::start_line_demo()
889 {
890  display.set_backlight_brightness(0);
891  time_out = 0.25f;
892  demo_timer.start();
893  state = 0;
894  speed = 0;
895  led_step = 0;
896  demo_running = 1;
897  demo_timeout.attach_us(this,&Demo::line_demo_cycle,1000);
898 }
899 
900 void Demo::start_obstacle_demo()
901 {
902  display.set_backlight_brightness(0);
903  time_out = 0.25f;
904  demo_timer.start();
905  state = 0;
906  speed = 0;
907  led_step = 0;
908  demo_running = 1;
909  demo_timeout.attach_us(this,&Demo::obstacle_demo_cycle,1000);
910 }
911 
912 void Demo::start_stress_demo()
913 {
914  display.set_backlight_brightness(0.25);
915  display.write_string("STRESS TEST");
916  display.set_position(1,0);
917  display.write_string("----25%----");
918  time_out = 0.04f;
919  demo_timer.start();
920  state = 0;
921  speed = 0;
922  stress_step = 0;
923  spin_step = 0;
924  demo_running = 1;
925  demo_timeout.attach_us(this,&Demo::stress_demo_cycle,1000);
926 }
927 
928 void Demo::start_spinning_demo()
929 {
930  display.set_backlight_brightness(0);
931  time_out = 0.0f;
932  demo_timer.start();
933  state = 0;
934  speed = 0;
935  led_step = 0;
936  spin_step = 0;
937  demo_running = 1;
938  demo_timeout.attach_us(this,&Demo::spinning_demo_cycle,1000);
939 }
940 
941 void Demo::line_demo_cycle()
942 {
943  if(demo_timer.read() > time_out) {
944  sensors.store_line_position();
945  if(line_found) {
946  time_out = 0.01f;
947  state = 0;
948  // Get the position of the line.
949  lf_current_pos_of_line = line_position;
950  lf_proportional = lf_current_pos_of_line;
951 
952  // Compute the derivative
953  lf_derivative = lf_current_pos_of_line - lf_previous_pos_of_line;
954 
955  // Compute the integral
956  lf_integral += lf_proportional;
957 
958  // Remember the last position.
959  lf_previous_pos_of_line = lf_current_pos_of_line;
960 
961  // Compute the power
962  lf_power = (lf_proportional * (LF_P_TERM) ) + (lf_integral*(LF_I_TERM)) + (lf_derivative*(LF_D_TERM)) ;
963 
964  // Compute new speeds
965  lf_right = lf_speed-lf_power;
966  lf_left = lf_speed+lf_power;
967 
968  // limit checks
969  if (lf_right < 0)
970  lf_right = 0;
971  else if (lf_right > 1.0f)
972  lf_right = 1.0f;
973 
974  if (lf_left < 0)
975  lf_left = 0;
976  else if (lf_left > 1.0f)
977  lf_left = 1.0f;
978  }else{
979  //Cannot see line: hunt for it
980  if(lf_left > lf_right){
981  //Currently turning left, keep turning left
982  state ++;
983  float d_step = state * 0.04;
984  lf_left = 0.2 + d_step;
985  lf_right = -0.2 - d_step;
986  if(state > 20){
987  state = 0;
988  lf_right = 0.2;
989  lf_left = -0.2;
990  time_out += 0.01f;
991  if(time_out > 0.1f) demo_running = 0;
992  }
993  }else{
994  //Currently turning right, keep turning right
995  state ++;
996  float d_step = state * 0.04;
997  lf_left = -0.2 - d_step;
998  lf_right = 0.2 + d_step;
999  if(state > 20){
1000  state = 0;
1001  lf_right = -0.2;
1002  lf_left = 0.2;
1003  time_out += 0.01f;
1004  if(time_out > 0.1f) demo_running = 0;
1005  }
1006  }
1007  }
1008  // set speed
1009  motors.set_left_motor_speed(lf_left);
1010  motors.set_right_motor_speed(lf_right);
1011 
1012 
1013  demo_timer.reset();
1014  }
1015  if(demo_running == 1)demo_timeout.attach_us(this,&Demo::line_demo_cycle,500);
1016  else {
1017  motors.stop();
1018  display.set_backlight_brightness(bl_brightness * 0.01f);
1019  }
1020 }
1021 
1022 void Demo::stress_demo_cycle()
1023 {
1024  if(demo_timer.read() > time_out) {
1025  float pct = 0.25 + (0.25 * stress_step);
1026  switch(state) {
1027  case 0:
1028  if(spin_step % 2 == 0) {
1029  motors.forward(pct);
1030  led.set_leds(0xFF,0xFF);
1031  } else {
1032  motors.backward(pct);
1033  led.set_leds(0,0xFF);
1034  }
1035  spin_step ++;
1036  if(spin_step > 199) {
1037  state ++;
1038  spin_step = 0;
1039  }
1040  break;
1041  case 1:
1042  if(stress_step < 3) stress_step ++;
1043  float pct = 0.25 + (0.25 * stress_step);
1044  display.set_backlight_brightness(pct);
1045  display.set_position(1,0);
1046  switch(stress_step) {
1047  case 1:
1048  display.write_string("----50%----");
1049  break;
1050  case 2:
1051  display.write_string("----75%----");
1052  break;
1053  case 3:
1054  display.write_string("---100%----");
1055  break;
1056  }
1057  state = 0;
1058  break;
1059  }
1060  demo_timer.reset();
1061  }
1062  if(demo_running == 1)demo_timeout.attach_us(this,&Demo::stress_demo_cycle,500);
1063  else {
1064  motors.stop();
1065  display.set_backlight_brightness(bl_brightness * 0.01f);
1066  }
1067 }
1068 
1069 void Demo::spinning_demo_cycle()
1070 {
1071  if(demo_timer.read() > time_out) {
1072  switch(state) {
1073  case 0: //Robot is stopped
1074  led.set_leds(0,0xFF);
1075  led.set_center_led(1,1);
1076  speed = 0.1f;
1077  motors.brake();
1078  time_out = 0.5;
1079  state = 1;
1080  led_step = 0;
1081  break;
1082  case 1: //Motor is turning right, accelerating
1083  time_out = 0.1;
1084  led.set_center_led(2,1);
1085  switch(led_step) {
1086  case 0:
1087  led.set_leds(0x01,0);
1088  break;
1089  case 1:
1090  led.set_leds(0x02,0);
1091  break;
1092  case 2:
1093  led.set_leds(0x04,0);
1094  break;
1095  case 3:
1096  led.set_leds(0x08,0);
1097  break;
1098  case 4:
1099  led.set_leds(0x10,0);
1100  break;
1101  case 5:
1102  led.set_leds(0x20,0);
1103  break;
1104  case 6:
1105  led.set_leds(0x40,0);
1106  break;
1107  case 7:
1108  led.set_leds(0x80,0);
1109  break;
1110  }
1111  led_step ++;
1112  if(led_step == 8) led_step =0;
1113  if(speed < 1) {
1114  speed += 0.0125;
1115  motors.turn(speed);
1116  } else {
1117  state = 2;
1118  spin_step = 0;
1119  led_step =0;
1120  }
1121  break;
1122  case 2: //Motor is turning right, full speed
1123  led.set_center_led(3,1);
1124  switch(led_step) {
1125  case 0:
1126  led.set_leds(0x33,0x33);
1127  break;
1128  case 1:
1129  led.set_leds(0x66,0x66);
1130  break;
1131  case 2:
1132  led.set_leds(0xCC,0xCC);
1133  break;
1134  case 3:
1135  led.set_leds(0x99,0x99);
1136  break;
1137  }
1138  led_step ++;
1139  if(led_step == 4) led_step = 0;
1140  spin_step ++;
1141  if(spin_step == 40) {
1142  state = 3;
1143  led_step = 0;
1144  }
1145  break;
1146  case 3: //Motor is turning right, decelerating
1147  led.set_center_led(2,1);
1148  switch(led_step) {
1149  case 0:
1150  led.set_leds(0x01,0);
1151  break;
1152  case 1:
1153  led.set_leds(0x02,0);
1154  break;
1155  case 2:
1156  led.set_leds(0x04,0);
1157  break;
1158  case 3:
1159  led.set_leds(0x08,0);
1160  break;
1161  case 4:
1162  led.set_leds(0x10,0);
1163  break;
1164  case 5:
1165  led.set_leds(0x20,0);
1166  break;
1167  case 6:
1168  led.set_leds(0x40,0);
1169  break;
1170  case 7:
1171  led.set_leds(0x80,0);
1172  break;
1173  }
1174  if(led_step == 0) led_step =8;
1175  led_step --;
1176  if(speed > 0.1) {
1177  speed -= 0.025;
1178  motors.turn(speed);
1179  } else {
1180  state = 4;
1181  spin_step = 0;
1182  led_step =0;
1183  }
1184  break;
1185  case 4: //Robot is stopped
1186  led.set_leds(0,0xFF);
1187  led.set_center_led(1,1);
1188  speed = 0.1f;
1189  motors.brake();
1190  time_out = 0.5;
1191  led_step =0;
1192  state = 5;
1193  break;
1194  case 5: //Motor is turning left, accelerating
1195  time_out = 0.1;
1196  led.set_center_led(2,1);
1197  switch(led_step) {
1198  case 0:
1199  led.set_leds(0x01,0);
1200  break;
1201  case 1:
1202  led.set_leds(0x02,0);
1203  break;
1204  case 2:
1205  led.set_leds(0x04,0);
1206  break;
1207  case 3:
1208  led.set_leds(0x08,0);
1209  break;
1210  case 4:
1211  led.set_leds(0x10,0);
1212  break;
1213  case 5:
1214  led.set_leds(0x20,0);
1215  break;
1216  case 6:
1217  led.set_leds(0x40,0);
1218  break;
1219  case 7:
1220  led.set_leds(0x80,0);
1221  break;
1222  }
1223  if(led_step == 0) led_step =8;
1224  led_step --;
1225  if(speed < 1) {
1226  speed += 0.0125;
1227  motors.turn(-speed);
1228  } else {
1229  state = 6;
1230  spin_step = 0;
1231  led_step = 0;
1232  }
1233  break;
1234  case 6: //Motor is turning left, full speed
1235  led.set_center_led(3,1);
1236  switch(led_step) {
1237  case 0:
1238  led.set_leds(0x33,0x33);
1239  break;
1240  case 1:
1241  led.set_leds(0x66,0x66);
1242  break;
1243  case 2:
1244  led.set_leds(0xCC,0xCC);
1245  break;
1246  case 3:
1247  led.set_leds(0x99,0x99);
1248  break;
1249  }
1250  if(led_step == 0) led_step = 4;
1251  led_step --;
1252  spin_step ++;
1253  if(spin_step == 40) {
1254  state = 7;
1255  led_step = 0;
1256  }
1257  break;
1258  case 7: //Motor is turning left, decelerating
1259  led.set_center_led(2,1);
1260  switch(led_step) {
1261  case 0:
1262  led.set_leds(0x01,0);
1263  break;
1264  case 1:
1265  led.set_leds(0x02,0);
1266  break;
1267  case 2:
1268  led.set_leds(0x04,0);
1269  break;
1270  case 3:
1271  led.set_leds(0x08,0);
1272  break;
1273  case 4:
1274  led.set_leds(0x10,0);
1275  break;
1276  case 5:
1277  led.set_leds(0x20,0);
1278  break;
1279  case 6:
1280  led.set_leds(0x40,0);
1281  break;
1282  case 7:
1283  led.set_leds(0x80,0);
1284  break;
1285  }
1286  led_step ++;
1287  if(led_step == 8) led_step =0;
1288  if(speed > 0.1) {
1289  speed -= 0.025;
1290  motors.turn(-speed);
1291  } else {
1292  state = 0;
1293  spin_step = 0;
1294  led_step = 0;
1295  }
1296  break;
1297  }
1298  demo_timer.reset();
1299  }
1300  if(demo_running == 1)demo_timeout.attach_us(this,&Demo::spinning_demo_cycle,500);
1301  else {
1302  motors.stop();
1303  display.set_backlight_brightness(bl_brightness * 0.01f);
1304  }
1305 }
1306 
1307 void Demo::obstacle_demo_cycle()
1308 {
1309  if(demo_timer.read() > time_out) {
1310  switch(state) {
1311  case 0: //Robot is stopped
1312  led.set_leds(0,0xFF);
1313  led.set_center_led(1,0.4);
1314  speed = 0.3f;
1315  motors.forward(speed);
1316  time_out = 0.05;
1317  state = 1;
1318  break;
1319  case 1: { //Motor is moving forward
1320  sensors.store_ir_values();
1321  int front_right = sensors.read_illuminated_raw_ir_value(0);
1322  int front_left = sensors.read_illuminated_raw_ir_value(7);
1323  if(front_left > 400 || front_right > 400) {
1324  motors.brake();
1325  time_out = 0.04;
1326  if(front_left > front_right)state=2;
1327  else state=3;
1328  } else {
1329  if(speed < 0.5) {
1330  speed += 0.03;
1331  motors.forward(speed);
1332  }
1333  switch(led_step) {
1334  case 0:
1335  led.set_leds(0x01,0);
1336  break;
1337  case 1:
1338  led.set_leds(0x38,0);
1339  break;
1340  case 2:
1341  led.set_leds(0x6C,0);
1342  break;
1343  case 3:
1344  led.set_leds(0xC6,0);
1345  break;
1346  case 4:
1347  led.set_leds(0x83,0);
1348  break;
1349  }
1350  led.set_center_led(2, 0.6);
1351  led_step ++;
1352  if(led_step == 5) led_step = 0;
1353  }
1354  break;
1355  }
1356  case 2: //Turn right
1357  motors.set_left_motor_speed(0.85);
1358  motors.set_right_motor_speed(-0.85);
1359  time_out = 0.4;
1360  state = 0;
1361  led.set_leds(0x0E,0x0E);
1362  led.set_center_led(3,0.5);
1363  break;
1364  case 3: //Turn left
1365  motors.set_left_motor_speed(-0.85);
1366  motors.set_right_motor_speed(0.85);
1367  time_out = 0.4;
1368  state = 0;
1369  led.set_leds(0xE0,0xE0);
1370  led.set_center_led(3,0.5);
1371  break;
1372  }
1373  demo_timer.reset();
1374  }
1375  if(demo_running == 1)demo_timeout.attach_us(this,&Demo::obstacle_demo_cycle,200);
1376  else {
1377  motors.stop();
1378  display.set_backlight_brightness(bl_brightness * 0.01f);
1379  }
1380 }
1381 
1382 void Demo::demo_update_leds()
1383 {
1384  char red = 0;
1385  char green = 0;
1386  for(int i=0; i<8; i++) {
1387  if(led_state[i]==1 || led_state[i]==3) red+=(1<<i);
1388  if(led_state[i]==2 || led_state[i]==3) green+=(1<<i);
1389  }
1390  led.set_leds(green,red);
1391  float brightness_f = brightness / 100.0f;
1392  led.set_center_led(led_state[8], brightness_f);
1393  led.set_base_led(base_led_state);
1394 }
Display::write_string
void write_string(char *message)
Definition: display.cpp:131
Sensors::store_illuminated_raw_ir_values
void store_illuminated_raw_ir_values(void)
Definition: sensors.cpp:216
Sensors::read_illuminated_raw_ir_value
unsigned short read_illuminated_raw_ir_value(char index)
Definition: sensors.cpp:256
Sensors::store_background_raw_ir_values
void store_background_raw_ir_values(void)
Definition: sensors.cpp:207
Led::set_leds
void set_leds(char green, char red)
Definition: led.cpp:38
Display::set_position
void set_position(char row, char column)
Definition: display.cpp:174
Led::set_center_led
void set_center_led(char state)
Definition: led.cpp:78
Display::clear_display
void clear_display(void)
Definition: display.cpp:230
Sensors::store_ir_values
void store_ir_values(void)
Definition: sensors.cpp:200
Led::set_base_led
void set_base_led(char state)
Definition: led.cpp:45
Sensors::update_ultrasonic_measure
void update_ultrasonic_measure(void)
Definition: sensors.cpp:42
Motors::brake
void brake(void)
Definition: motors.cpp:56
Motors::turn
void turn(float speed)
Definition: motors.cpp:92
Sensors::get_background_base_ir_value
unsigned short get_background_base_ir_value(char index)
Definition: sensors.cpp:299
Sensors::get_dc_voltage
float get_dc_voltage(void)
Definition: sensors.cpp:101
Sensors::get_current
float get_current(void)
Definition: sensors.cpp:93
Sensors::store_background_base_ir_values
void store_background_base_ir_values(void)
Definition: sensors.cpp:325
Display::set_backlight_brightness
void set_backlight_brightness(float brightness)
Definition: display.cpp:198
Sensors::get_battery_voltage
float get_battery_voltage(void)
Definition: sensors.cpp:87
Sensors::get_illuminated_base_ir_value
unsigned short get_illuminated_base_ir_value(char index)
Definition: sensors.cpp:307
Motors::set_left_motor_speed
void set_left_motor_speed(float speed)
Definition: motors.cpp:28
Motors::backward
void backward(float speed)
Definition: motors.cpp:83
Sensors::get_temperature
float get_temperature(void)
Definition: sensors.cpp:77
Motors::stop
void stop(void)
Definition: motors.cpp:65
Sensors::get_background_raw_ir_value
unsigned short get_background_raw_ir_value(char index)
Definition: sensors.cpp:175
Sensors::store_illuminated_base_ir_values
void store_illuminated_base_ir_values(void)
Definition: sensors.cpp:334
Psiswarm::debug
void debug(const char *format,...)
Definition: psiswarm.cpp:325
Motors::set_right_motor_speed
void set_right_motor_speed(float speed)
Definition: motors.cpp:35
Motors::forward
void forward(float speed)
Definition: motors.cpp:74
Demo::start_demo_mode
void start_demo_mode(void)
Definition: demo.cpp:80
Sensors::get_illuminated_raw_ir_value
unsigned short get_illuminated_raw_ir_value(char index)
Definition: sensors.cpp:183