C++ Library for the PsiSwarm Robot - Version 0.7
Fork of PsiSwarmV7 by
Diff: dances.cpp
- Revision:
- 0:d6269d17c8cf
- Child:
- 2:c6986ee3c7c5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dances.cpp Thu Feb 04 21:48:54 2016 +0000 @@ -0,0 +1,46 @@ +/* University of York Robotics Laboratory PsiSwarm Library: Dances Source File + * + * Library of simple predetermined movements + * + * File: dances.cpp + * + * (C) Dept. Electronics & Computer Science, University of York + * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis + * + * PsiSwarm Library Version: 0.4 + * + * February 2016 + * + * + */ + + +#include "psiswarm.h" + +char vibrate_counter = 0; +Timeout dances_timeout; + +///Do a simple wiggle +void vibrate(void) +{ + if(vibrate_counter == 0)save_led_states(); + if(vibrate_counter % 2 == 0) { + set_leds(0xC7,0x00); + turn(1.0); + } else { + set_leds(0x00,0xC7); + turn(-1.0); + } + vibrate_counter++; + + if(vibrate_counter < 14) { + float wiggle_timeout_period = 0.06; + //Move less on first 'wiggle' so that we stay in roughly the same place! + if(vibrate_counter == 0) wiggle_timeout_period = 0.03; + dances_timeout.attach(vibrate, wiggle_timeout_period); + } else { + vibrate_counter = 0; + brake(); + restore_led_states(); + } +} \ No newline at end of file