dances.cpp

00001 /* University of York Robotics Laboratory PsiSwarm Library: Dances Source File
00002  *
00003  * Library of simple predetermined movements
00004  *
00005  * File: dances.cpp
00006  *
00007  * (C) Dept. Electronics & Computer Science, University of York
00008  * James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
00009  *
00010  * PsiSwarm Library Version: 0.3
00011  *
00012  * October 2015
00013  *
00014  */
00015 
00016 
00017 #include "psiswarm.h"
00018 
00019 char vibrate_counter = 0;
00020 Timeout dances_timeout;
00021 
00022 ///Do a simple wiggle
00023 void vibrate(void)
00024 {
00025     if(vibrate_counter == 0)save_led_states();
00026     if(vibrate_counter % 2 == 0) {
00027         set_leds(0xC7,0x00);
00028         turn(1.0);
00029     } else {
00030         set_leds(0x00,0xC7);
00031         turn(-1.0);
00032     }
00033     vibrate_counter++;
00034 
00035     if(vibrate_counter < 14) {
00036         float wiggle_timeout_period = 0.06;
00037         //Move less on first 'wiggle' so that we stay in roughly the same place!
00038         if(vibrate_counter == 0) wiggle_timeout_period = 0.03;
00039         dances_timeout.attach(vibrate, wiggle_timeout_period);
00040     } else {
00041         vibrate_counter = 0;
00042         brake();
00043         restore_led_states();
00044     }
00045 }